视觉SLAM十四讲-(3.15)罗德里格斯公式推导

视觉SLAM十四讲-(3.15)罗德里格斯公式推导

罗德里格斯公式详细推导过程

罗德里格斯公式（Rodrigues’ Rotation Formula）用于将旋转向量（轴角表示）转换为旋转矩阵。其核心推导步骤如下：

1. 旋转向量的定义

• 旋转由旋转轴（单位向量 ( \mathbf{k} = [k_x, k_y, k_z]^TKaTeX parse error: Can't use function '\(' in math mode at position 7: ）和旋转角 \̲(̲ \theta描述。
• 旋转向量表示为 ( \boldsymbol{\theta} = \theta \mathbf{k}KaTeX parse error: Can't use function '\(' in math mode at position 5: ，其中 \̲(̲ \|\mathbf{k}\|…。

2. 向量分解

任意向量 ( \mathbf{v}KaTeX parse error: Can't use function '\(' in math mode at position 15: 可分解为平行于旋转轴的分量 \̲(̲ \mathbf{v}_{\p…和垂直于旋转轴的分量 ( \mathbf{v}_{\perp}$：

• 平行分量：( \mathbf{v}_{\parallel} = (\mathbf{v} \cdot \mathbf{k}) \mathbf{k}$
• 垂直分量：( \mathbf{v}{\perp} = \mathbf{v} - \mathbf{v}{\parallel} = \mathbf{v} - (\mathbf{v} \cdot \mathbf{k}) \mathbf{k}$

3. 旋转后的向量分量

旋转后，平行分量 ( \mathbf{v}_{\parallel}KaTeX parse error: Can't use function '\(' in math mode at position 11: 保持不变，垂直分量 \̲(̲ \mathbf{v}_{\p…绕轴旋转 ( \theta$角：

• 平行分量旋转：( \mathbf{v}{\parallel \text{rot}} = \mathbf{v}{\parallel}$
• 垂直分量旋转：( \mathbf{v}{\perp \text{rot}} = \cos \theta \mathbf{v}{\perp} + \sin \theta (\mathbf{k} \times \mathbf{v})$

4. 合并旋转向量表达式

旋转后的向量 ( \mathbf{v}_{\text{rot}}$为：
${\mathbf{v}}_{\text{rot}}={\mathbf{v}}_{\parallel \text{rot}}+{\mathbf{v}}_{\perp \text{rot}}=(\mathbf{v}\cdot \mathbf{k})\mathbf{k}+\cos \theta (\mathbf{v}-(\mathbf{v}\cdot \mathbf{k})\mathbf{k})+\sin \theta (\mathbf{k}\times \mathbf{v})$

5. 矩阵形式转换

将点积和叉积转换为矩阵运算：

• 点积项：( (\mathbf{v} \cdot \mathbf{k}) \mathbf{k} = \mathbf{k} \mathbf{k}^T \mathbf{v}$
• 叉积项：( \mathbf{k} \times \mathbf{v} = \mathbf{K} \mathbf{v}KaTeX parse error: Can't use function '\(' in math mode at position 5: ，其中 \̲(̲ \mathbf{K}是 ( \mathbf{k}$的反对称矩阵：
  $\mathbf{K}=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]$

代入后，表达式变为：
${\mathbf{v}}_{\text{rot}}=\cos \theta \mathbf{v}+(1-\cos \theta )\mathbf{k}{\mathbf{k}}^T\mathbf{v}+\sin \theta \mathbf{Kv}$

6. 旋转矩阵的推导

提取公共矩阵项，得到旋转矩阵 ( \mathbf{R}$：
${\mathbf{v}}_{\text{rot}}=\left[\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{k}{\mathbf{k}}^T+\sin \theta \mathbf{K}\right]\mathbf{v}$
因此，旋转矩阵 ( \mathbf{R}$为：
$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{k}{\mathbf{k}}^T+\sin \theta \mathbf{K}$

7. 公式总结

罗德里格斯公式的最终形式为：
$\mathbf{R}=\mathbf{I}+\sin \theta \mathbf{K}+(1-\cos \theta ){\mathbf{K}}^2$
其中 ( \mathbf{K}^2$是反对称矩阵的平方，可展开为：
${\mathbf{K}}^2=\left[\begin{array}{ccc}-k_y^2-k_z^2& k_x{k}_y& k_x{k}_z\\ k_x{k}_y& -k_x^2-k_z^2& k_y{k}_z\\ k_x{k}_z& k_y{k}_z& -k_x^2-k_y^2\end{array}\right]$

几何意义与验证

• 平行分量：保持原方向，对应公式中的 ( (\mathbf{v} \cdot \mathbf{k}) \mathbf{k}$。
• 垂直分量：在垂直平面内旋转 ( \thetaKaTeX parse error: Can't use function '\(' in math mode at position 5: 角，由 \̲(̲ \cos \theta \m…和 ( \sin \theta (\mathbf{k} \times \mathbf{v})$组合。
• 等号条件：当 ( \theta = 0KaTeX parse error: Can't use function '\(' in math mode at position 3: 时，\̲(̲ \mathbf{R} = \…；当 ( \theta = 2\pi$时，旋转一周回到原位。

应用场景

• 计算机视觉：相机位姿估计、SLAM中的旋转表示。
• 机器人学：机械臂运动规划、姿态控制。
• 航空航天：飞行器姿态动力学、导航系统。

罗德里格斯公式通过简洁的矩阵运算实现了旋转向量的高效转换，是三维旋转表示的核心工具之一。