罗德里格斯（Rodrigues）旋转公式推导

た入场券 2023-07-06 04:58 19阅读 0赞

# 定义 #

> 定义 v \\pmb\{v\} vvv是三维空间 R 3 \\R^3 R3中的一个任意向量， k \\pmb\{k\} kkk是一个单位向量，向量 v \\pmb\{v\} vvv绕向量 k \\pmb\{k\} kkk旋转 θ \\theta θ角度，则表示旋转后向量 v r o t \\pmb\{v\}\_\{rot\} vvvrot的罗德里格斯公式为：  
>  v r o t = v cos ⁡ θ + ( k × v ) sin ⁡ θ + k ( k ⋅ v ) ( 1 − cos ⁡ θ ) v\_\{rot\}=\\pmb\{v\}\\cos\\theta+(\\pmb\{k\}\\times\\pmb\{v\})\\sin\\theta+\\pmb\{k\}(\\pmb\{k\}\\cdot\\pmb\{v\})(1-\\cos\\theta) vrot=vvvcosθ\+(kkk×vvv)sinθ\+kkk(kkk⋅vvv)(1−cosθ)

# 公式推导 #

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc_size_16_color_FFFFFF_t_70]  
定义 v \\pmb\{v\} vvv是三维空间 R 3 \\R^3 R3中的一个任意向量， k \\pmb\{k\} kkk是一个单位向量，向量 v \\pmb\{v\} vvv绕向量 k \\pmb\{k\} kkk旋转 θ \\theta θ角度（[右手定律][Link 1]，图中逆时针方向）。

使用点乘和叉乘， v \\pmb\{v\} vvv可被分解为与 k \\pmb\{k\} kkk平行和垂直的分量：

v = v ∥ + v ⊥ (1) \\pmb\{v\}=\\pmb\{v\}\_\{\\|\}+\\pmb\{v\}\_\\perp \\tag\{1\} vvv=vvv∥\+vvv⊥(1)

其中平行于 k \\pmb\{k\} kkk的分量为：

v ∥ = ( k ⋅ v ) k (2) \\pmb\{v\}\_\{\\|\}=(\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\} \\tag\{2\} vvv∥=(kkk⋅vvv)kkk(2)

向量 v ∥ \\pmb\{v\}\_\{\\|\} vvv∥称为向量 v \\pmb\{v\} vvv在向量 k \\pmb\{k\} kkk上的投影（vector projection），垂直于 k \\pmb\{k\} kkk的分量为：

v ⊥ = v − v ∥ = v − ( k ⋅ v ) k = − k × ( k × v ) (3) \\begin\{aligned\} \\pmb\{v\}\_\{\\perp\}&=\\pmb\{v\}-\\pmb\{v\}\_\{\\|\} \\\\ &=\\pmb\{v\}-(\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\} \\\\ &=-\\pmb\{k\}\\times(\\pmb\{k\}\\times\\pmb\{v\}) \\end\{aligned\} \\tag\{3\} vvv⊥=vvv−vvv∥=vvv−(kkk⋅vvv)kkk=−kkk×(kkk×vvv)(3)

向量 v ⊥ \\pmb\{v\}\_\{\\perp\} vvv⊥称为向量 v \\pmb\{v\} vvv在向量 k \\pmb\{k\} kkk上的vector rejection（没有找到合适的翻译）。

**公式(3)最后一项的推导**：向量 k × v \\pmb\{k\}\\times\\pmb\{v\} kkk×vvv可以看作是向量 v ⊥ \\pmb\{v\}\_\{\\perp\} vvv⊥绕向量 k \\pmb\{k\} kkk逆时针旋转90°的副本，所以它们大小相等但方向垂直。同样的，向量 k × ( k × v ) \\pmb\{k\}\\times(\\pmb\{k\}\\times\\pmb\{v\}) kkk×(kkk×vvv)是 v ⊥ \\pmb\{v\}\_\{\\perp\} vvv⊥绕 k \\pmb\{k\} kkk逆时针旋转180°的副本，所以 k × ( k × v ) \\pmb\{k\}\\times(\\pmb\{k\}\\times\\pmb\{v\}) kkk×(kkk×vvv)和 v ⊥ \\pmb\{v\}\_\{\\perp\} vvv⊥的大小相等但方向相反（因此符号相反）。

