旋转表示、转换及更新
本文内容均默认使用右手定则，且旋转向量在局部坐标系中表达（即右乘）
- 代码实现
1. 旋转表示
1.1 旋转向量
- 用\(\boldsymbol{\phi} = \phi \boldsymbol{n}\)表示绕着单位轴\(\boldsymbol{n}\)旋转\(\phi\)弧度，用\(\boldsymbol{\omega}\)表示旋转角速率
1.2 旋转矩阵
旋转行为： \[ \boldsymbol{x}^{'} = \boldsymbol{R}\boldsymbol{x} \tag{1} \]
约束： \[ \boldsymbol{R}\boldsymbol{R}^\mathrm{T} = \boldsymbol{R}^\mathrm{T}\boldsymbol{R} = \boldsymbol{I}, \quad det(\boldsymbol{R}) = 1 \tag{2} \]
微分： \[ \dot{\boldsymbol{R}}=\boldsymbol{R}\boldsymbol{\omega}^{\wedge} \tag{3} \]
1.3 四元数
表示： \[ \boldsymbol{q}=[s, \boldsymbol{v}]^{\mathrm{T}}, \quad s=q_w \in \mathbb{R}, \quad \boldsymbol{v}=\left[q_x, q_y, q_z\right]^{\mathrm{T}} \in \mathbb{R}^3 \tag{4} \]
只有单位四元数才能表示旋转
四元数乘法： \[ \mathbf{q}_a \otimes \mathbf{q}_b =\left[\begin{array}{c} s_a s_b-\mathbf{v}_a^{\top} \mathbf{v}_b \\ s_a \mathbf{v}_b+s_b \mathbf{v}_a+\mathbf{v}_a \times \mathbf{v}_b \end{array}\right] \tag{5} \]
!!!四元数乘法不满足乘法结合律
旋转行为： \[ \boldsymbol{x}^{'} = \boldsymbol{q} \otimes \boldsymbol{x} \otimes \boldsymbol{q}^* \tag{6} \]
约束： \[ \mathbf{q} \otimes \mathbf{q}^* = \mathbf{q}^* \otimes \mathbf{q} =1 \tag{7} \]
微分： \[ \dot{\mathbf{q}}=\frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega} \tag{8} \]
2. 旋转转换
2.1 旋转向量与旋转矩阵
2.1.1 从旋转向量转换到旋转矩阵的转换
\[ \begin{aligned} \boldsymbol{R} &=\exp \left(\boldsymbol{\phi}^{\wedge}\right) = \sum_{n=0}^{\infty} \frac{1}{n!}\left(\boldsymbol{\phi}^{\wedge}\right)^n \approx \boldsymbol{I}+\boldsymbol{\phi}^{\wedge} \\ &= \exp \left(\phi \boldsymbol{n}^{\wedge}\right) = \cos \phi \boldsymbol{I}+(1-\cos \phi)\boldsymbol{n}\boldsymbol{n}^{\mathrm{T}}+\sin \phi \boldsymbol{n}^{\wedge} \end{aligned} \tag{9} \]
式(9)的第二行即罗德里格斯公式
2.1.2 从旋转矩阵转换到旋转向量的转换
\[ \begin{aligned} \phi & =\arccos \frac{\operatorname{tr}(\boldsymbol{R})-1}{2} \\ \boldsymbol{n} & =\frac{\left(\boldsymbol{R}-\boldsymbol{R}^{\top}\right)^{\vee}}{2 \sin \phi} \end{aligned} \tag{10} \]
2.2 旋转向量与四元数
2.2.1 从旋转向量到四元数的转换
将\(\boldsymbol{\phi}\)转换为纯四元数（pure quaternion），则有 \[ \boldsymbol{q} = exp(\frac{\boldsymbol{\phi}}{2}) = \left[\begin{array}{c} \cos \frac{\phi}{2} \\ \boldsymbol{n} \sin \frac{\phi}{2} \end{array}\right] \approx \left[\begin{array}{c} 1 \\ \frac{1}{2}\boldsymbol{\phi} \end{array}\right] \tag{11} \]
上式中存在\(\boldsymbol{\phi}\)到纯四元数（pure quaternion）的隐含转换
2.2.2 从四元数到旋转向量的转换
\[ \begin{aligned} \phi & =2 \arctan \left(||\boldsymbol{v}||, s \right) \\ \boldsymbol{n} & =\frac{\boldsymbol{v}}{ ||\boldsymbol{v}||} \end{aligned} \tag{12} \]
2.3 旋转矩阵与四元数
2.3.1 从旋转矩阵到四元数的转换
\[ \left\{ \begin{aligned} q_w = sign(q_w) \frac{1}{2} \sqrt{1+r_{11}+r_{22} + r_{33}} \\ q_x = sign(q_x) \frac{1}{2} \sqrt{1+r_{11}-r_{22} - r_{33}} \\ q_y = sign(q_y) \frac{1}{2} \sqrt{1-r_{11}+r_{22} - r_{33}} \\ q_z = sign(q_z) \frac{1}{2} \sqrt{1-r_{11}-r_{22} + r_{33}} \\ \end{aligned} \right. \tag{13} \]
2.3.2 从四元数到旋转矩阵的转换
\[ \boldsymbol{R}=\left(s^2-\boldsymbol{v}^{\mathrm{T}} \boldsymbol{v}\right) \boldsymbol{I}+2 \boldsymbol{v} \boldsymbol{v}^{\mathrm{T}}+2 s \boldsymbol{v}^{\wedge} \tag{14} \]
3. 旋转更新
- 在优化姿态时，通常计算一个旋转向量增量\(\delta \boldsymbol{\phi}\)，再用它更新当前的估计值\(\mathbf{R} \leftarrow \mathbf{R} \exp \left(\boldsymbol{\delta \boldsymbol{\phi}}^{\wedge}\right)\)或\(\mathbf{q} \leftarrow \mathbf{q} \otimes exp(\frac{\boldsymbol{\delta\phi}}{2})\)
参考文献
- 《视觉SLAM十四讲》
- Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.
- 《机器人学中的状态估计》
- 《多旋翼飞行器设计与控制》
- 深蓝学院《从零手写VIO》课程