罗德里格斯旋转公式（Rodrigues‘ Rotation Formula）完整推导

罗德里格斯旋转公式 Rodrigues' Rotation Formula 完整推导

• [Bilibili 同步视频](#Bilibili 同步视频)
• [1. 设定与向量正交分解](#1. 设定与向量正交分解)
• [2. 构造旋转平面内的正交基](#2. 构造旋转平面内的正交基)
• [3. 对垂直分量执行平面旋转](#3. 对垂直分量执行平面旋转)
• [4. 合并分量并整理公式](#4. 合并分量并整理公式)
• [5. 矩阵形式（工程常用）](#5. 矩阵形式（工程常用）)
• 补充推导：从旋转矩阵到四元数
• [实例验证：绕 Z 轴旋转 90°](#实例验证：绕 Z 轴旋转 90°)

Bilibili 同步视频

[简化版 GAMES 101] 计算机图形学 加更：罗德里格斯旋转公式

罗德里格斯公式是三维空间刚体运动学的核心，它巧妙地利用向量叉乘与点乘 ，将"绕任意单位向量 u \boldsymbol{u} u 旋转 θ \theta θ 角"的复杂空间操作降维求解。其最终的向量表达式为：

v ′ = cos ⁡ θ ⋅ v + ( 1 − cos ⁡ θ ) ( u ⋅ v ) u + sin ⁡ θ ⋅ ( u × v ) \boldsymbol{v}' = \cos\theta \cdot \boldsymbol{v} + (1-\cos\theta)(\boldsymbol{u}\cdot\boldsymbol{v})\boldsymbol{u} + \sin\theta \cdot (\boldsymbol{u}\times\boldsymbol{v}) v′=cosθ⋅v+(1−cosθ)(u⋅v)u+sinθ⋅(u×v)

1. 设定与向量正交分解

设待旋转向量为 v \boldsymbol{v} v，旋转轴为单位向量 u \boldsymbol{u} u，旋转角为 θ \theta θ（遵循右手定则）。

为了简化计算，我们将 v \boldsymbol{v} v 沿着旋转轴 u \boldsymbol{u} u 进行正交分解：

v = v ∥ + v ⊥ \boldsymbol{v} = \boldsymbol{v}\parallel + \boldsymbol{v}\perp v=v∥+v⊥

• 平行分量 v ∥ = ( u ⋅ v ) u \boldsymbol{v}_\parallel = (\boldsymbol{u}\cdot\boldsymbol{v})\boldsymbol{u} v∥=(u⋅v)u：该分量与旋转轴平行，旋转过程中保持不变。
• 垂直分量 v ⊥ = v − v ∥ \boldsymbol{v}\perp = \boldsymbol{v}-\boldsymbol{v}\parallel v⊥=v−v∥：该分量垂直于旋转轴，旋转时将在垂直平面内绕轴转动。

2. 构造旋转平面内的正交基

在垂直于 u \boldsymbol{u} u 的平面内，我们可以选取一组正交基来描述 v ⊥ \boldsymbol{v}_\perp v⊥ 的旋转轨迹：

e 1 = v ⊥ , e 2 = u × v ⊥ \boldsymbol{e}1 = \boldsymbol{v}\perp, \quad \boldsymbol{e}2 = \boldsymbol{u}\times\boldsymbol{v}\perp e1=v⊥,e2=u×v⊥

根据叉乘的右手定则与正交性质，容易验证以下关系：
u × e 1 = e 2 , u × e 2 = − e 1 \boldsymbol{u}\times\boldsymbol{e}_1 = \boldsymbol{e}_2, \quad \boldsymbol{u}\times\boldsymbol{e}_2 = -\boldsymbol{e}_1 u×e1=e2,u×e2=−e1

3. 对垂直分量执行平面旋转

垂直分量 v ⊥ \boldsymbol{v}\perp v⊥ 在平面内旋转 θ \theta θ 角后的新向量 v ⊥ ′ \boldsymbol{v}\perp' v⊥′ 可以表示为：

v ⊥ ′ = cos ⁡ θ ⋅ e 1 + sin ⁡ θ ⋅ e 2 = cos ⁡ θ ⋅ v ⊥ + sin ⁡ θ ⋅ ( u × v ⊥ ) \boldsymbol{v}_\perp' = \cos\theta \cdot \boldsymbol{e}1 + \sin\theta \cdot \boldsymbol{e}2 = \cos\theta \cdot \boldsymbol{v}\perp + \sin\theta \cdot (\boldsymbol{u}\times\boldsymbol{v}\perp) v⊥′=cosθ⋅e1+sinθ⋅e2=cosθ⋅v⊥+sinθ⋅(u×v⊥)

由于 u \boldsymbol{u} u 与平行分量 v ∥ \boldsymbol{v}\parallel v∥ 平行，其叉乘为零向量（ u × v ∥ = 0 \boldsymbol{u}\times\boldsymbol{v}\parallel=0 u×v∥=0），因此我们可以将叉乘项简化：
u × v ⊥ = u × ( v − v ∥ ) = u × v \boldsymbol{u}\times\boldsymbol{v}\perp = \boldsymbol{u}\times(\boldsymbol{v} - \boldsymbol{v}\parallel) = \boldsymbol{u}\times\boldsymbol{v} u×v⊥=u×(v−v∥)=u×v

4. 合并分量并整理公式

旋转后的最终向量 v ′ \boldsymbol{v}' v′ 等于"不变的平行分量"加上"旋转后的垂直分量"：

v ′ = v ∥ + v ⊥ ′ \boldsymbol{v}' = \boldsymbol{v}\parallel + \boldsymbol{v}\perp' v′=v∥+v⊥′

将前述推导代入：
v ′ = ( u ⋅ v ) u + cos ⁡ θ ( v − ( u ⋅ v ) u ) + sin ⁡ θ ( u × v ) \boldsymbol{v}' = (\boldsymbol{u}\cdot\boldsymbol{v})\boldsymbol{u} + \cos\theta(\boldsymbol{v}-(\boldsymbol{u}\cdot\boldsymbol{v})\boldsymbol{u}) + \sin\theta(\boldsymbol{u}\times\boldsymbol{v}) v′=(u⋅v)u+cosθ(v−(u⋅v)u)+sinθ(u×v)

合并同类项，即可得到罗德里格斯旋转公式 的向量形式：
v ′ = cos ⁡ θ ⋅ v + ( 1 − cos ⁡ θ ) ( u ⋅ v ) u + sin ⁡ θ ⋅ ( u × v ) \boldsymbol{v}' = \cos\theta \cdot \boldsymbol{v} + (1-\cos\theta)(\boldsymbol{u}\cdot\boldsymbol{v})\boldsymbol{u} + \sin\theta \cdot (\boldsymbol{u}\times\boldsymbol{v}) v′=cosθ⋅v+(1−cosθ)(u⋅v)u+sinθ⋅(u×v)

5. 矩阵形式（工程常用）

在计算机图形学和机器人学中，为了便于批量计算，常将叉乘转换为反对称矩阵运算。定义向量 u = [ u x , u y , u z ] T \boldsymbol{u}=[u_x, u_y, u_z]^T u=[ux,uy,uz]T 的反对称矩阵 [ u ] × [\boldsymbol{u}]_\times [u]× 为：

u \] × = \[ 0 − u z u y u z 0 − u x − u y u x 0 \] \[\\boldsymbol{u}\]_\\times = \\begin{bmatrix} 0 \& -u_z \& u_y \\\\ u_z \& 0 \& -u_x \\\\ -u_y \& u_x \& 0 \\end{bmatrix} \[u\]×= 0uz−uy−uz0uxuy−ux0 利用关系式 ( u ⋅ v ) u = u u T v (\\boldsymbol{u}\\cdot\\boldsymbol{v})\\boldsymbol{u} = \\boldsymbol{u}\\boldsymbol{u}\^T\\boldsymbol{v} (u⋅v)u=uuTv 和 u × v = \[ u \] × v \\boldsymbol{u}\\times\\boldsymbol{v} = \[\\boldsymbol{u}\]_\\times\\boldsymbol{v} u×v=\[u\]×v，可推导出对应的旋转矩阵 R R R： R = I + sin ⁡ θ ⋅ \[ u \] × + ( 1 − cos ⁡ θ ) ⋅ \[ u \] × 2 R = I + \\sin\\theta \\cdot \[\\boldsymbol{u}\]_\\times + (1-\\cos\\theta) \\cdot \[\\boldsymbol{u}\]_\\times\^2 R=I+sinθ⋅\[u\]×+(1−cosθ)⋅\[u\]×2 *** ** * ** *** ## 补充推导：从旋转矩阵到四元数 四元数 q = \[ w , x , y , z \] q = \[w, x, y, z\] q=\[w,x,y,z\]（ w w w 为实部）在姿态解算中能完美避开"万向节死锁"。假设已知旋转矩阵 R R R，我们可以通过矩阵的迹（Trace）反推四元数。 单位四元数对应的旋转矩阵 R R R 为： R = \[ 1 − 2 y 2 − 2 z 2 2 x y − 2 w z 2 x z + 2 w y 2 x y + 2 w z 1 − 2 x 2 − 2 z 2 2 y z − 2 w x 2 x z − 2 w y 2 y z + 2 w x 1 − 2 x 2 − 2 y 2 \] R = \\begin{bmatrix} 1-2y\^2-2z\^2 \& 2xy-2wz \& 2xz+2wy \\\\ 2xy+2wz \& 1-2x\^2-2z\^2 \& 2yz-2wx \\\\ 2xz-2wy \& 2yz+2wx \& 1-2x\^2-2y\^2 \\end{bmatrix} R= 1−2y2−2z22xy+2wz2xz−2wy2xy−2wz1−2x2−2z22yz+2wx2xz+2wy2yz−2wx1−2x2−2y2 **1. 求实部 w w w：** 计算矩阵的迹 tr ( R ) = R 11 + R 22 + R 33 \\text{tr}(R) = R_{11} + R_{22} + R_{33} tr(R)=R11+R22+R33： tr ( R ) = 3 − 4 ( x 2 + y 2 + z 2 ) \\text{tr}(R) = 3 - 4(x\^2 + y\^2 + z\^2) tr(R)=3−4(x2+y2+z2) 利用单位四元数性质 x 2 + y 2 + z 2 = 1 − w 2 x\^2 + y\^2 + z\^2 = 1 - w\^2 x2+y2+z2=1−w2，代入得： tr ( R ) = 3 − 4 ( 1 − w 2 ) = 4 w 2 − 1 \\text{tr}(R) = 3 - 4(1 - w\^2) = 4w\^2 - 1 tr(R)=3−4(1−w2)=4w2−1 解得实部： w = tr ( R ) + 1 2 w = \\frac{\\sqrt{\\text{tr}(R) + 1}}{2} w=2tr(R)+1 **2. 求虚部 x , y , z x, y, z x,y,z：** 通过矩阵非对角线元素的差值求解。例如计算 R 32 − R 23 R_{32} - R_{23} R32−R23： R 32 − R 23 = ( 2 y z + 2 w x ) − ( 2 y z − 2 w x ) = 4 w x    ⟹    x = R 32 − R 23 4 w R_{32} - R_{23} = (2yz+2wx) - (2yz-2wx) = 4wx \\implies x = \\frac{R_{32} - R_{23}}{4w} R32−R23=(2yz+2wx)−(2yz−2wx)=4wx⟹x=4wR32−R23 同理可得： y = R 13 − R 31 4 w , z = R 21 − R 12 4 w y = \\frac{R_{13} - R_{31}}{4w}, \\quad z = \\frac{R_{21} - R_{12}}{4w} y=4wR13−R31,z=4wR21−R12 *(注：在实际代码实现中，为防止 w → 0 w \\to 0 w→0 导致除零溢出，通常会根据 R R R 对角线元素的大小，选择数值最稳定的路径进行计算。)* *** ** * ** *** ## 实例验证：绕 Z 轴旋转 90° 我们用罗德里格斯公式验证一个经典场景：将 X 轴上的向量 v = \[ 1 , 0 , 0 \] T \\boldsymbol{v}=\[1, 0, 0\]\^T v=\[1,0,0\]T 绕 Z 轴旋转 90°。 * **输入条件** ： v = \[ 1 , 0 , 0 \] T \\boldsymbol{v} = \[1, 0, 0\]\^T v=\[1,0,0\]T，旋转轴 u = \[ 0 , 0 , 1 \] T \\boldsymbol{u} = \[0, 0, 1\]\^T u=\[0,0,1\]T，角度 θ = 90 ∘ \\theta = 90\^\\circ θ=90∘（即 cos ⁡ θ = 0 , sin ⁡ θ = 1 \\cos\\theta=0, \\sin\\theta=1 cosθ=0,sinθ=1）。 * **代入公式** ： v ′ = 0 ⋅ v + ( 1 − 0 ) ( u ⋅ v ) u + 1 ⋅ ( u × v ) \\boldsymbol{v}' = 0 \\cdot \\boldsymbol{v} + (1-0)(\\boldsymbol{u}\\cdot\\boldsymbol{v})\\boldsymbol{u} + 1 \\cdot (\\boldsymbol{u}\\times\\boldsymbol{v}) v′=0⋅v+(1−0)(u⋅v)u+1⋅(u×v) * **分步计算**： 1. **点积项** ： u ⋅ v = 0 \\boldsymbol{u}\\cdot\\boldsymbol{v} = 0 u⋅v=0（X轴与Z轴垂直），故第二项为 0 \\boldsymbol{0} 0。 2. **叉乘项** ： u × v = \[ 0 , 0 , 1 \] T × \[ 1 , 0 , 0 \] T = \[ 0 , 1 , 0 \] T \\boldsymbol{u}\\times\\boldsymbol{v} = \[0, 0, 1\]\^T \\times \[1, 0, 0\]\^T = \[0, 1, 0\]\^T u×v=\[0,0,1\]T×\[1,0,0\]T=\[0,1,0\]T（根据右手定则指向Y轴）。 * **最终结果** ： v ′ = \[ 0 , 0 , 0 \] T + \[ 0 , 0 , 0 \] T + \[ 0 , 1 , 0 \] T = \[ 0 , 1 , 0 \] T \\boldsymbol{v}' = \[0, 0, 0\]\^T + \[0, 0, 0\]\^T + \[0, 1, 0\]\^T = \[0, 1, 0\]\^T v′=\[0,0,0\]T+\[0,0,0\]T+\[0,1,0\]T=\[0,1,0\]T  **结论** ：向量 \[ 1 , 0 , 0 \] \[1, 0, 0\] \[1,0,0\] 绕 Z 轴旋转 90° 后变为 \[ 0 , 1 , 0 \] \[0, 1, 0\] \[0,1,0\]，与理论预期完全一致，验证了公式的正确性。