指數映射

 旋轉變換的指數形式

 　　用單位向量$\hat{\omega}$表明旋轉軸，以及$\theta$表明繞該軸的旋轉角度。則能夠用三維向量$\hat{\omega}\theta\in\mathbb{R}^3$以指數形式來描述旋轉。若是將$\hat{\omega}$和$\theta$分開描述，即爲Axis-Angle形式。用$\hat{\omega}\theta$來描述旋轉矩陣R能夠有下面幾種理解方法：

1. 某座標系初始與參考座標系{s}重合，繞軸$\hat{\omega}$旋轉$\theta$角度後當到達當前姿態，其相對於{s}的旋轉矩陣爲R。這種描述方法爲 Axis–angle representation
   
2. 角速度矢量$\hat{\omega}\theta$ 在參考座標系{s}中描述，某座標系初始與參考座標系{s}重合，以角速度$\hat{\omega}\theta$轉動，通過單位時間到達當前姿態（可由矩陣R表示）。
3. 角速度矢量$\hat{\omega}$在參考座標系{s}中描述，某座標系初始與參考座標系{s}重合，以角速度$\hat{\omega}$轉動，通過$\theta$時間到達當前姿態（可由矩陣R表示）。

•  Essential Results from Linear Differential Equations Theory
  

　　考慮下面的一階線性微分方程：$$\dot{x}(t)=Ax(t)$$

　　其中，$x(t)\in\mathbb{R}^n$，$A\in\mathbb{R}^{n\times n}$是一個常量矩陣。若給出初始條件$x(0)=x_0$，則可獲得微分方程的解爲：$$x(t)=x_0e^{At}$$

　　矩陣指數$e^{At}$能夠根據泰勒展開式來計算：$$e^{At}=I+At+\frac{(At)^2}{2!}+\frac{(At)^3}{3!}+...$$

　　若是矩陣$A$能夠表達成$A=PDP^{-1}$，$P$爲可逆矩陣，則有：

$$\begin{align*} e^{At}&=I+At+\frac{(At)^2}{2!}+\frac{(At)^3}{3!}+...\\
&=I+(PDP^{-1})t+(PDP^{-1})(PDP^{-1})\frac{t^2}{2!}+...\\
&=P(I+Dt+\frac{(Dt)^2}{2!}+...)P^{-1}\\
&=Pe^{Dt}P^{-1}
\end{align*}$$

 　　更進一步，若是$A$能夠對角化，即$D$是對角矩陣：$D=diag\{d_1,d_2,...,d_n\}$，則$e^{Dt}$能夠很方便的計算：

$$e^{Dt}=\begin{bmatrix}
e^{d_1t} & 0 & \cdots & 0 \\
0 & e^{d_2t} & \cdots & 0 \\
\vdots & \vdots & \ddots &\vdots \\
0 & 0 & \cdots & e^{d_nt}
\end{bmatrix}$$

•  Exponential Coordinates of Rotations

 　　參考下圖中的描述，假設三維向量$p(0)$繞着轉軸$\hat{\omega}$旋轉$\theta$度後到達$p(\theta)$。用$p(t)$表明$t$時刻向量$p$的位置，則該過程也能夠描述爲：$p(0)$以角速度$\hat{\omega}$旋轉（$\hat{\omega}$爲單位角速度），從$t=0$運動到$t=\theta$。

 　　旋轉時的速度可用$\dot{p}$來描述，且有：$$\dot{p}=\hat{\omega}\times p$$

　　用斜對稱矩陣$[\hat{\omega}]$能夠將向量叉乘變爲矩陣與向量乘法，所以上面的微分方程可寫爲：$$\dot{p}=[\hat{\omega}]p$$

 　　若$\hat{\omega}=[\omega_x,\omega_y,\omega_z]^T$，定義斜對稱矩陣$[\hat{\omega}]$：

$$[\hat{\omega}]=\begin{bmatrix}
0 & -\omega_z & \omega_y \\
\omega_z & 0 & -\omega_x\\
-\omega_y & \omega_x & 0
\end{bmatrix}$$

　　已知初始條件$p(0)$，該方程形式如前面研究過的$\dot{x}=Ax$，所以其解爲：$p(t)=e^{[\hat{\omega}]t}p(0)$

　　因爲變量$t$和$\theta$可互換，則上面方程可寫爲：$$p(\theta)=e^{[\hat{\omega}]\theta}p(0)$$

　　下面將$e^{[\hat{\omega}]t}$進行泰勒展開，計算斜對稱矩陣$[\hat{\omega}]$的冪可得關係式：$[\hat{\omega}]^3=-[\hat{\omega}]$，利用這一關係咱們能夠將$[\hat{\omega}]^3$替換爲$-[\hat{\omega}]$，$[\hat{\omega}]^4$替換爲$-[\hat{\omega}]^2$，$[\hat{\omega}]^5$替換爲$-[\hat{\omega}]^3$...因而能夠獲得：

