三维刚体运动（二）

旋转变换

本文主要介绍三维刚体运动中的不同旋转表示之间的转换推导。

1 欧拉角和旋转矩阵

假设角度都是相对于自转物体本身，即内旋，按照Z-Y-X的顺序旋转推导如下：
$$\begin{gathered} R({\theta }_3,{\theta }_2,{\theta }_1)=R_z({\theta }_3)R_y({\theta }_2)R_x({\theta }_1) \\ =\left[\begin{array}{ccc}{c}_3& -s_3& 0\\ s_3& c_3& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{c}_2& 0& s_2\\ 0& 1& 0\\ -s_2& 0& c_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c_1& -s_1\\ 0& s_1& c_1\end{array}\right] \\ =\left[\begin{array}{ccc}{c}_2{c}_3& -s_3& s_2{c}_3\\ c_2{s}_3& c_3& s_2{s}_3\\ -s_2& 0& c_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c_1& -s_1\\ 0& s_1& c_1\end{array}\right] \\ =\left[\begin{array}{ccc}{c}_2{c}_3& s_1{s}_2{c}_3-c_1{s}_3& c_1{s}_2{c}_3+s_1{s}_3\\ c_2{s}_3& s_1{s}_2{s}_3+c_1{c}_3& c_1{s}_2{s}_3-s_1{c}_3\\ -s_2& s_1{c}_2& c_1{c}_2\end{array}\right] \end{gathered}$$

旋转矩阵转欧拉角，不考虑万向锁的情况，推导如下：
$$\begin{gathered} {\theta }_2=arcsin(s_2)=arcsin(-m_{31}) \\ {\theta }_1=atan2(s_1{c}_2,c_1{c}_2)=atan2(m_{32},m_{33}) \\ {\theta }_3=atan2(c_2{s}_3,c_2{c}_3)=atan2(m_{21},m_{11}) \end{gathered}$$

2 四元数和旋转矩阵

对于单位四元数转旋转矩阵，推导如下：
$$\begin{gathered} \mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k \\ ||\mathbf{q}||=q_0^2+q_1^2+q_2^2+q_3^2=1 \\ R(\mathbf{q})=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2-2q_3{q}_0& 2q_1{q}_3+2q_2{q}_0\\ 2q_1{q}_2+2q_3{q}_0& 1-2q_1^2-2q_3^2& 2q_2{q}_3-2q_1{q}_0\\ 2q_1{q}_3-2q_2{q}_0& 2q_2{q}_3+2q_1{q}_0& 1-2q_1^2-2q_2^2\end{array}\right] \end{gathered}$$
对于旋转矩阵转四元数，推导如下：
$$\begin{gathered} tr(R(\mathbf{q}))=m_{11}+m_{22}+m_{33} \\ =3-4(q_1^2+q_2^2+q_3^2) \\ =4(1-((q_1^2+q_2^2+q_3^2)))-1 \\ =4q_0^2-1 \\ {q}_0=\frac{\sqrt{tr(R(\mathbf{q}))+1}}{2} \\ {q}_1=\frac{m_{32}-m_{23}}{4q_0} \\ {q}_2=\frac{m_{13}-m_{31}}{4q_0} \\ {q}_3=\frac{m_{21}-m_{12}}{4q_0} \end{gathered}$$

3 欧拉角和四元数

欧拉角转四元数推导如下，其中乘法要遵守四元数乘法：
$$\begin{gathered} \mathbf{q}({\theta }_3,{\theta }_2,{\theta }_1)={\mathbf{q}}_z({\theta }_3)\otimes {\mathbf{q}}_y({\theta }_2)\otimes {\mathbf{q}}_x({\theta }_1) \\ =\left[\begin{array}{c}cos(\frac{{\theta }_3}{2})\\ 0\\ 0\\ sin(\frac{{\theta }_3}{2})\end{array}\right]\otimes \left[\begin{array}{c}cos(\frac{{\theta }_2}{2})\\ 0\\ sin(\frac{{\theta }_2}{2})\\ 0\end{array}\right]\otimes \left[\begin{array}{c}cos(\frac{{\theta }_1}{2})\\ sin(\frac{{\theta }_1}{2})\\ 0\\ 0\end{array}\right] \end{gathered}$$

4 轴角

轴角转四元数如下：
$$\mathbf{q}(w,\vec{v})=(cos(\frac{\theta }{2}),\vec{u}sin(\frac{\theta }{2}))$$
轴角转旋转矩阵，即罗德里格斯公式：
$$R=cos\theta I+(1-cos\theta )\vec{u}{\vec{u}}^T+sin\theta [\vec{u}\times ]$$