视觉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}}$为：
vrot=v∥rot+v⊥rot=(v⋅k)k+cos⁡θ(v−(v⋅k)k)+sin⁡θ(k×v)\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})vrot​=v∥rot​+v⊥rot​=(v⋅k)k+cosθ(v−(v⋅k)k)+sinθ(k×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}$的反对称矩阵：
  K=[0−kzkykz0−kx−kykx0]\mathbf{K} = \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix}K=​0kz​−ky​​−kz​0kx​​ky​−kx​0​​

代入后，表达式变为：
vrot=cos⁡θv+(1−cos⁡θ)kkTv+sin⁡θKv\mathbf{v}_{\text{rot}} = \cos \theta \mathbf{v} + (1 - \cos \theta) \mathbf{k} \mathbf{k}^T \mathbf{v} + \sin \theta \mathbf{K} \mathbf{v}vrot​=cosθv+(1−cosθ)kkTv+sinθKv

6. 旋转矩阵的推导

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

7. 公式总结

罗德里格斯公式的最终形式为：
R=I+sin⁡θK+(1−cos⁡θ)K2\mathbf{R} = \mathbf{I} + \sin \theta \mathbf{K} + (1 - \cos \theta) \mathbf{K}^2R=I+sinθK+(1−cosθ)K2
其中 ( \mathbf{K}^2$是反对称矩阵的平方，可展开为：
K2=[−ky2−kz2kxkykxkzkxky−kx2−kz2kykzkxkzkykz−kx2−ky2]\mathbf{K}^2 = \begin{bmatrix} -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{bmatrix}K2=​−ky2​−kz2​kx​ky​kx​kz​​kx​ky​−kx2​−kz2​ky​kz​​kx​kz​ky​kz​−kx2​−ky2​​​

几何意义与验证

• 平行分量：保持原方向，对应公式中的 ( (\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中的旋转表示。
• 机器人学：机械臂运动规划、姿态控制。
• 航空航天：飞行器姿态动力学、导航系统。

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