> **扩展**：  
> 向量的三重叉积连接了平行分量和垂直分量，对于给定的3个向量 a \\pmb\{a\} aaa， b \\pmb\{b\} bbb， c \\pmb\{c\} ccc，有：  
>  a × ( b × c ) = ( a ⋅ c ) b − ( a ⋅ b ) c \\pmb\{a\}\\times(\\pmb\{b\}\\times\\pmb\{c\})=(\\pmb\{a\}\\cdot\\pmb\{c\})\\pmb\{b\}-(\\pmb\{a\}\\cdot\\pmb\{b\})\\pmb\{c\} aaa×(bbb×ccc)=(aaa⋅ccc)bbb−(aaa⋅bbb)ccc

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc_size_16_color_FFFFFF_t_70 1]

由于平行于旋转轴的分量在旋转时不会改变其幅度和方向，因此有：

v ∥ r o t = v ∥ (4) \\pmb\{v\}\_\{\\|rot\}=\\pmb\{v\}\_\{\\|\} \\tag\{4\} vvv∥rot=vvv∥(4)

垂直分量在旋转时会改变方向，但会保持大小不变，有：

∣ v ⊥ r o t ∣ = ∣ v ⊥ ∣ (5) |\\pmb\{v\}\_\{\\perp rot\}|=|\\pmb\{v\}\_\{\\perp\}| \\tag\{5\} ∣vvv⊥rot∣=∣vvv⊥∣(5)

v ⊥ r o t = cos ⁡ θ v ⊥ + sin ⁡ θ k × v ⊥ (6) \\pmb\{v\}\_\{\\perp rot\}=\\cos \\theta \\pmb\{v\}\_\{\\perp\}+\\sin \\theta \\pmb\{k\} \\times \\pmb\{v\}\_\{\\perp\} \\tag\{6\} vvv⊥rot=cosθvvv⊥\+sinθkkk×vvv⊥(6)

由于 k \\pmb\{k\} kkk和 v ∥ \\pmb\{v\}\_\{\\|\} vvv∥是平行的，所以它们的叉积为0，即 k × v = 0 \\pmb\{k\}\\times\\pmb\{v\}=0 kkk×vvv=0，所以我们有：

k × v ⊥ = k × ( v − v ∥ ) = k × v − k × v ∥ = k × v (7) \\begin\{aligned\} \\pmb\{k\}\\times\\pmb\{v\}\_\{\\perp\}&=\\pmb\{k\}\\times(\\pmb\{v\}-\\pmb\{v\}\_\{\\|\}) \\\\ &=\\pmb\{k\}\\times\\pmb\{v\}-\\pmb\{k\}\\times\\pmb\{v\}\_\{\\|\} \\\\ &=\\pmb\{k\}\\times\\pmb\{v\} \\end\{aligned\} \\tag\{7\} kkk×vvv⊥=kkk×(vvv−vvv∥)=kkk×vvv−kkk×vvv∥=kkk×vvv(7)

将(6)式带入(7)式，有：

v ⊥ r o t = cos ⁡ θ v ⊥ + sin ⁡ θ k × v (8) \\pmb\{v\}\_\{\\perp rot\}=\\cos\\theta\\pmb\{v\}\_\{\\perp\}+\\sin\\theta\\pmb\{k\}\\times\\pmb\{v\} \\tag\{8\} vvv⊥rot=cosθvvv⊥\+sinθkkk×vvv(8)

