姿态表示方法

摘要：讨论不同的刚体姿态表示方法。包括：欧拉角、四元数、李群李代数。

定义惯性坐标系为$O_i$，刚体坐标系为$O_b$

1 旋转矩阵

 惯性坐标系下的刚体姿态可以使用旋转矩阵表示：

$$R=[\boldsymbol{r}_1,\boldsymbol{r}_2,\boldsymbol{r}_3]^{\mathrm{T}} \tag{1}$$

其中：$\boldsymbol{r}_1,\boldsymbol{r}_2,\boldsymbol{r}_3$分别表示刚体坐标系x,y,z方向上的单位向量在惯性坐标系中的表示。

 旋转矩阵为单位矩阵。满足性质：

$$\mathrm{det}(R)=I,~R^{\mathrm{T}}R=I \tag{2}$$

1.1 绕x,y,z轴的旋转

$$Rot_x(\theta)=\begin{bmatrix} 1& 0 &0 \\ 0& \mathrm{cos}\theta &-\mathrm{sin}\theta \\ 0& \mathrm{sin}\theta&\mathrm{cos}\theta \end{bmatrix}\\ ~\\ Rot_y(\theta)=\begin{bmatrix} \mathrm{cos}\theta& 0 &\mathrm{sin}\theta \\ 0& 1&0\\ -\mathrm{sin}\theta& 0&\mathrm{cos}\theta \end{bmatrix}\\ ~\\ Rot_z(\theta)=\begin{bmatrix} \mathrm{cos}\theta& -\mathrm{sin}\theta&0 \\ \mathrm{sin}\theta& \mathrm{cos}\theta &0 \\ 0& 0&1 \end{bmatrix} \tag{3}$$

其中旋转方向可使用右手定则确定。

1.2 旋转矩阵的导数

 记刚体旋转的角速度在惯性坐标系和刚体坐标系下的表示分别为$\boldsymbol{\omega}_i$和$\boldsymbol{\omega}_b$。则旋转矩阵的导数可以表示为：

$$\begin{align} &\dot{R}=\mathrm{sk}(\boldsymbol{\omega}_i)R \tag{4} \\ &\dot{R}=R\mathrm{sk}(\boldsymbol{\omega}_b) \tag{5} \end{align}$$

其中$\mathrm{sk}(\boldsymbol{\omega})=\begin{bmatrix}0 & -\omega_z &\omega_y \\ \omega_z&0 &-\omega_x \\
-\omega_y& \omega_x& 0\end{bmatrix}$为向量$\boldsymbol{\omega}=[\omega_x,\omega_y,\omega_z]^{\mathrm{T}}$对应的反对称矩阵。公式(4)和(5)可以利用如下性质互相转化：

$$R\mathrm{sk}(\boldsymbol{\omega})R^{\mathrm{T}}=\mathrm{sk}(R\boldsymbol{\omega}) \tag{6}$$

2 欧拉角

 记依次绕自身坐标系z-y-x轴旋转的欧拉角为$\boldsymbol{\Phi}=[\phi,\theta,\psi]^\mathrm{T}$，欧拉角对应的旋转矩阵可以表示为：

$$R=Rot_{z}(\psi)Rot_{y}(\theta)Rot_{x}(\phi)=\begin{bmatrix} c\psi c\theta& c\psi s\theta s\phi-s\psi c\phi&c\psi s\theta c\phi+s\psi s\phi \\ s\psi c\theta&s\phi s\theta s\phi+c\psi c\phi &s\psi s\theta c\phi-c\psi s\phi \\ -s\theta& c\theta s\phi&c\theta c\phi \end{bmatrix} \tag{7}$$

2.1 由旋转矩阵计算欧拉角

 记旋转矩阵为$R=\begin{bmatrix}
r_{11}&r_{12}&r_{13} \\
r_{21}&r_{22}&r_{23} \\
r_{31}&r_{32}&r_{33}
\end{bmatrix}$，旋转矩阵对应的欧拉角可由以下方式计算：

