罗德里格斯公式(Rodrigues’ rotation formula)推导

向量形式

如图所示，三维空间中的一个矢量 v \bold v v绕轴 k \bold k k旋转 θ \theta θ角度后得到 v r o t \bold v_{rot} vrot。将 v \bold v v沿 k \bold k k和垂直 k \bold k k的方向分解，有：
v = v ∥ + v ⊥ \bold v= \bold v_{\parallel}+ \bold v_{\perp} v=v∥+v⊥
而 v ∥ \bold v_{\parallel} v∥和 v ⊥ \bold v_{\perp} v⊥又可以表示为：
v ∥ = ( v ⋅ k ) k = ∣ v ∣ ∣ k ∣ c o s < v , k > k v ⊥ = v − v ∥ = v − ( k ⋅ v ) k = − k × ( k × v ) \\\bold v_{\parallel}=(\bold v\cdot \bold k)\bold k=|v||k|cos<v,k> \bold k \\\bold v_{\perp}=\bold v - \bold v_{\parallel}=\bold v-(\bold k \cdot \bold v)\bold k=-\bold k\times(\bold k \times \bold v) v∥=(v⋅k)k=∣v∣∣k∣cos<v,k>kv⊥=v−v∥=v−(k⋅v)k=−k×(k×v)
由于 v \bold v v平行于旋转轴 k \bold k k的分量 v ∥ \bold v_{\parallel} v∥的大小和方向不会因旋转而改变，因而有：
v ∥ = v ∥ r o t \bold v_{\parallel}=\bold v_{\parallel rot} v∥=v∥rot
而垂直于旋转轴的分量大小不变，方向改变，如图所示：

∣ v ⊥ r o t ∣ = ∣ v ⊥ ∣ v ⊥ r o t = c o s ( θ ) v ⊥ + s i n ( θ ) k × v ⊥ = c o s ( θ ) v ⊥ + s i n ( θ ) k × v |\bold v_{\perp rot}|=|\bold v_{\perp}| \\ \begin{aligned} \bold v_{\perp rot} &=cos(\theta)\bold v_{\perp}+sin(\theta)\bold k\times \bold v_{\perp} \\ &=cos(\theta)\bold v_{\perp}+sin(\theta)\bold k\times\bold v \end{aligned} ∣v⊥rot∣=∣v⊥∣v⊥rot=cos(θ)v⊥+sin(θ)k×v⊥=cos(θ)v⊥+sin(θ)k×v
那么，旋转后的向量 v r o t \bold v_{rot} vrot为：
v r o t = v ∥ r o t + v ⊥ r o t = v ∥ + c o s ( θ ) v ⊥ + s i n ( θ ) k × v = ( v ⋅ k ) k + c o s ( θ ) ( v − ( k ⋅ v ) k ) + s i n ( θ ) k × v = c o s ( θ ) v + ( 1 − c o s ( θ ) ) ( k ⋅ v ) k + s i n ( θ ) k × v \begin{aligned} \bold v_{rot} &= \bold v_{\parallel rot}+ \bold v_{\perp rot} \\ &=\bold v_{\parallel}+cos(\theta)\bold v_{\perp}+sin(\theta)\bold k\times \bold v\\ &=(\bold v\cdot \bold k)\bold k+cos(\theta)(\bold v-(\bold k \cdot \bold v)\bold k)+sin(\theta)\bold k \times \bold v\\ &=cos(\theta)\bold v+(1-cos(\theta))(\bold k \cdot \bold v)\bold k+sin(\theta)\bold k \times \bold v \end{aligned} vrot=v∥rot+v⊥rot=v∥+cos(θ)v⊥+sin(θ)k×v=(v⋅k)k+cos(θ)(v−(k⋅v)k)+sin(θ)k×v=cos(θ)v+(1−cos(θ))(k⋅v)k+sin(θ)k×v
这个就是罗德里格斯公式的向量形式。

矩阵形式