> **说明**：  
> 可以通过前面的图片来理解公式(8)。向量 v ⊥ \\pmb\{v\}\_\{\\perp\} vvv⊥和 k × v \\pmb\{k\}\\times\\pmb\{v\} kkk×vvv具有相同的长度，且 k × v \\pmb\{k\}\\times\\pmb\{v\} kkk×vvv是 v ⊥ \\pmb\{v\}\_\{\\perp\} vvv⊥绕 k \\pmb\{k\} kkk旋转90°得到的。利用三角函数（正弦、余弦）可得到旋转后向量的垂直分量。旋转分量的形式与笛卡尔基的2D平面极坐标 ( r , θ ) (r,\\theta) (r,θ)中的径向向量类似：  
>  r = r cos ⁡ θ e x + r sin ⁡ θ e y \\pmb\{r\}=r\\cos\\theta\\pmb\{e\}\_\{x\}+r\\sin\\theta\\pmb\{e\}\_\{y\} rrr=rcosθeeex\+rsinθeeey  
> 其中， e x \\pmb\{e\}\_\{x\} eeex， e y \\pmb\{e\}\_\{y\} eeey为它们指示方向上的单位向量。

现在，旋转后的向量可完整表示为：

v r o t = v ∥ r o t + v ⊥ r o t (9) \\pmb\{v\}\_\{rot\}=\\pmb\{v\}\_\{\\| rot\}+\\pmb\{v\}\_\{\\perp rot\} \\tag\{9\} vvvrot=vvv∥rot\+vvv⊥rot(9)

将式(4)和(8)和(2)依次带入(9)，有：

v r o t = v ∥ + cos ⁡ θ v ⊥ + sin ⁡ θ k × v = v ∥ + cos ⁡ θ ( v − v ∥ ) + sin ⁡ θ k × v = cos ⁡ θ v + ( 1 − cos ⁡ θ ) v ∥ + sin ⁡ θ k × v = cos ⁡ θ v + ( 1 − cos ⁡ θ ) ( k ⋅ v ) k + sin ⁡ θ k × v (10) \\begin\{aligned\} \\pmb\{v\}\_\{rot\}&=\\pmb\{v\}\_\{\\|\}+\\cos\\theta\\pmb\{v\}\_\{\\perp\}+\\sin\\theta\\pmb\{k\}\\times\\pmb\{v\} \\\\ &=\\pmb\{v\}\_\{\\|\}+\\cos\\theta(\\pmb\{v\}-\\pmb\{v\}\_\{\\|\})+\\sin\\theta\\pmb\{k\}\\times\\pmb\{v\} \\\\ &=\\cos\\theta\\pmb\{v\}+(1-\\cos\\theta)\\pmb\{v\}\_\{\\|\}+\\sin\\theta\\pmb\{k\}\\times\\pmb\{v\} \\\\ &=\\cos\\theta\\pmb\{v\}+(1-\\cos\\theta)(\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\}+\\sin\\theta\\pmb\{k\}\\times\\pmb\{v\} \\\\ \\end\{aligned\} \\tag\{10\} vvvrot=vvv∥\+cosθvvv⊥\+sinθkkk×vvv=vvv∥\+cosθ(vvv−vvv∥)\+sinθkkk×vvv=cosθvvv\+(1−cosθ)vvv∥\+sinθkkk×vvv=cosθvvv\+(1−cosθ)(kkk⋅vvv)kkk\+sinθkkk×vvv(10)

# 矩阵表示 #

将 v \\pmb\{v\} vvv和 k × v \\pmb\{k\}\\times\\pmb\{v\} kkk×vvv表示为列矩阵，则叉积可以表示为矩阵的乘积：

