Rotation Representations in Robotics

介绍了四种姿态表示方式：旋转矩阵、欧拉角、轴角 和 四元数，以及它们之间相互转换关系的推导

旋转矩阵 Rotation Matrix

对于物体坐标系 $\{O\}$ 下三个主轴的单位矢量 ${\hat{X}}_O$、${\hat{Y}}_O$ 和 ${\hat{Z}}_O$ 在世界坐标系 $\{W\}$ 上的投影 ${}^W{\hat{X}}_O$、${}^W{\hat{Y}}_O$ 和 ${}^W{\hat{Z}}_O$，可以利用旋转矩阵 ${}_O^{W}R$ 来表示物体坐标系的姿态

$$\begin{array}{r}{}_O^{W}R=\left(\begin{array}{ccc}{}^W{\hat{X}}_O& {}^W{\hat{Y}}_O& {}^W{\hat{Z}}_O\end{array}\right)=\left(\begin{array}{ccc}{\hat{X}}_O\cdot {\hat{X}}_W& {\hat{Y}}_O\cdot {\hat{X}}_W& {\hat{Z}}_O\cdot {\hat{X}}_W\\ {\hat{X}}_O\cdot {\hat{Y}}_W& {\hat{Y}}_O\cdot {\hat{Y}}_W& {\hat{Z}}_O\cdot {\hat{Y}}_W\\ {\hat{X}}_O\cdot {\hat{Z}}_W& {\hat{Y}}_O\cdot {\hat{Z}}_W& {\hat{Z}}_O\cdot {\hat{Z}}_W\end{array}\right)=\left(\begin{array}{c}{}^O{\hat{X}}_W^T\\ {}^O{\hat{Y}}_W^T\\ {}^O{\hat{Z}}_W^T\end{array}\right)\end{array}$$

显然，这是一个正交矩阵

$$\begin{array}{r}{}_O^W{R}^T{}_O^{W}R=\left(\begin{array}{c}{}^W{\hat{X}}_O^T\\ {}^W{\hat{Y}}_O^T\\ {}^W{\hat{Z}}_O^T\end{array}\right)\left(\begin{array}{ccc}{}^W{\hat{X}}_O& {}^W{\hat{Y}}_O& {}^W{\hat{Z}}_O\end{array}\right)=I_3\end{array}$$

进一步，由于旋转矩阵的所有行向量的模长均为 1，且行向量之间两两正交，有 $det(R)=1$

更普遍地，我们用 $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)$ 来表示一个旋转矩阵

接下来我们推导绕任一轴旋转的旋转矩阵的表达形式

记三维空间中一任意向量 $\mathbf{v}$ 绕旋转轴 $\mathbf{n}\mathbf{=}\left(\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right)$ 旋转 $\theta$ 得到新向量 ${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}$，其中 $\mathbf{n}$ 为单位向量，$n_x^2+n_y^2+n_z^2=1$

则 $\mathbf{v}$ 可以根据平行和正交于 $\mathbf{n}$ 分为两部分 ${\mathbf{v}}_{\parallel }$ 和 ${\mathbf{v}}_{\perp }$，满足 $\mathbf{v}\mathbf{=}{\mathbf{v}}_{\parallel }\mathbf{+}{\mathbf{v}}_{\perp }$

其中