• 如果$r_{31}\neq\pm1$
  • $\theta_1=-\mathrm{asin}(r_{31})$
  • $\theta_2=\pi-\theta_1$
  • $\phi_1=\mathrm{atan2}(\frac{r_{32}}{c\theta_1},\frac{r_{33}}{c\theta_1})$
  • $\phi_2=\mathrm{atan2}(\frac{r_{32}}{c\theta_2},\frac{r_{33}}{c\theta_2})$
  • $\psi_1=\mathrm{atan2}(\frac{r_{21}}{c\theta_1},\frac{r_{11}}{c\theta_1})$
  • $\psi_2=\mathrm{atan2}(\frac{r_{21}}{c\theta_2},\frac{r_{11}}{c\theta_2})$
• 如果$r_{31}=\pm1$
  • 万向节死锁，$\psi$可为任意值
  • 如果$r_{31}=1$
    • $\theta=\pi/2$
    • $\phi=\psi+\mathrm{atan2}(r_{12},r_{13})$
  • 如果$r_{31}=-1$
    • $\theta=-\pi/2$
    • $\phi=-\psi+\mathrm{atan2}(-r_{12},-r_{13})$

2.2 欧拉角的导数

 记$\boldsymbol{\Phi}=[\phi,\theta,\psi]^{\mathrm{T}}$为绕机体坐标轴按z-y-x顺序旋转的roll，pitch和yaw角，$\boldsymbol{\Phi}$的导数可表示为：

$$\dot{\boldsymbol{\Phi}}=\begin{bmatrix} 1 & \mathrm{sin}\phi \mathrm{tan}\theta&\mathrm{cos}\phi\mathrm{tan}\theta\\ 0 & \mathrm{cos}\phi & -\mathrm{sin}\phi\\ 0 & \mathrm{sin}\phi \mathrm{sec}\theta & \mathrm{cos}\phi \mathrm{sec}\theta \end{bmatrix}\boldsymbol{\omega}_b \tag{8}$$

同时：

$$\boldsymbol{\omega}_b=\begin{bmatrix} 1 & 0&-\mathrm{sin}\theta\\ 0 & \mathrm{cos}\phi & \mathrm{sin}\phi\mathrm{cos}\theta\\ 0 & -\mathrm{sin}\phi & \mathrm{cos}\phi \mathrm{cos}\theta \end{bmatrix}\dot{\boldsymbol{\Phi}} \tag{9}$$

可以看出上述结论仅在$\theta\neq\pm\pi/2$的条件下成立。

 推导

$$\begin{aligned} &\dot{R}=\dot{R}_{\psi}R_{\theta}R_{\phi}+R_{\psi}\dot{R}_{\theta}R_{\phi}+R_{\psi}R_{\theta}\dot{R}_{\phi}=R_{\psi}R_{\theta}R_{\phi}\mathrm{sk}(\boldsymbol{\omega_b})\\ &R_{\phi}^{\mathrm{T}}R_{\theta}^{\mathrm{T}}R_{\psi}^{\mathrm{T}}\dot{R}_{\psi}R_{\theta}R_{\phi}+R_{\phi}^{\mathrm{T}}R_{\theta}^{\mathrm{T}}\dot{R}_{\theta}R_{\phi}+R_{\phi}^{\mathrm{T}}\dot{R}_{\phi}=\mathrm{sk}(\boldsymbol{\omega_b})\\ &R_{\phi}^{\mathrm{T}}R_{\theta}^{\mathrm{T}}\mathrm{sk}([0,0,\dot{\psi}]^{\mathrm{T}})R_{\theta}R_{\phi}+R_{\phi}^{\mathrm{T}}\mathrm{sk}([0,\dot{\theta},0]^{\mathrm{T}})R_{\theta}+\mathrm{sk}([\dot{\phi},0,0]^{\mathrm{T}})=\mathrm{sk}(\boldsymbol{\omega_b})\\ &\mathrm{sk}(R_{\phi}^{\mathrm{T}}R_{\theta}^{\mathrm{T}}[0,0,\dot{\psi}]^{\mathrm{T}})+\mathrm{sk}(R_{\phi}^{\mathrm{T}}[0,\dot{\theta},0]^{\mathrm{T}})+\mathrm{sk}([\dot{\phi},0,0]^{\mathrm{T}})=\mathrm{sk}(\boldsymbol{\omega_b})\\ &\boldsymbol{\omega_b}=R_{\phi}^{\mathrm{T}}R_{\theta}^{\mathrm{T}}[0,0,\dot{\psi}]^{\mathrm{T}}+R_{\phi}^{\mathrm{T}}[0,\dot{\theta},0]^{\mathrm{T}}+[\dot{\phi},0,0]^{\mathrm{T}} \end{aligned}$$

