罗德里格斯公式推导)
视觉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−kz0kxky−kx0
代入后，表达式变为：
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−kz2kxkykxkzkxky−kx2−kz2kykzkxkzkykz−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中的旋转表示。
- 机器人学：机械臂运动规划、姿态控制。
- 航空航天：飞行器姿态动力学、导航系统。
罗德里格斯公式通过简洁的矩阵运算实现了旋转向量的高效转换，是三维旋转表示的核心工具之一。