$$\{\begin{array}{l}{\mathbf{v}}_{\parallel }\mathbf{=}\mathbf{(}\mathbf{v}\cdot \mathbf{n}\mathbf{)}\mathbf{n}\\ {\mathbf{v}}_{\perp }\mathbf{=}\mathbf{v}\mathbf{-}\mathbf{(}\mathbf{v}\cdot \mathbf{n}\mathbf{)}\mathbf{n}\mathbf{=}\mathbf{-}\mathbf{n}\times \mathbf{(}\mathbf{n}\times \mathbf{v}\mathbf{)}\end{array}$$

而

$$\begin{array}{rl}{\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}& ={\mathbf{v}}_{\parallel \mathbf{r}\mathbf{o}\mathbf{t}}\mathbf{+}{\mathbf{v}}_{\perp \mathbf{r}\mathbf{o}\mathbf{t}}\\ & ={\mathbf{v}}_{\parallel }\mathbf{+}\cos \theta {\mathbf{v}}_{\perp }\mathbf{+}\sin \theta \mathbf{n}\times {\mathbf{v}}_{\perp }\\ & =(\mathbf{v}\cdot \mathbf{n}\mathbf{)}\mathbf{n}\mathbf{+}\cos \theta {\mathbf{v}}_{\perp }\mathbf{+}\sin \theta \mathbf{n}\times \mathbf{v}\\ & =\cos \theta \mathbf{v}\mathbf{+}\mathbf{(}\mathbf{1}\mathbf{-}\cos \theta \mathbf{)}\mathbf{(}\mathbf{n}\cdot \mathbf{v}\mathbf{)}\mathbf{n}\mathbf{+}\sin \theta \mathbf{n}\times \mathbf{v}\end{array}$$

上式就是 罗德里格斯旋转公式 Rodrigues' Rotation Formula，进一步我们令 $\mathbf{v}$ 分别为 ${\left(\begin{array}{ccc}1& 0& 0\end{array}\right)}^T$，${\left(\begin{array}{ccc}0& 1& 0\end{array}\right)}^T$ 和 ${\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T$

解得

$$\begin{array}{rl}{\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}& =\cos \theta \left(\begin{array}{c}1\\ 0\\ 0\end{array}\right)+(1-\cos \theta )\cdot n_x\left(\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right)+\sin \theta \left(\begin{array}{c}0\\ n_z\\ -n_y\end{array}\right)\\ & =\left(\begin{array}{c}(1-\cos \theta )n_x^2+\cos \theta \\ (1-\cos \theta )n_x{n}_y+\sin \theta n_z\\ (1-\cos \theta )n_x{n}_z-\sin \theta n_y\end{array}\right)\end{array}$$$${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}\mathbf{=}\left(\begin{array}{c}(1-\cos \theta )n_y{n}_x-\sin \theta n_z\\ (1-\cos \theta )n_y^2+\cos \theta \\ (1-\cos \theta )n_y{n}_z+\sin \theta n_x\end{array}\right)$$$${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}\mathbf{=}\left(\begin{array}{c}(1-\cos \theta )n_z{n}_x+\sin \theta n_y\\ (1-\cos \theta )n_z{n}_y-\sin \theta n_x\\ (1-\cos \theta )n_z^2+\cos \theta \end{array}\right)$$

综上得到绕任意轴的旋转矩阵

$$R=\left(\begin{array}{ccc}{n}_x^2(1-\cos \theta )+\cos \theta & n_x{n}_y(1-\cos \theta )-n_z\sin \theta & n_x{n}_z(1-\cos \theta )+n_y\sin \theta \\ n_y{n}_x(1-\cos \theta )+n_z\sin \theta & n_y^2(1-\cos \theta )+\cos \theta & n_y{n}_z(1-\cos \theta )-n_x\sin \theta \\ n_z{n}_x(1-\cos \theta )-n_y\sin \theta & n_z{n}_y(1-\cos \theta )+n_x\sin \theta & n_z^2(1-\cos \theta )+\cos \theta \end{array}\right)$$

欧拉角 Euler Angles

定义：

• 顺规：合法的欧拉角组中，任何两个连续的旋转，必须绕着不同的转动轴旋转，因此一共有 12 种顺规，被划分为两大类：
  • 经典欧拉角 Proper Euler Angles: zxz, xyx, yzy, zyz, xzx, yxy
  • 泰特-布莱恩角 Tait-Bryan Angles: xyz, yzx, zxy, xzy, zyx, yxz 其中按zyx顺序旋转的情况又被称为 RPY 角或 XYZ 固定角
• 内外旋：根据绕旋转后的新轴旋转还是绕世界坐标系中固定不动的轴旋转，欧拉角被分为内旋和外旋：
  • 内旋 Intrisic Rotation, 又称为动态旋转，绕物体坐标系旋转
  • 外旋 Extrinsic Rotation, 又称为静态旋转，绕世界坐标系旋转
  • 内旋与外旋具有等价性

欧拉角 2 旋转矩阵

$$\begin{array}{rl}R& =R_Z(\gamma )R_Y(\beta )R_X(\alpha )=\left(\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right)\\ \\ & =R_X^{-1}(\gamma )R_Y^{-1}(\beta )R_Z^{-1}(\alpha )=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & \sin \alpha \\ 0& -\sin \alpha & \cos \alpha \end{array}\right)\left(\begin{array}{ccc}\cos \beta & 0& -\sin \beta \\ 0& 1& 0\\ \sin \beta & 0& \cos \beta \end{array}\right)\left(\begin{array}{ccc}\cos \gamma & \sin \gamma & 0\\ -\sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)\end{array}$$