\[ ( k × v ) x ( k × v ) y ( k × v ) z \] = \[ k y v z − k z v y k z v x − k x v z k x v y − k y v x \] = \[ 0 − k z k y k z 0 − k x − k y k x 0 \] \[ v x v y v z \] (11) \\begin\{aligned\} \\begin\{bmatrix\} (\\pmb\{k\}\\times\\pmb\{v\})\_x \\\\ (\\pmb\{k\}\\times\\pmb\{v\})\_y \\\\ (\\pmb\{k\}\\times\\pmb\{v\})\_z \\end\{bmatrix\}&= \\begin\{bmatrix\} k\_y v\_z-k\_z v\_y \\\\ k\_z v\_x-k\_x v\_z \\\\ k\_x v\_y-k\_y v\_x \\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\} \\end\{aligned\} \\tag\{11\} ⎣⎡(kkk×vvv)x(kkk×vvv)y(kkk×vvv)z⎦⎤=⎣⎡kyvz−kzvykzvx−kxvzkxvy−kyvx⎦⎤=⎣⎡0kz−ky−kz0kxky−kx0⎦⎤⎣⎡vxvyvz⎦⎤(11)

令 K \\pmb\{K\} KKK表示单位向量 k \\pmb\{k\} kkk的“叉积矩阵”：

K = \[ 0 − k z k y k z 0 − k x − k y k x 0 \] (12) \\pmb\{K\}= \\begin\{bmatrix\} 0 & -k\_z & k\_y \\\\ k\_z & 0 & -k\_x \\\\ -k\_y & k\_x & 0 \\end\{bmatrix\} \\tag\{12\} KKK=⎣⎡0kz−ky−kz0kxky−kx0⎦⎤(12)

则对于任意向量 v \\pmb\{v\} vvv，矩阵方程(11)可以表示为：

K v = k × v (13) \\pmb\{K\}\\pmb\{v\}=\\pmb\{k\}\\times\\pmb\{v\} \\tag\{13\} KKKvvv=kkk×vvv(13)

事实上，K是具有这个性质的唯一矩阵，它的特征值为0和 ± i \\pm i ±i。

在等式右侧迭代叉乘等价于在等式左侧迭代左乘叉积矩阵，有（迭代2次的情况）：

K ( K v ) = K 2 v = k ( k × v ) (14) \\pmb\{K\}(\\pmb\{K\}\\pmb\{v\})=\\pmb\{K\}^2\\pmb\{v\}=\\pmb\{k\}(\\pmb\{k\}\\times\\pmb\{v\}) \\tag\{14\} KKK(KKKvvv)=KKK2vvv=kkk(kkk×vvv)(14)

由于 k \\pmb\{k\} kkk是单位向量，所以 k \\pmb\{k\} kkk具有单位的2范数，因此旋转公式(10)可以表示为：

