刚体姿态及旋转的表示方法

坐标系中，一个刚体的状态可用位置和姿态来描述，位置即为该刚体在坐标系中的空间位置，用一个坐标向量即可表达。而要描述刚体的姿态，有很多种表示方法，以下列出常用的：

1 旋转矩阵（Rotation matrix）

旋转矩阵是在乘以一个向量的时候有改变向量的方向但不改变大小的效果的矩阵，旋转矩阵可以直接运算，它是用9个量来表达一个旋转，但实际上一次旋转只有3个自由度。因此旋转矩阵表达式是冗余的。同时，对于旋转矩阵自身也有约束，它必须是正交矩阵，且行列式为1，这些约束会使求解变得困难。
旋转矩阵比较好理解，这里不多说，可见：坐标变换及旋转矩阵

2 欧拉角（Euler angles）

2.1 欧拉角

欧拉角是描述刚体姿态的一组3个角值的参量，同时需要指定三个角度转动时分别围绕的坐标轴，以及旋转的顺序，一旦确定了这些，那么这个欧拉角的描述就可以分解为三个基本的旋转，三个基本旋转的旋转矩阵为：
绕X轴旋转θ的旋转矩阵：$R_X(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -sin\theta \\ 0& sin\theta & cos\theta \end{array}\right]$
绕Y轴旋转θ的旋转矩阵：$R_Y(\theta )=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]$
绕Z轴旋转θ的旋转矩阵：$R_Z(\theta )=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]$
欧拉角的个人理解

2.1 欧拉角与旋转矩阵

任何关于刚体旋转的旋转矩阵均可以由三个基本旋转矩阵复合而成。但要考虑到欧拉角分为内旋和外旋，内旋和外旋的三个基本旋转组合顺序是有区别的，内旋时三个基本旋转矩阵需右乘，外旋时三个基本旋转矩阵需左乘。

外旋欧拉角计算

设三个轴x，y，z的欧拉角分别为α,β,γ，外旋，那么旋转矩阵为（为了简洁，以下sin和cos分别用s和c表达）
$$R(\alpha ,\beta ,\gamma )=R_z(\gamma )R_y(\beta )R_x(\alpha )=\left[\begin{array}{ccc}c\beta c\gamma & c\gamma s\alpha s\beta -c\alpha s\gamma & s\alpha s\gamma +c\alpha c\gamma s\beta \\ c\beta s\gamma & c\alpha c\gamma +s\alpha s\beta s\gamma & c\alpha s\beta s\gamma -c\gamma s\gamma \\ -s\beta & c\beta s\alpha & c\alpha c\beta \end{array}\right]$$
反之，对应这个旋转，已知旋转矩阵$\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$，欧拉角可解得
$$\begin{gathered} \alpha =arctg(\frac{r_{32}}{r_{33}}) \\ \beta =arctg(\frac{-r_{31}}{\sqrt{r_{32}^2+r_{33}^2}}) \\ \gamma =arctg(\frac{r_{21}}{r_{11}}) \end{gathered}$$

内旋欧拉角计算

设内旋顺序为Z-X-Y（Z轴向上，Y轴向前），旋转角分别为yaw、pitch、roll，则旋转矩阵为
$$R(yaw,pitch,roll)=R_z(yaw)R_x(pitch)R_y(roll)$$
反之，对应这个旋转，已知旋转矩阵$\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$，内旋欧拉角可解得
$$\begin{gathered} yaw=-arctg(\frac{r_{12}}{r_{22}}) \\ pitch=arcsin(r_{32}) \\ roll=-arctg(\frac{r_{31}}{r_{33}}) \end{gathered}$$

3 轴角（Axial angle）

3.1 轴角

轴角是用两个参数表示旋转: 一个轴或直线，和绕这个轴的旋转的角度。它也叫做旋转的指数坐标。
给定一个向量$\hat{K}=(k_x,k_y,k_z)$，以这个向量$\hat{K}$为旋转轴，旋转$\theta$角，这样我们用一个四维向量$(k_x,k_y,k_z,\theta )$就可以来表示这个旋转。