旋转矩阵 2 欧拉角

以旋转顺序为 zyx 的内旋欧拉角为例

$$\begin{array}{rl}R=R_Z(\gamma )R_Y(\beta )R_X(\alpha )& =\left(\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right)\\ \\ & =\left(\begin{array}{ccc}\cos \gamma \cos \beta & -\sin \gamma \cos \alpha +\cos \gamma \sin \beta \sin \alpha & \sin \gamma \sin \alpha +\cos \gamma \sin \beta \cos \alpha \\ \sin \gamma \cos \beta & \cos \gamma \cos \alpha +\sin \gamma \sin \beta \sin \alpha & -\cos \gamma \sin \alpha +\sin \gamma \sin \beta \cos \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \cos \alpha \end{array}\right)\end{array}$$

由此

$$\{\begin{array}{l}\alpha =\arctan (\frac{r_{32}}{r_{33}})\\ \beta =\arcsin (-r_{31})\\ \gamma =\arctan (\frac{r_{21}}{r_{11}})\end{array}$$

但当 $\beta =\frac{\pi }{2}$ 时，$R=\left(\begin{array}{ccc}0& -\sin (\gamma -\alpha )& cos(\gamma -\alpha )\\ 0& \cos (\gamma -\alpha )& \sin (\gamma -\alpha )\\ 1& 0& 0\end{array}\right)$，旋转矩阵退化，无法解出唯一 $\gamma$ 和$\alpha$，这种情况被称为 奇异性 Singularity 或 万向节死锁 Glimbal Lock，可以被视为三维空间的物体姿态无法通过三个实数进行完全表达。在实践中，可以预分析机械最难出现的姿态，并通过有意地设计欧拉角顺规，尽可能避免出现万向节死锁的情况。

轴角 Axis Angle

轴角通过两个参数描述一个旋转：一条轴和描述绕这个轴的旋转量的角度，记为 $$\begin{gathered} <axis,angle>=\left(\begin{array}{c}{\left[a_x \\ {a}_y \\ {a}_z\right]}^T,\theta \end{array}\right) \end{gathered}$$

轴角 2 旋转矩阵

由罗德里格斯旋转公式可推出绕任意轴旋转的旋转矩阵，具体推导见 旋转矩阵 Rotation Matrix

$$R=\left(\begin{array}{ccc}{a}_x^2(1-\cos \theta )+\cos \theta & a_x{a}_y(1-\cos \theta )-a_z\sin \theta & a_x{a}_z(1-\cos \theta )+a_y\sin \theta \\ a_y{a}_x(1-\cos \theta )+a_z\sin \theta & a_y^2(1-\cos \theta )+\cos \theta & a_y{a}_z(1-\cos \theta )-a_x\sin \theta \\ a_z{a}_x(1-\cos \theta )-a_y\sin \theta & a_z{a}_y(1-\cos \theta )+a_x\sin \theta & a_z^2(1-\cos \theta )+\cos \theta \end{array}\right)$$

旋转矩阵 2 轴角

观察上式，对给定旋转矩阵，有

$$\{\begin{array}{l}\theta =\arccos (\frac{1}{2}(r_{11}+r_{22}+r_{33}-1))\\ a_x=\frac{r_{32}-r_{23}}{\sqrt{(r_{32}-r_{23}{)}^2+(r_{13}-r_{31}{)}^2+(r_{21}-r_{12}{)}^2}}\\ a_y=\frac{r_{13}-r_{31}}{\sqrt{(r_{32}-r_{23}{)}^2+(r_{13}-r_{31}{)}^2+(r_{21}-r_{12}{)}^2}}\\ a_z=\frac{r_{21}-r_{12}}{\sqrt{(r_{32}-r_{23}{)}^2+(r_{13}-r_{31}{)}^2+(r_{21}-r_{12}{)}^2}}\end{array}$$

欧拉角 2 轴角

愉快地，我们可以利用 欧拉角 --> 四元数 --> 轴角 的方式得到

$$\{\begin{array}{l}\theta =2\ast \arccos (\cos \gamma \cos \beta \cos \alpha -\sin \gamma \sin \beta \sin \alpha )\\ a_x=\sin \gamma \sin \beta \cos \alpha +\cos \gamma \cos \beta \sin \alpha \\ a_y=\sin \gamma \cos \beta \cos \alpha +\cos \gamma \sin \beta \sin \alpha \\ a_z=\cos \gamma \sin \beta \cos \alpha -\sin \gamma \cos \beta \sin \alpha \end{array}$$

轴角 2 欧拉角

直接从轴角转为欧拉角比较困难，我们可以利用 轴角 --> 旋转矩阵 --> 欧拉角 的间接转化方式减少计算难度

$$\{\begin{array}{l}\alpha =\arctan (\frac{a_y{a}_z(1-\cos \theta )-a_x\sin \theta }{a_z^2(1-\cos \theta )+\cos \theta })\\ \beta =\arcsin (a_y\sin \theta -a_x{a}_z(1-\cos \theta ))\\ \gamma =\arctan (\frac{a_x{a}_y(1-\cos \theta )-a_z\sin \theta }{a_x^2(1-\cos \theta )+\cos \theta })\end{array}$$

使用欧拉角进行表示时，某些情况会出现万向节锁和奇异的现象，需要特殊处理

四元数 Quaterions

不妨记四元数为 $q=w+xi+yj+zk$，其中 $i^2=j^2=k^2=ijk=-1$，而单位四元数满足 $w^2+x^2+y^2+z^2=1$

绕任意轴 $n$ 旋转 $\theta$ 的四元数，表示为 $q=[w,x,y,z]=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}{n}_x,\sin \frac{\theta }{2}{n}_y,\sin \frac{\theta }{2}{n}_z]$

轴角 2 四元数

$$\{\begin{array}{l}w=\cos \frac{\theta }{2}\\ x=a_x\sin \frac{\theta }{2}\\ y=a_y\sin \frac{\theta }{2}\\ z=a_z\sin \frac{\theta }{2}\end{array}$$

四元数 2 轴角

$$\{\begin{array}{l}\theta =2\ast \arccos (w)\\ a_x=\frac{x}{\sqrt{1-w^2}}\\ a_y=\frac{y}{\sqrt{1-w^2}}\\ a_z=\frac{z}{\sqrt{1-w^2}}\end{array}$$

四元数 2 旋转矩阵

由罗德里格斯旋转公式可推出绕任意轴旋转的旋转矩阵，具体推导见 旋转矩阵 Rotation Matrix

$$R=\left(\begin{array}{ccc}{n}_x^2(1-\cos \theta )+\cos \theta & n_x{n}_y(1-\cos \theta )-n_z\sin \theta & n_x{n}_z(1-\cos \theta )+n_y\sin \theta \\ n_y{n}_x(1-\cos \theta )+n_z\sin \theta & n_y^2(1-\cos \theta )+\cos \theta & n_y{n}_z(1-\cos \theta )-n_x\sin \theta \\ n_z{n}_x(1-\cos \theta )-n_y\sin \theta & n_z{n}_y(1-\cos \theta )+n_x\sin \theta & n_z^2(1-\cos \theta )+\cos \theta \end{array}\right)$$

将 $n_x,n_y,n_z,\cos \theta ,\sin \theta$ 用 $w,x,y,z$ 表示

$$\{\begin{array}{l}\cos \theta =1-2{\cos }^2\frac{\theta }{2}\\ \sin \theta =2\sin \frac{\theta }{2}\cos \frac{\theta }{2}\end{array}$$

可以推出以下表达

$$\begin{array}{r}{R}_q=\left(\begin{array}{ccc}1-2y^2-2z^2& 2xy-2wz& 2xz+2wy\\ 2xy+2wz& 1-2x^2-2z^2& 2yz-2wx\\ 2xz-2wy& 2yz+2wx& 1-2x^2-2y^2\end{array}\right)\end{array}$$

旋转矩阵 2 四元数

当给定旋转矩阵后，观察得到

$$\begin{array}{r}{r}_{32}-r_{23}=(2yz+2wx)-(2yz-2wx)=4wx\\ r_{13}-r_{31}=(2xz+2wy)-(2xz-2wy)=4wy\\ r_{21}-r_{12}=(2xy+2wz)-(2xy-2wz)=4wz\end{array}$$

又有

$$r_{11}+r_{22}+r_{33}=3-4x^2-4y^2-4z^2=4w^2-1$$

进一步，可得

$$\{\begin{array}{l}w=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}}\\ x=\frac{r_{32}-r_{23}}{4w}\\ y=\frac{r_{13}-r_{31}}{4w}\\ z=\frac{r_{21}-r_{12}}{4w}\end{array}$$

欧拉角 2 四元数

对于欧拉角转四元数，我们可以根据四元数的定义写出以下表达

$$\begin{array}{r}q(\gamma ,\beta ,\alpha )=q_z(\gamma )q_y(\beta )q_x(\alpha )=\left(\begin{array}{c}\cos \frac{\gamma }{2}\\ 0\\ 0\\ \sin \frac{\gamma }{2}\end{array}\right)\left(\begin{array}{c}\cos \frac{\beta }{2}\\ 0\\ \sin \frac{\beta }{2}\\ 0\end{array}\right)\left(\begin{array}{c}\cos \frac{\alpha }{2}\\ \sin \frac{\alpha }{2}\\ 0\\ 0\end{array}\right)=\left(\begin{array}{c}\cos \frac{\gamma }{2}\cos \frac{\beta }{2}\cos \frac{\alpha }{2}-\sin \frac{\gamma }{2}\sin \frac{\beta }{2}\sin \frac{\alpha }{2}\\ \sin \frac{\gamma }{2}\sin \frac{\beta }{2}\cos \frac{\alpha }{2}+\cos \frac{\gamma }{2}\cos \frac{\beta }{2}\sin \frac{\alpha }{2}\\ \cos \frac{\gamma }{2}\sin \frac{\beta }{2}\cos \frac{\alpha }{2}-\sin \frac{\gamma }{2}\cos \frac{\beta }{2}\sin \frac{\alpha }{2}\\ \sin \frac{\gamma }{2}\cos \frac{\beta }{2}\cos \frac{\alpha }{2}+\cos \frac{\gamma }{2}\sin \frac{\beta }{2}\sin \frac{\alpha }{2}\end{array}\right)\end{array}$$

四元数 2 欧拉角

直接从四元数转为欧拉角比较困难，我们可以利用 四元数 --> 旋转矩阵 --> 欧拉角 的间接转化方式减少计算难度

$$\{\begin{array}{l}\alpha =\arctan (\frac{2yz+2wx}{1-2x^2-2y^2})\\ \beta =\arcsin (2wy-2xz)\\ \gamma =\arctan (\frac{2xy+2wz}{1-2y^2-2z^2})\end{array}$$

同样地，使用欧拉角进行表示，某些情况会出现万向节锁和奇异的现象，需要特殊处理

表示方法对比

	旋转矩阵 Rotation Matrix	欧拉角 Euler Angles	轴角 Axis Angle	四元数 Quarternions
优	可以表示三维空间任一姿态，并推广到 n 维空间	符合直觉	解决了万向锁	可以平滑插值，无万向节锁现象（本质是从四维空间描述三维姿态）
劣	三维空间的姿态却用了九个量进行表示，存储冗余	奇异和万向节锁现象无法克服，无法平滑插值	无法平滑插值（本质是三维的）	只对三维空间有效

参考资料

影响	来源
	https://en.wikipedia.org/wiki/Rodrigues'_rotation_formula#/media/File:Orthogonal_decomposition_unit_vector_rodrigues_rotation_formula.svg