3 四元数

 单位四元数可用于旋转的表示：

$$\boldsymbol{q}=[s,\boldsymbol{v}]^{\mathrm{T}} =[cos\frac{\theta}{2},\boldsymbol{n}sin\frac{\theta}{2}]^{\mathrm{T}} \tag{10}$$

其中$\theta\in\mathbb{R}$和$\boldsymbol{n}\in\mathbb{R}^3$分别为旋转角和单位旋转轴，$||\boldsymbol{q}||=1$。

3.1 四元数的运算

 定义两个四元数$\boldsymbol{q}_1=[s_1,\boldsymbol{v}_1]^{\mathrm{T}}$和$\boldsymbol{q}_2=[s_2,\boldsymbol{v}_2]^{\mathrm{T}}$。

加法运算

$$\boldsymbol{q}_1\pm\boldsymbol{q}_2=[s_1\pm s_2,\boldsymbol{v}_1\pm\boldsymbol{v}_2]^{\mathrm{T}} \tag{11}$$

积运算

$$\begin{aligned} &\boldsymbol{q}_1\otimes\boldsymbol{q}_2= \begin{bmatrix} s_1&-\boldsymbol{v}^{\mathrm{T}}_1 \\ \boldsymbol{v}_1&s_1I+\mathrm{sk}(\boldsymbol{v}_1) \end{bmatrix} \boldsymbol{q}_2=Q\boldsymbol{q}_2\\ &\boldsymbol{q}_1\otimes\boldsymbol{q}_2= \begin{bmatrix} s_2&-\boldsymbol{v}^{\mathrm{T}}_2 \\ \boldsymbol{v}_2&s_2I-\mathrm{sk}(\boldsymbol{v}_2) \end{bmatrix} \boldsymbol{q}_1=\bar{Q}\boldsymbol{q}_1 \end{aligned}\tag{12}$$

或

$$\boldsymbol{q}_1\otimes\boldsymbol{q}_2=[s_1s_2-\boldsymbol{v}_1^{\mathrm{T}}\boldsymbol{v}_2,s_1\boldsymbol{v}_2+s_2\boldsymbol{v}_1+\boldsymbol{v}_1\times\boldsymbol{v}_2]^{\mathrm{T}} \tag{13}$$

共轭与逆
 $\boldsymbol{q}_1$的共轭为：

$$\bar{\boldsymbol{q}}_1=[s_1,-\boldsymbol{v}_1]^{\mathrm{T}} \tag{14}$$

 $\boldsymbol{q}_1$的逆为：

$$\boldsymbol{q}_1^{-1}=\frac{\bar{\boldsymbol{q}}_1}{||\boldsymbol{q}_1||}=\bar{\boldsymbol{q}}_1 \tag{15}$$

 共轭四元数满足关系：

$$\begin{aligned} &Q^{\mathrm{T}}(\boldsymbol{q})=Q(\bar{\boldsymbol{q}})\\ &\bar{Q}^{\mathrm{T}}(\boldsymbol{q})=\bar{Q}(\bar{\boldsymbol{q}}) \end{aligned}\tag{16}$$