已知罗德里格斯公式的向量形式为：
v r o t = c o s ( θ ) v + ( 1 − c o s ( θ ) ) ( k ⋅ v ) k + s i n ( θ ) k × v \bold v_{rot} = cos(\theta)\bold v+(1-cos(\theta))(\bold k \cdot \bold v)\bold k+sin(\theta)\bold k \times \bold v vrot=cos(θ)v+(1−cos(θ))(k⋅v)k+sin(θ)k×v
把 v \bold v v, k \bold k k写作列向量为：
k = [ k x k y k z ] , v = [ v x v y v z ] \bold k = \begin{bmatrix} k_x\\ k_y\\ k_z \end{bmatrix}, \bold v = \begin{bmatrix} v_x\\ v_y\\ v_z \end{bmatrix} k=⎣⎡kxkykz⎦⎤,v=⎣⎡vxvyvz⎦⎤
则：
k × v = [ ( k × v ) x ( k × v ) y ( k × v ) z ] = [ 0 − k z k y k z 0 − k x − k y k x 0 ] [ v x v y v z ] = K v , \bold k\times \bold v=\begin{bmatrix} (\bold k\times \bold v)_x\\ (\bold k\times \bold v)_y\\ (\bold k\times \bold v)_z \end{bmatrix}=\begin{bmatrix} 0 & -k_z & k_y\\ k_z & 0 &-k_x\\ -k_y & k_x & 0 \end{bmatrix}\begin{bmatrix} v_x\\ v_y\\ v_z \end{bmatrix}=\bold K\bold v, k×v=⎣⎡(k×v)x(k×v)y(k×v)z⎦⎤=⎣⎡0kz−ky−kz0kxky−kx0⎦⎤⎣⎡vxvyvz⎦⎤=Kv,
k × ( k × v ) = K ( K v ) = K 2 v \bold k \times (\bold k \times \bold v)=\bold K(\bold K\bold v)=\bold{K}^2\bold v k×(k×v)=K(Kv)=K2v
那么，旋转后的向量 v r o t \bold v_{rot} vrot可以表示为：
v r o t = c o s ( θ ) v + ( 1 − c o s ( θ ) ) ( k ⋅ v ) k + s i n ( θ ) k × v = c o s ( θ ) v + ( 1 − c o s ( θ ) ) ( v − v ∥ ) + s i n ( θ ) K v = c o s ( θ ) v + ( 1 − c o s ( θ ) ) ( v + k × ( k × v ) + s i n ( θ ) K v = c o s ( θ ) v + ( 1 − c o s ( θ ) ) ( v + K 2 v ) + s i n ( θ ) K v = ( I + ( 1 − c o s ( θ ) ) K 2 + s i n ( θ ) K ) v \begin{aligned} \bold v_{rot} &= cos(\theta)\bold v+(1-cos(\theta))(\bold k \cdot \bold v)\bold k+sin(\theta)\bold k \times \bold v \\ &=cos(\theta)\bold v+(1-cos(\theta))(\bold v-\bold v_{\parallel})+sin(\theta)\bold K \bold v\\ &=cos(\theta)\bold v+(1-cos(\theta))(\bold v+\bold k\times(\bold k \times \bold v)+sin(\theta)\bold K \bold v\\ &=cos(\theta)\bold v+(1-cos(\theta))(\bold v+\bold{K}^2\bold v)+sin(\theta)\bold K \bold v\\ &=(\bold I+(1-cos(\theta))\bold{K}^2+sin(\theta)\bold K)\bold v \end{aligned} vrot=cos(θ)v+(1−cos(θ))(k⋅v)k+sin(θ)k×v=cos(θ)v+(1−cos(θ))(v−v∥)+sin(θ)Kv=cos(θ)v+(1−cos(θ))(v+k×(k×v)+sin(θ)Kv=cos(θ)v+(1−cos(θ))(v+K2v)+sin(θ)Kv=(I+(1−cos(θ))K2+sin(θ)K)v
即：
v r o t = R v , R = ( I + ( 1 − c o s ( θ ) ) K 2 + s i n ( θ ) K ) \bold v_{rot}=\bold R\bold v , \ \ \bold R =(\bold I+(1-cos(\theta))\bold{K}^2+sin(\theta)\bold K) vrot=Rv, R=(I+(1−cos(θ))K2+sin(θ)K)
这个就是罗德里格斯公式的矩阵形式。