v r o t = cos ⁡ θ v + ( 1 − cos ⁡ θ ) ( k ⋅ v ) k + sin ⁡ θ k × v = v − v + cos ⁡ θ v + ( 1 − cos ⁡ θ ) ( k ⋅ v ) k + sin ⁡ θ K v = v − ( 1 − cos ⁡ θ ) v + ( 1 − cos ⁡ θ ) ( k ⋅ v ) k + sin ⁡ θ K v = v − ( 1 − cos ⁡ θ ) ( k ⋅ k ) v + ( 1 − cos ⁡ θ ) ( k ⋅ v ) k + sin ⁡ θ K v = v + ( 1 − cos ⁡ θ ) ( ( k ⋅ v ) k − ( k ⋅ k ) v ) + sin ⁡ θ K v = v + ( 1 − cos ⁡ θ ) ( k × ( k × v ) ) + sin ⁡ θ K v = v + ( sin ⁡ θ ) K v + ( 1 − cos ⁡ θ ) K 2 v = ( I + ( sin ⁡ θ ) K + ( 1 − cos ⁡ θ ) K 2 ) v = R v , ∥ K ∥ 2 = 1 (15) \\begin\{aligned\} \\pmb\{v\}\_\{rot\}&=\\cos\\theta\\pmb\{v\}+(1-\\cos\\theta)(\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\}+\\sin\\theta\\pmb\{k\}\\times\\pmb\{v\} \\\\ &=\\pmb\{v\}-\\pmb\{v\}+\\cos\\theta\\pmb\{v\}+(1-\\cos\\theta)(\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\}+\\sin\\theta\\pmb\{K\}\\pmb\{v\} \\\\ &=\\pmb\{v\}-(1-\\cos\\theta)\\pmb\{v\}+(1-\\cos\\theta)(\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\}+\\sin\\theta\\pmb\{K\}\\pmb\{v\} \\\\ &=\\pmb\{v\}-(1-\\cos\\theta)(\\pmb\{k\}\\cdot\\pmb\{k\})\\pmb\{v\}+(1-\\cos\\theta)(\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\}+\\sin\\theta\\pmb\{K\}\\pmb\{v\} \\\\ &=\\pmb\{v\}+(1-\\cos\\theta)((\\pmb\{k\}\\cdot\\pmb\{v\})\\pmb\{k\}-(\\pmb\{k\}\\cdot\\pmb\{k\})\\pmb\{v\})+\\sin\\theta\\pmb\{K\}\\pmb\{v\} \\\\ &=\\pmb\{v\}+(1-\\cos\\theta)(\\pmb\{k\}\\times(\\pmb\{k\}\\times\\pmb\{v\}))+\\sin\\theta\\pmb\{K\}\\pmb\{v\} \\\\ &=\\pmb\{v\}+(\\sin\\theta)\\pmb\{K\}\\pmb\{v\}+(1-\\cos\\theta)\\pmb\{K\}^2\\pmb\{v\} \\\\ &=(\\pmb\{I\}+(\\sin\\theta)\\pmb\{K\}+(1-\\cos\\theta)\\pmb\{K\}^2)\\pmb\{v\} \\\\ &=\\pmb\{R\}\\pmb\{v\} \\\\ ,\\|\\pmb\{K\}\\|\_2&=1 \\end\{aligned\} \\tag\{15\} vvvrot,∥KKK∥2=cosθvvv\+(1−cosθ)(kkk⋅vvv)kkk\+sinθkkk×vvv=vvv−vvv\+cosθvvv\+(1−cosθ)(kkk⋅vvv)kkk\+sinθKKKvvv=vvv−(1−cosθ)vvv\+(1−cosθ)(kkk⋅vvv)kkk\+sinθKKKvvv=vvv−(1−cosθ)(kkk⋅kkk)vvv\+(1−cosθ)(kkk⋅vvv)kkk\+sinθKKKvvv=vvv\+(1−cosθ)((kkk⋅vvv)kkk−(kkk⋅kkk)vvv)\+sinθKKKvvv=vvv\+(1−cosθ)(kkk×(kkk×vvv))\+sinθKKKvvv=vvv\+(sinθ)KKKvvv\+(1−cosθ)KKK2vvv=(III\+(sinθ)KKK\+(1−cosθ)KKK2)vvv=RRRvvv=1(15)

有：

R = I + ( sin ⁡ θ ) K + ( 1 − cos ⁡ θ ) K 2 (16) \\pmb\{R\}=\\pmb\{I\}+(\\sin\\theta)\\pmb\{K\}+(1-\\cos\\theta)\\pmb\{K\}^2 \\tag\{16\} RRR=III\+(sinθ)KKK\+(1−cosθ)KKK2(16)

这是一个绕轴 k \\pmb\{k\} kkk逆时针旋转 θ \\theta θ角度的旋转矩阵，其中 I \\pmb\{I\} III为一个 3 × 3 3\\times3 3×3的单位矩阵。

矩阵 R \\pmb\{R\} RRR是实数空间 R 3 \\R^3 R3中旋转群 S O ( 3 ) SO(3) SO(3)中的元素， K \\pmb\{K\} KKK是生成该李群的李代数 s o ( 3 ) so(3) so(3)中的元素（注意， K \\pmb\{K\} KKK具有 s o ( 3 ) so(3) so(3)斜对称的特征）。

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200216152612150.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc=,size_16,color_FFFFFF,t_70
[Link 1]: https://en.wikipedia.org/wiki/Right_hand_rule
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200216161513869.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2NmYW45Mjc=,size_16,color_FFFFFF,t_70