3.2 四元数旋转变换

 定义矢量$\boldsymbol{p}=[0,x,y,z]^{\mathrm{T}}$，四元数$\boldsymbol{q}=[s,\boldsymbol{v}]^{\mathrm{T}}$。经四元数$\boldsymbol{q}$变换得到的矢量$\boldsymbol{p}’$可表示为：

$$\boldsymbol{p}'=\boldsymbol{q}\otimes\boldsymbol{p}\otimes\boldsymbol{q}^* \tag{17}$$

写成矩阵形式：

$$\boldsymbol{p}'= \begin{bmatrix} s&-\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v}&sI+\mathrm{sk}(\boldsymbol{v}) \end{bmatrix}\begin{bmatrix} s&\boldsymbol{v}^{\mathrm{T}} \\ -\boldsymbol{v}&sI+\mathrm{sk}(\boldsymbol{v}) \end{bmatrix}\boldsymbol{p} \tag{18}$$

3.3 四元数、旋转矩阵和欧拉角

四元数对应的旋转矩阵
 由(18)可以得到四元数对应的旋转矩阵为

$$R=\boldsymbol{v}\boldsymbol{v}^{\mathrm{T}}+(sI+\mathrm{sk}(\boldsymbol{v}))(sI+\mathrm{sk}(\boldsymbol{v}))\tag{19}$$

展开可写作：

$$R=\begin{bmatrix} 1-2v_y^2-2v_z^2&2v_xv_y-2sv_z&2v_xv_z+2sv_y \\ 2v_xv_y+2sv_z& 1-2v_x^2-2v_z^2&2v_yv_z-2sv_x\\ 2v_xv_z-2sv_y&2v_yv_z+2sv_x&1-2v_x^2-2v_y^2 \end{bmatrix} \tag{20}$$

欧拉角对应的四元数
 $\boldsymbol{\Phi}=[\phi,\theta,\psi]^{\mathrm{T}}$绕机体轴按z-y-x顺序的欧拉角对应的四元数可表示为：

$$\begin{aligned} \boldsymbol{q}=&\begin{bmatrix} \mathrm{cos}(\psi/2)\\ 0\\ 0\\ \mathrm{sin}(\psi/2) \end{bmatrix}\otimes\begin{bmatrix} \mathrm{cos}(\theta/2)\\ 0\\ \mathrm{sin}(\theta/2)\\ 0 \end{bmatrix}\otimes\begin{bmatrix} \mathrm{cos}(\phi/2)\\ \mathrm{sin}(\phi/2)\\ 0\\ 0 \end{bmatrix}\\ =&\begin{bmatrix} c(\phi/2)c(\theta/2)c(\psi/2)+s(\phi/2)s(\theta/2)s(\psi/2)\\ s(\phi/2)c(\theta/2)c(\psi/2)-c(\phi/2)s(\theta/2)s(\psi/2)\\ c(\phi/2)s(\theta/2)c(\psi/2)+s(\phi/2)c(\theta/2)s(\psi/2)\\ c(\phi/2)c(\theta/2)s(\psi/2)-s(\phi/2)s(\theta/2)c(\psi/2) \end{bmatrix} \end{aligned} \tag{21}$$

旋转矩阵对应的四元数
 记旋转矩阵为$R=\begin{bmatrix}
r_{11}&r_{12}&r_{13} \\
r_{21}&r_{22}&r_{23} \\
r_{31}&r_{32}&r_{33}
\end{bmatrix}$，旋转矩阵对应的四元数$\boldsymbol{q}=[s,v_x,v_y,v_z]^{\mathrm{T}}$可由以下方式计算：

$$\begin{aligned} &s=\frac{1}{2}\sqrt{1+r_{11}^2+r_{22}^2+r_{33}^2}\\ &v_x=\frac{1}{2}\sqrt{1+r_{11}^2-r_{22}^2-r_{33}^2}\\ &v_y=\frac{1}{2}\sqrt{1-r_{11}^2+r_{22}^2-r_{33}^2}\\ &v_z=\frac{1}{2}\sqrt{1-r_{11}^2-r_{22}^2+r_{33}^2}\\ \end{aligned}\tag{22}$$