3.2 旋转向量

轴角有时候也用旋转向量表示，以上说轴角是用一个四维向量来表达一个旋转，而更紧凑的表示方式是用一个单位向量来表示轴，而用该三维向量的长度来表示角度值θ。这样一来，就可以简化到用一个三维向量来表示这个旋转，这就是旋转向量。
即：将以上的轴向量$\hat{K}=(k_x,k_y,k_z)$取为单位向量${\hat{K}}_0=(k_{x0},k_{y0},k_{z0})$，得到旋转向量即为：
$$\mathbf{R}=\left[\begin{array}{c}{R}_x\\ R_y\\ R_z\end{array}\right]=\left[\begin{array}{c}\theta \cdot k_{x0}\\ \theta \cdot k_{y0}\\ \theta \cdot k_{z0}\end{array}\right]$$
同样，通过旋转向量也可得到轴角参数
$$\begin{gathered} \theta =\sqrt{R_x^2+R_y^2+R_z^2} \\ {k}_{x0}=R_x/\theta ,k_{y0}=R_y/\theta ,k_{z0}=R_z/\theta \end{gathered}$$

3.3 轴角与旋转矩阵

已知轴角矢量$(k_x,k_y,k_z,\theta )$，根据Rodrigues旋转公式，可求得旋转矩阵为
$$R_{\hat{K}}(\theta )=c\theta I+(1-c\theta )r{r}^T+s\theta \left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$$
其中$r=(k_x,k_y,k_z{)}^T$，即：$$R_{\hat{K}}(\theta )=\left[\begin{array}{ccc}c\theta +k_x^2(1-c\theta )& k_x{k}_y(1-c\theta )-k_{z}s\theta & k_x{k}_z(1-c\theta )+k_{y}s\theta \\ k_y{k}_x(1-c\theta )+k_{z}s\theta & c\theta +k_y^2(1-c\theta )& k_y{k}_z(1-c\theta )-k_{x}s\theta \\ k_z{k}_x(1-c\theta )-k_{y}s\theta & k_z{k}_y(1-c\theta )+k_{x}s\theta & c\theta +k_z^2(1-c\theta )\end{array}\right]$$
以上转换公式在Opencv中有相关的实现函数：Rodrigues2()。
反之，已知旋转矩阵$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$，也可以求出轴角参数
$$s\theta \left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]=\frac{R-R^T}{2}$$
即
$$\begin{gathered} \theta =arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2}) \\ \hat{K}=\left[\begin{array}{c}{k}_x\\ k_y\\ k_z\end{array}\right]=\frac{1}{2sin\theta }\left[\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right] \end{gathered}$$
可以看到，当$\theta$为0或$2\Pi$时，旋转轴不能确定，是奇异的。

4 四元数（Quaternions）

4.1 四元数

引入四元数以解决欧拉角、轴角、旋转矩阵等的不足：

• 欧拉角的万向节锁问题；
• 轴角的奇异问题；
• 旋转矩阵的9个数值不相互独立。

四元数是简单的超复数。 复数是由实数加上虚数单位 $i$ 组成，其中$i^2=-1$。 相似地，四元数是由实数加上三个虚数单位 $i$、$j$、$k$组成，而且它们有如下的关系： $i^2=j^2=k^2=-1，i^0=j^0=k^0=1$，每个四元数都是1、$i$、$j$和$k$ 的线性组合，即是四元数一般可表示为$a+bi+cj+dk$，其中$a$、$b$、$c$、$d$是实数。
设单位四元数$\mathbf{q}=(q_0,q_1,q_2,q_3)$，有$q_0^2+q_1^2+q_2^2+q_3^2=1$，如有单位向量$(x,y,z)$为旋转轴，旋转角度$\theta$，则对应的四元数为：$\mathbf{q}=(cos\frac{\theta }{2},xsin\frac{\theta }{2},ysin\frac{\theta }{2},zsin\frac{\theta }{2})$
设有一个点${\mathbf{P}}_1=(p_x,p_y,p_z)$，我们要用以上这个旋转四元数将其旋转变换到${\mathbf{P}}_1^{\prime }=(p_x^{\prime },p_y^{\prime },p_z^{\prime })$，计算过程如下：

