罗德里格斯旋转公式（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$ ，与理论预期完全一致，验证了公式的正确性。