3.4 四元数的导数

 定义四元数$\boldsymbol{q}=[s,\boldsymbol{v}]^{\mathrm{T}}$，$\boldsymbol{q}$的导数可以表示为：

$$\begin{aligned} &\dot{\boldsymbol{q}}=\frac{1}{2}[0,\boldsymbol{\omega}_i]^{\mathrm{T}}\otimes\boldsymbol{q}=\frac{1}{2}\begin{bmatrix} s&-\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v}&sI-\mathrm{sk}(\boldsymbol{v}) \end{bmatrix}\begin{bmatrix}0\\\boldsymbol{\omega}_i\end{bmatrix}\\ &\dot{\boldsymbol{q}}=\frac{1}{2}\boldsymbol{q}\otimes[0,\boldsymbol{\omega}_b]^{\mathrm{T}}=\frac{1}{2}\begin{bmatrix} s&-\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v}&sI+\mathrm{sk}(\boldsymbol{v}) \end{bmatrix}\begin{bmatrix}0\\\boldsymbol{\omega}_b\end{bmatrix} \end{aligned} \tag{23}$$

4 李群李代数

4.1 $SO(3)$和$SE(3)$群

 群是一个集合$A$和一种运算$\cdot$构成的代数结构。集合$A$和运算$\cdot$需要满足以下条件：

