介绍
记录并分享罗德里格斯公式(Rodrigues’ rotation formula)的推导过程。
推导过程
向量 v v v绕单位向量 k k k旋转角度 θ \theta θ，旋转后向量为 v r o t v_{rot} vrot。罗德里格斯公式描述如何了从轴角 θ k \theta k θk构造旋转矩阵 R R R，以表示 v r o t v_{rot} vrot与 v v v之间的旋转关系。
向量 v v v可以拆分为与单位向量 k k k平行的向量 v ∥ v_\parallel v∥和垂直的向量 v ⊥ v_{\perp} v⊥，表示为 v = v ∥ + v ⊥ v=v_\parallel+v_{\perp} v=v∥+v⊥向量 v ∥ v_\parallel v∥表示为 v ∥ = ( v ⋅ k ) k = k ( v ⋅ k ) = k k T v v_\parallel=(v\cdot{k}){k}=k(v\cdot{k})=kk^Tv v∥=(v⋅k)k=k(v⋅k)=kkTv定义向量 w = k × v w=k\times{v} w=k×v，且根据叉乘特性，有 ∣ w ∣ = ∣ k ∣ ∣ v ∣ sin ϕ |w|=|k||v|\sin\phi ∣w∣=∣k∣∣v∣sinϕ，其中 ϕ \phi ϕ为 v v v和 k k k的夹角，则向量 v ⊥ v_{\perp} v⊥表示为 v ⊥ = w × k = − k × w v_{\perp}=w\times{k}=-k\times{w} v⊥=w×k=−k×w且有 ∣ v ⊥ ∣ = ∣ w ∣ |v_{\perp}|=|w| ∣v⊥∣=∣w∣。令 k ∧ = [ 0 − k 3 k 2 k 3 0 − k 1 − k 2 k 1 0 ] k^\land=\begin{bmatrix}0&-k_3&k_2\\k_3&0&-k_1\\-k_2&k_1&0\end{bmatrix} k∧= 0k3−k2−k30k1k2−k10 表示 k k k的反对称矩阵，可将向量叉乘表示成矩阵乘法的形式，即有 k × v = k ∧ v k\times{v}=k^\land{v} k×v=k∧v，进而可知 w = k ∧ v w=k^\land{v} w=k∧v， v ⊥ = − k ∧ k ∧ v v_{\perp}=-k^\land k^\land{v} v⊥=−k∧k∧v。
旋转过程中向量 v ∥ v_\parallel v∥保持不变，向量 v ⊥ v_{\perp} v⊥绕单位向量 k k k旋转了 θ \theta θ角。向量 v ⊥ v_{\perp} v⊥绕单位向量 k k k旋转了 θ \theta θ角后，沿 v ⊥ v_{\perp} v⊥方向的分量表示为 v ⊥ ∣ v ⊥ ∣ ∣ v ⊥ ∣ cos θ = v ⊥ cos θ \frac{v_{\perp}}{|v_{\perp}|}|v_{\perp}|\cos\theta=v_{\perp}\cos\theta ∣v⊥∣v⊥∣v⊥∣cosθ=v⊥cosθ，沿 w w w方向的分量表示为 w ∣ w ∣ ∣ v ⊥ ∣ sin θ = w sin θ \frac{w}{|w|}|v_{\perp}|\sin\theta=w\sin\theta ∣w∣w∣v⊥∣sinθ=wsinθ。则旋转后的向量表示为 v r o t = v ∥ + v ⊥ cos θ + w sin θ v_{rot}=v_\parallel+v_{\perp}\cos\theta+w\sin\theta vrot=v∥+v⊥cosθ+wsinθ代入 v ∥ = v − v ⊥ v_\parallel=v-v_{\perp} v∥=v−v⊥得 v r o t = v − v ⊥ + v ⊥ cos θ + w sin θ = v − ( cos θ − 1 ) v ⊥ + w sin θ = [ I + ( 1 − cos θ ) k ∧ k ∧ + sin θ k ∧ ] v v_{rot}=v-v_{\perp}+v_{\perp}\cos\theta+w\sin\theta\\=v-(\cos\theta-1)v_{\perp}+w\sin\theta\\=[I+(1-\cos\theta)k^\land{k^\land}+\sin\theta k^\land]v vrot=v−v⊥+v⊥cosθ+wsinθ=v−(cosθ−1)v⊥+wsinθ=[I+(1−cosθ)k∧k∧+sinθk∧]v代入 v ⊥ = v − v ∥ v_{\perp}=v-v_\parallel v⊥=v−v∥得 v r o t = v ∥ + ( v − v ∥ ) cos θ + w sin θ = cos θ v + ( 1 − cos θ ) v ∥ + w sin θ = [ cos θ I + ( 1 − cos θ ) k k T + sin θ k ∧ ] v v_{rot}=v_\parallel+(v-v_\parallel)\cos\theta+w\sin\theta\\=\cos\theta v+(1-\cos\theta)v_\parallel+w\sin\theta\\=[\cos\theta{I}+(1-\cos\theta)kk^T+\sin\theta k^\land]v vrot=v∥+(v−v∥)cosθ+wsinθ=cosθv+(1−cosθ)v∥+wsinθ=[cosθI+(1−cosθ)kkT+sinθk∧]v最终，得到两种形式的罗德里格斯公式 R = I + ( 1 − cos θ ) k ∧ k ∧ + sin θ k ∧ R = cos θ I + ( 1 − cos θ ) k k T + sin θ k ∧ R=I+(1-\cos\theta)k^\land{k^\land}+\sin\theta k^\land\\R=\cos\theta{I}+(1-\cos\theta)kk^T+\sin\theta k^\land R=I+(1−cosθ)k∧k∧+sinθk∧R=cosθI+(1−cosθ)kkT+sinθk∧（ k k k为单位向量时，有以下等式成立， k ∧ k ∧ = [ − k 2 2 − k 3 2 k 1 k 2 k 1 k 3 k 1 k 2 − k 1 2 − k 2 3 k 2 k 3 k 1 k 3 k 2 k 3 − k 1 2 − k 2 2 ] = k k T − I k^\land k^\land=\begin{bmatrix}-k_2^2-k_3^2&k_1k_2&k_1k_3\\k_1k_2&-k_1^2-k^3_2&k_2k_3\\k_1k_3&k_2k_3&-k_1^2-k_2^2\end{bmatrix}=kk^T-I k∧k∧= −k22−k32k1k2k1k3k1k2−k12−k23k2k3k1k3k2k3−k12−k22 =kkT−I，带入上述两个公式可以发现二者是等价的。）
