三维刚体运动（一）

三维刚体运动

运动 = 旋转 + 平移（欧式变换不考虑物体的形变），本文主要介绍三维刚体运动中的旋转部分的几种表示方式。

1 旋转

1.1 旋转矩阵

旋转矩阵的性质：行列式为1的正交矩阵（逆为自身的转置）。

1. 平面2D旋转
   
   $$\begin{gathered} x=||\mathbf{OP}||cos\alpha \\ y=||\mathbf{OP}||sin\alpha \\ {x}^{\prime }=||\mathbf{OP}||cos(\alpha -\beta ) \\ {y}^{\prime }=||\mathbf{OP}||sin(\alpha -\beta ) \\ \left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{cc}cos\beta & sin\beta \\ -sin\beta & cos\beta \end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right] \end{gathered}$$
2. 空间3D旋转，分别从xyz三个轴进行旋转分解
   • 绕x轴旋转
     
     $$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\beta & sin\beta \\ 0& -sin\beta & cos\beta \end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$
   • 绕y轴旋转
     
     $$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\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}{c}x\\ y\\ z\end{array}\right]$$
   • 绕z轴旋转
     
     $$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]=\left[\begin{array}{ccc}cos\beta & sin\beta & 0\\ -sin\beta & cos\beta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$$

1.2 旋转向量

1. 旋转矩阵的缺点：有9个元素，但一次旋转只有3个自由度，表达冗余，必须是正交矩阵且行列式为1，求解困难；
2. 旋转向量简化：任意旋转都可以用一个旋转轴和一个旋转角来刻画；
3. 定义一个旋转向量/角轴，其方向与旋转轴一致，长度等于旋转角；
4. 旋转矩阵有9个量，有正交性和行列式约束；角轴有3个量，没有约束。

1.3 欧拉角

如下图绕Z-Y-X轴旋转，把一个旋转分解成3次绕不同轴的旋转，欧拉角会出现万向锁的情况，导致奇异解

1.4 四元数

1. 定义
   四元数是Hamilton找到的一种扩展的复数，一个四元数拥有一个实部和三个虚部：
   $$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$$
   其中$i,j,k$为四元数的三个虚部，同时满足以下性质：
   $$\begin{gathered} i^2=j^2=k^2=-1 \\ ij=k,ji=-k \\ jk=i,kj=-i \\ ki=j,ik=-j \end{gathered}$$
   由于它的特殊表示形式，有时会表示为一个标量和一个向量：
   $$\begin{gathered} \mathbf{q}=[s,\mathbf{v}] \\ s=q_0\in \mathbb{R},\mathbf{v}=[q_1,q_2,q_3]\in {\mathbb{R}}^3 \end{gathered}$$
   如果一个四元数虚部为0，称之为实四元数。反之，若它的实部为0，称之为虚四元数。该定义和复数是相似的。
2. 性质
   • 加减
     $${\mathbf{q}}_{\mathbf{a}}\pm {\mathbf{q}}_{\mathbf{b}}=[s_a\pm s_b,{\mathbf{v}}_{\mathbf{a}}\pm {\mathbf{v}}_{\mathbf{b}}]$$
   • 乘法
     $$\begin{gathered} {\mathbf{q}}_{\mathbf{a}}\otimes {\mathbf{q}}_{\mathbf{b}}=(s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b) \\ +(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i \\ +(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)j \\ +(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b)k \\ =[s_a{s}_b-{\mathbf{v}}_{\mathbf{a}}{\mathbf{v}}_{\mathbf{b}},s_a{\mathbf{v}}_{\mathbf{b}}+s_b{\mathbf{v}}_{\mathbf{a}}+{\mathbf{v}}_{\mathbf{a}}\times {\mathbf{v}}_{\mathbf{b}}] \end{gathered}$$
   • 模长
     四元数的模长定义为：
     $$||\mathbf{q}||=\sqrt{s^2+x^2+y^2+z^2}$$
     可以验证，两个四元数乘积的模长是模长的乘积，这使得单位四元数相乘后仍是单位四元数。
     $$||{\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}||=||{\mathbf{q}}_{\mathbf{a}}||||{\mathbf{q}}_{\mathbf{b}}||$$
   • 共轭
     $${\mathbf{q}}^{\ast }=s-xi-yj-zk=[s,-\mathbf{v}]$$
     四元数的共轭即把虚部取为相反数，四元数共轭与自身相乘会得到一个实四元数，实部为四元数模长的平方：
     $$\mathbf{q}\otimes {\mathbf{q}}^{\ast }=[s^2+{\mathbf{v}}^T\mathbf{v},0]$$
   • 逆
     $${\mathbf{q}}^{-1}=\frac{{\mathbf{q}}^{\ast }}{||\mathbf{q}||}$$
     四元数与自身的逆的乘积为实四元数的1
     $$\mathbf{q}\otimes {\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\otimes \mathbf{q}=1$$
     同时乘积的逆和矩阵运算有相似的性质：
     $$({\mathbf{q}}_{\mathbf{a}}\otimes {\mathbf{q}}_{\mathbf{b}}{)}^{-1}={{\mathbf{q}}_{\mathbf{b}}}^{-1}\otimes {{\mathbf{q}}_{\mathbf{a}}}^{-1}$$
     对于单位四元数，即$||\mathbf{q}||=1$，它的逆就是它的共轭四元数。
   • 数乘和点乘
     $$\begin{gathered} k\mathbf{q}=[ks,k\mathbf{v}] \\ {\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}=s_a{s}_b+x_a{x}_{b}i+y_a{y}_{b}j+z_a{z}_{b}k \end{gathered}$$

1.5 比较

方法	简单理解	优点	缺点	应用
旋转矩阵	$a^{\prime }=Ra$	直观	9个量表示旋转，较为冗余 约束：正交矩阵且行列式为1	机器人
旋转向量	旋转轴 + 旋转角	几何上较为直观	不实用，很难找到转轴的位置	imu
欧拉角	将一个旋转分解成绕3个不同轴的旋转	参数少，几何上较为直观	存在奇点，万向锁	航空航天，无人机，2D运动
四元数	复数的推广$p^{\prime }=\mathbf{q}p{\mathbf{q}}^{-1}$	速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等	几何上较难理解	计算机图形学、控制理论、信号处理、轨道力学等