• 封闭性：$\forall a_1,a_2\in A, \: a_1\cdot a_2\in A$
• 结合律：$\forall a_1,a_2,a_3\in A, \:\: (a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$
• 有单位元（幺元）：$\exists a_0\in A,\:s.t. \:\forall a\in A,\:a_0\cdot a=a\cdot a_0=a$
• 有逆元：$\forall a\in A,\: \exists a^{-1}\in A,\: a\cdot a^{-1}=a_0$

可以看出，旋转矩阵和矩阵乘法是一个李群，记作特殊正交群$SO(3)$：

$$SO(3)=\{R\in\mathbb{R}^{3\times3}|RR^{\mathrm{T}}=I,\mathrm{det}(R)=1\} \tag{24}$$

其中$I\in\mathbb{R}^3$为三维单位矩阵。同样齐次变换矩阵和矩阵乘法也是一个李群，记作特殊欧式群$SE(3)$：

$$SE(3)=\{T=\begin{bmatrix}R&\boldsymbol{t}\\\boldsymbol{0}&1\end{bmatrix}\in\mathbb{R}^{4\times4}|R\in SO(3),\boldsymbol{t}\in\mathbb{R}^3\} \tag{25}$$

4.2 $\mathfrak{so}(3)$和$\mathfrak{se}(3)$李代数

 每个李群都有对应的李代数，李代数描述了李群的局部性质，因此也称为群的切空间。李代数由一个集合$\mathbb{V}$，一个数域$\mathbb{F}$和一个二元运算$[,]$组成的代数结构。集合$\mathbb{V}$，数域$\mathbb{F}$和二元运算$[,]$需要满足以下条件：

• 封闭性：$\forall X,Y\in\mathbb{V},~[X,Y]\in\mathbb{V}$
• 双线性：$\forall X,Y,Z\in\mathbb{V},~a,b\in\mathbb{F}$，有$[aX+bY,Z]=a[X,Z]+b[Y,Z]$和$[Z,aX+bY]=a[Z,X]+b[Z,Y]$
• 自反性：$\forall X\in\mathbb{V},~[X,Z]=0$
• 雅可比等价性：$\forall X,Y,Z\in\mathbb{V}$，有$[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$

其中二元运算$[,]$也称为李括号。可以看出，向量是一个李代数，其对应的李括号为向量积运算。向量对应的反对称矩阵也是一个李代数，反对称矩阵$\Phi_1$和$\Phi_2$的李括号为运算：$[\Phi_1,\Phi_2]=\Phi_1\Phi_2-\Phi_2\Phi_1$。向量和对应反对称矩阵的李括号运算是等价的。

 $SO(3)$群对应的李代数为一个$\mathbb{R}^3$向量或该向量对应的$\mathbb{R}^{3\times3}$反对称矩阵，记作：

$$\mathfrak{so}(3)=\{ \boldsymbol{\phi}\in\mathbb{R}^3,\Phi=\boldsymbol{\phi}^{\wedge}\in\mathbb{R}^{3\times3} \} \tag{26}$$

其中，在$\mathfrak{so}(3)$的定义中，$\wedge$为向量到反对称矩阵的映射，记作$\mathbb{R}^3\rightarrow\mathbb{R}^{3\times3}~\rm{in}~\mathfrak{so}(3)$；$\vee$为反对称矩阵到向量的映射，记作$\mathbb{R^{3\times3}}\rightarrow\mathbb{R}^3~\rm{in}~\mathfrak{so}(3)$。对应的李括号为$[\boldsymbol{\phi}_1, \boldsymbol{\phi}_2]=\boldsymbol{\phi}_1\times\boldsymbol{\phi}_2$或$[\boldsymbol{\phi}_1,\boldsymbol{\phi}_2]=(\Phi_1\Phi_2-\Phi_2\Phi_1)^{\vee}$

 $SE(3)$群对应的李代数为一个$\mathbb{R}^6$向量或该向量对应的$\mathbb{R}^{4\times4}$矩阵，记作：

$$\mathfrak{se}(3)=\{ \boldsymbol{\xi}=\begin{bmatrix}\boldsymbol{\rho}\\ \boldsymbol{\phi}\end{bmatrix}\in\mathbb{R}^6,\boldsymbol{\rho}\in\mathbb{R}^3,\boldsymbol{\phi}\in\mathfrak{so}(3),\boldsymbol{\xi}^{\wedge}=\begin{bmatrix}\boldsymbol{\phi}^{\wedge}&\boldsymbol{\rho}\\\boldsymbol{0}&0\end{bmatrix}\in\mathbb{R}^{4\times4} \} \tag{27}$$

其中，在$\mathfrak{se}(3)$的定义中，$\wedge$为$\mathbb{R}^6\rightarrow\mathbb{R}^{4\times4}~\rm{in}~\mathfrak{se}(3)$的映射；$\vee$为$\mathbb{R^{4\times4}}\rightarrow\mathbb{R}^6~\rm{in}~\mathfrak{se}(3)$。对应的李括号为$[\boldsymbol{\xi}_1, \boldsymbol{\xi}_2]=(\boldsymbol{\xi}^{\wedge}_1\boldsymbol{\xi}^{\wedge}_2-\boldsymbol{\xi}^{\wedge}_2\boldsymbol{\xi}^{\wedge}_1)^{\vee}$。

4.3 指数映射和对数映射

 李群和李代数之间可以通过指数映射和对数映射互相转化：

指数映射：

1. $\mathfrak{so}(3)\rightarrow SO(3)$: $$R=\mathrm{exp}(\boldsymbol{\phi}^{\wedge}) \tag{28}$$ 记：$\boldsymbol{\phi}=\theta\boldsymbol{a}$，$\boldsymbol{a}\in\mathbb{R}^3$为单位向量，利用泰勒展开，指数映射可以表示为: $$\mathrm{exp}(\theta\boldsymbol{a}^{\wedge})=\mathrm{cos}\theta I+(1-\mathrm{cos}\theta)\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+\mathrm{sin}\theta\boldsymbol{a}^{\wedge} \tag{29}$$

1. $\mathfrak{se}(3)\rightarrow SE(3)$： $$\begin{aligned} &T=\begin{bmatrix}\mathrm{exp}(\boldsymbol{\phi}^{\wedge})&J\boldsymbol{\rho}\\\boldsymbol{0}&1\end{bmatrix}\\ &J=\frac{\mathrm{sin}\theta}{\theta}I+(1-\frac{\mathrm{sin}\theta}{\theta})\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+\frac{1-\mathrm{cos}\theta}{\theta}\boldsymbol{a}^{\wedge} \end{aligned}\tag{30}$$

对数映射：

1. $SO(3)\rightarrow \mathfrak{so}(3)$: $$\boldsymbol{\phi}=\mathrm{ln}(R)^{\vee} \tag{31}$$ 记$\boldsymbol{a}=[a_x,a_y,a_z]^{\mathrm{T}}$，对公式(29)展开： $$R=\begin{bmatrix} a_x^2+(1-a_x^2)\mathrm{cos}\theta&a_xa_y(1-\mathrm{cos}\theta)-a_z\mathrm{sin}\theta&a_xa_z(1-\mathrm{cos}\theta)+a_y\mathrm{sin}\theta \\ a_xa_y(1-\mathrm{cos}\theta)+a_z\mathrm{sin}\theta& a_y^2+(1-a_y^2)\mathrm{cos}\theta&a_ya_z(1-\mathrm{cos}\theta)-a_x\mathrm{sin}\theta\\ a_xa_z(1-\mathrm{cos}\theta)-a_y\mathrm{sin}\theta&a_ya_z(1-\mathrm{cos}\theta)+a_x\mathrm{sin}\theta& a_z^2+(1-a_z^2)\mathrm{cos}\theta \end{bmatrix} \tag{32}$$ 可得： $$\begin{aligned} &\boldsymbol{\phi}=\theta\boldsymbol{a}\\ &\theta=\mathrm{acos}(\frac{\mathrm{tr}(R)-1}{2})\\ &\boldsymbol{a}=\frac{1}{2\mathrm{sin}\theta}(R-R^{\mathrm{T}})^{\vee}=\frac{(R-R^{\mathrm{T}})^{\vee}}{||(R-R^{\mathrm{T}})^{\vee}||} \\ &\mathrm{ln}(R)=\frac{\theta}{2\mathrm{sin}\theta}(R-R^{\mathrm{T}}) \end{aligned}\tag{33}$$

1. $SE(3)\rightarrow\mathfrak{se}(3)$: $$\begin{aligned} &\boldsymbol{\xi}=\begin{bmatrix}\boldsymbol{\rho}\\ \boldsymbol{\phi}\end{bmatrix}\\ &\boldsymbol{\phi}=\mathrm{ln}(R)^{\vee}\\ &\boldsymbol{\rho}=J^{-1}\boldsymbol{t} \end{aligned}\tag{34}$$

 可以看出李代数将$SO(3)$群映射为了一个$\mathbb{R}^3$向量，把$SE(3)$群映射成了一个$\mathbb{R}^6$向量。

5 总结和比较

5.1 旋转表示的目标

1. 把旋转表示成一个$\mathbb{R}^3$向量。在运动学和动力学中的角速度$\boldsymbol{\omega}$和力矩$\boldsymbol{\tau}$是$\mathbb{R}^3$向量。因此将旋转表示成$\mathbb{R}^3$向量有助于控制算法和优化算法的设计。

2. 能够使用简单的运算连续且平滑地表示旋转的误差和插值。$SO(3)$不是一个线性空间，当把旋转表示成$\mathbb{R}^3$后，$\mathbb{R}^3$上的线性差值和增量在$SO(3)$空间中并不是两个姿态之间的最短路径。如果直接使$\mathbb{R}^3$的线性差值和增量来表示旋转误差或插值，会导致旋转不平滑，甚至奇异问题的出现。因此，在把旋转表示成$\mathbb{R}^3$向量后，还需要能找到一种关于这个向量的简单运算，使得其能够被用于$SO(3)$空间中旋转误差的表示。

5.1 旋转表示法的物理含义

旋转的“最短路径”
 欧拉旋转定理表明，在三维空间里，一个刚体在做一个旋转的时候，刚体内部至少有一点固定不动。因此刚体的三维旋转可以表示为绕着一个经过旋转点的固定轴的旋转。对于一个刚体，如果能够找到两个姿态之间的旋转轴和旋转角，让一个姿态绕着旋转旋转对应角度到达另一个姿态，则认为这个旋转是两个姿态之间的“最短路径”。在$SO(3)$空间中需要使用这一“最短旋转路径”来表示旋转误差和插值，以保证姿态控制和插值的连续平滑。

欧拉角
 欧拉角将旋转拆分成了3个简单的顺序旋转，能够帮助我们快速建立坐标系之间的旋转关系。对于一些机构，例如满足piper准则的机械臂球型手腕，其关节空间反映的就是末端坐标系的欧拉角空间，这在机构控制中会带来一些便捷。但欧拉角表示存在奇异，也就是常说的万向节死锁问题，这对大范围的姿态旋转表示是不利的，此外欧拉角也无法通过简单的运算来平滑地表示旋转误差和旋转插值。

四元数
 对于四元数$\boldsymbol{q}=[s,\boldsymbol{v}]^{\mathrm{T}}=[\mathrm{cos}\frac{\theta}{2},\mathrm{sin}\frac{\theta}{2}\boldsymbol{a}]^{\mathrm{T}}$。$\boldsymbol{a}$为轴角法中的旋转轴，$\theta$为绕旋转轴旋转的角度。四元数能够使用乘法运算来平滑地表示两个姿态的旋转误差，同时将这一误差映射到单位四维超球面$\mathbb{S}^3\subset\mathbb{R}^4$空间。
 根据轴角法的定义，对于相同的旋转轴，$\theta$和$\theta+2\pi$表示的是同一个旋转，将这两个角带如四元数表示中，会得到$\boldsymbol{q}=-\boldsymbol{q}$的结果，这就是四元数的双倍覆盖问题。也就是说值相同，符号相反的两个四元数表示的是同一个旋转，因此四元数对旋转的表示是不唯一的。

李代数
 对于李代数$\boldsymbol{\phi}=\theta\boldsymbol{a}$，$\boldsymbol{a}$为轴角法中的旋转轴，$\theta$为绕旋转轴旋转的角度。$SO(3)$空间的旋转误差通过李代数映射被成一个$\mathbb{R^3}$向量。

5.3 旋转表示法的比较

姿态描述	所属空间	全局性	唯一性	旋转误差	误差范数在$SO(3)$空间中是否为最短路径
旋转矩阵	$SO(3)\subset\mathbb{R}^{3\times3}$	是	是	$R_d^\mathrm{T}R\rightarrow I$	是
欧拉角	$\mathbb{R}^3$	否	否	$\boldsymbol{\Phi}-\boldsymbol{\Phi}_d\rightarrow \boldsymbol{0}$	否
四元数	$\mathbb{S}^3\subset\mathbb{R}^4$	是	否	$\bar{\boldsymbol{q}}_d\otimes\boldsymbol{q}\rightarrow[1,0,0,0]^\mathrm{T}$	是
轴角法	$\mathbb{R}^4$	是	否	$\theta-\theta_d\rightarrow 0$	是
李代数	$\mathfrak{so}_3\subset\mathbb{R}^3$	是	是	$\mathrm{ln}(R_d^\mathrm{T}R)^{\vee}\rightarrow \boldsymbol{0}$	是

参考文献

[1] Lynch K M, Park F C. Modern robotics[M]. Cambridge University Press, 2017.
[2] Chaturvedi N A, Sanyal A K, McClamroch N H. Rigid-body attitude control[J]. IEEE control systems magazine, 2011, 31(3): 30-51.
[3] https://github.com/Krasjet/quaternion
[4] 高翔，张涛，”视觉SLAM十四讲”[M]. 电子工业出版社，2017.