• 1 定义纯四元数：${\mathbf{q}}_p=(0,p_x,p_y,p_z)=0+p_x\ast i+p_y\ast j+p_z\ast k$
• 2 进行四元数运算：${\mathbf{q}}_p^{\prime }=\mathbf{q}{\mathbf{q}}_p{\mathbf{q}}^{-1}$，其中${\mathbf{q}}^{-1}$为$\mathbf{q}$的共轭：${\mathbf{q}}^{-1}=(cos\frac{\theta }{2},-xsin\frac{\theta }{2},-ysin\frac{\theta }{2},-zsin\frac{\theta }{2})$
• 3 产生的${\mathbf{q}}_p^{\prime }$一定是纯四元数，也就是说它的第一项为0，有如下形式：${\mathbf{q}}_p^{\prime }=(0,p_x^{\prime },p_y^{\prime },p_z^{\prime })=0+p_x^{\prime }\ast i+p_y^{\prime }\ast j+p_z^{\prime }\ast k$
  后三项$(p_x^{\prime },p_y^{\prime },p_z^{\prime })$就是旋转变换后的坐标向量。

因此，引入四元数后，对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，没有轴角计算的奇点现象，表示方式更加紧凑，计算量也可以小一些。
同理，如果你有一个四元数：$\mathbf{q}=(q_0,q_1,q_2,q_3)=(cos\frac{\theta }{2},xsin\frac{\theta }{2},ysin\frac{\theta }{2},zsin\frac{\theta }{2})$，那么，它对应一个以向量$(x,y,z)$为轴旋转θ角度的旋转操作。

4.2 四元数与旋转矩阵

已知四元数$\mathbf{q}=(q_0,q_1,q_2,q_3)$，可求得旋转矩阵
$$R=\left[\begin{array}{ccc}2q_0^2+2q_1^2-1& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_3-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 2q_0^2+2q_2^2-1& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 2q_0^2+2q_3^2-1\end{array}\right]$$
反之，已知旋转矩阵$\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$，求四元数，分为两种情况：
情况1：
$$\begin{gathered} q_0=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2} \\ {q}_1=\frac{r_{32}-r_{23}}{4q_0},q_2=\frac{r_{13}-r_{31}}{4q_0},q_3=\frac{r_{21}-r_{12}}{4q_0}, \end{gathered}$$
情况2：如果$q_0$趋近于0，则求解四元数过程为：
2.1 如果$max(r_{11},r_{22},r_{33})=r_{11}$，
$$\begin{gathered} t=\sqrt{1+r_{11}-r_{22}-r_{33}} \\ {q}_0=\frac{r_{32}-r_{23}}{t},q_1=t/4,q_2=\frac{r_{13}+r_{31}}{t},q_3=\frac{r_{12}+r_{21}}{t} \end{gathered}$$
2.2 如果$max(r_{11},r_{22},r_{33})=r_{22}$，
$$\begin{gathered} t=\sqrt{1-r_{11}+r_{22}-r_{33}} \\ {q}_0=\frac{r_{13}-r_{31}}{t},q_1=\frac{r_{12}+r_{21}}{t},q_2=t/4,q_3=\frac{r_{32}+r_{23}}{t} \end{gathered}$$
2.3 如果$max(r_{11},r_{22},r_{33})=r_{33}$，
$$\begin{gathered} t=\sqrt{1-r_{11}-r_{22}+r_{33}} \\ {q}_0=\frac{r_{21}-r_{12}}{t},q_1=\frac{r_{13}+r_{31}}{t},q_2=\frac{r_{23}-r_{32}}{t},q_3=t/4 \end{gathered}$$

参考

简书：姿态描述—四元数，旋转矩阵以欧拉角
知乎：如何形象地理解四元数？
旋转矩阵、欧拉角、四元数理论及其转换关系
三维重建学习(1)：基础知识：旋转矩阵与旋转向量