$$\begin{align*} e^{[\hat{\omega}]\theta}&=I+[\hat{\omega}]\theta+[\hat{\omega}]^2\frac{\theta^2}{2!}+[\hat{\omega}]^3\frac{\theta^3}{3!}+...\\ &=I+(\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-...)[\hat{\omega}]+(\frac{\theta^2}{2!}-\frac{\theta^4}{4!}-\frac{\theta^6}{6!}-...)[\hat{\omega}]^2\end{align*}$$

　　根據正弦和餘弦函數的泰勒展開式：

$$\begin{align*} \sin\theta&= \theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-...\\
\cos\theta&=1-\frac{\theta^2}{2!}+\frac{\theta^4}{4!}-...\end{align*}$$

　　能夠將上面公式進行簡化。給定$\hat{\omega}\theta\in\mathbb{R}^3$，其中$\theta$爲任意標量，$\hat{\omega}\in\mathbb{R}^3$且爲單位向量。則根據$[\hat{\omega}]$、$\theta$進行旋轉的旋轉矩陣爲：$$\boxed{Rot(\hat{\omega},\theta)=e^{[\hat{\omega}]\theta}=I+\sin\theta[\hat{\omega}]+(1-\cos\theta)[\hat{\omega}]^2}$$

　　該公式也稱爲Rodrigues’ formula（羅德里格斯公式） 。

 　　舉個例子，下圖中座標系{b}相對於固定參考座標系{s}的姿態能夠描述爲：初始時刻兩座標系一致，{b}繞單位向量$\hat{\omega_1}=(0,0.866,0.5)$旋轉$\theta_1=30^°=0.524rad$後到達當前姿態。

　　則{b}相對於{s}的旋轉矩陣能夠計算爲：

$$\begin{align*}
R&=e^{[\hat{\omega_1}]\theta_1}=I+\sin\theta_1[\hat{\omega_1}]+(1-\cos\theta_1)[\hat{\omega_1}]^2 \\
&=I+0.5\begin{bmatrix}0&-0.5&0.866\\0.5&0&0\\-0.866&0&0\end{bmatrix}+0.134\begin{bmatrix}0&-0.5&0.866\\0.5&0&0\\-0.866&0&0\end{bmatrix}^2\\
&=\begin{bmatrix}0.866&-0.250&0.433\\0.250&0.967&0.058\\-0.433&0.058&0.899\end{bmatrix}
\end{align*}$$

　　座標系{b}的姿態可由矩陣$R$描述，或者由單位向量$\hat{\omega_1}=(0,0.866,0.5)$以及轉角$\theta_1=0.524rad$來描述，即旋轉矩陣R的指數座標爲$\hat{\omega_1}\theta_1=(0,0.453,0.262)$

　　在Mathematica中RotationMatrix函數能夠根據轉軸和轉角計算旋轉矩陣：

•  Matrix Logarithm of Rotations

　　若是向量$\hat{\omega}\theta\in\mathbb{R}^3$表達了旋轉矩陣R的指數座標，則斜對稱矩陣$[\hat{\omega}\theta]=[\hat{\omega}]\theta$是旋轉矩陣R的對數。矩陣的對數是矩陣指數的逆：

　　當轉角$\theta$不爲$\pi$的整數倍時，能夠根據旋轉矩陣R計算出轉軸：$$\begin{align*} \hat{\omega_x}=\frac{1}{2\sin\theta}(r_{32}-r_{23})\\\hat{\omega_y}=\frac{1}{2\sin\theta}(r_{13}-r_{31})\\\hat{\omega_z}=\frac{1}{2\sin\theta}(r_{21}-r_{12})\end{align*}$$

　　或表述爲斜對稱矩陣形式：$$[\hat{\omega}]=\begin{bmatrix}0&-\hat{\omega_z}&\hat{\omega_y}\\ \hat{\omega_z}&0&-\hat{\omega_x}\\-\hat{\omega_y}&\hat{\omega_x}&0 \end{bmatrix}=\frac{1}{2\sin\theta}(R-R^T)$$

　　具體推導和細節可參考：Modern Robotics: Mechanics, Planning, and Control   3.2.3.3 Matrix Logarithm of Rotations 

 

 

 

 

參考：

物理引擎中的剛體轉動2

四元數指數映射旋轉參數化的實際應用

視覺SLAM中的數學基礎 第二篇 四元數

視覺SLAM中的數學基礎 第三篇 李羣與李代數

李羣與李代數 - part 2 指數與對數映射、李代數求導與擾動模型

Axis–angle representation

Rodrigues' rotation formula

Lie Groups for 2D and 3D Transformations

Modern Robotics: Mechanics, Planning, and Control  3.2.3 Exponential Coordinate Representation of Rotation

Lie groups, Lie algebras, projective geometry and optimization for 3D Geometry, Engineering and Computer Vision