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

原博文链接：https://www.cnblogs.com/wtyuan/p/12324495.html

本文综合了几个相关的维基百科，加了点自己的理解，从比较基础的向量投影和叉积讲起，推导出罗德里格斯旋转公式。公式比较繁杂，如有错误，欢迎评论区指出。

对于向量的三维旋转问题，给定旋转轴和旋转角度，用罗德里格斯（Rodrigues）旋转公式可以得出旋转后的向量。另外，罗德里格斯旋转公式可以用旋转矩阵表示，即将三维旋转的轴-角（axis-angle）表示转变为旋转矩阵表示。

向量投影（Vector projection）
　　向量a在非零向量b上的向量投影指的是a在平行于向量b的直线上的正交投影。结果是一个平行于b的向量，定义为a1=a1b^{，其中，a1是一个标量，称为a在b上的标量投影，b}是与b同向的单位向量。a1=∥a∥cosθ=a⋅b^=a⋅b∥b∥，其中⋅表示点积(又称标量积)，∥a∥表示a的长度，θ表示a和b的夹角。标量投影有正负，正负号与夹角θ有关。

有了向量投影a1，向量a可以表示为a=a1+a2，其中a2称为a from b的vector rejection（没找到比较官方的翻译），也即a向正交于b的超平面的正交投影，a2=a−a1=a−(∥a∥cosθ)b^。下图比较清晰地表示出a、a1、a2的关系。
　　

Projection of a on b, rejection of a from b

当90∘<θ≤180∘时，向量投影示意图如图2所示：

大于90°时的向量投影示意图

记号
　　向量a在b上的向量投影用加粗的a1表示，标量投影用不加粗的a1。有时向量投影和vector rejection分别用a∥b和a⊥b表示。

用a和b表示
　　当θ未知时，可通过a和b计算得出，cosθ=a⋅b∥a∥∥b∥，从而标量投影、向量投影和vector rejection可以分别表示如下：

标量投影：

a1=∥a∥cosθ=∥a∥a⋅b∥a∥∥b∥=a⋅b∥b∥ (1)

向量投影：

a1=a1b^{=a⋅b∥b∥b∥b∥=(a⋅b})b^=a⋅bb⋅bb (2)
vector rejection：
a2=a−a1=a−a⋅bb⋅bb (3)

叉积

定义
　　叉积（又称向量积）是三维空间（R3）向量的二元操作，用符号×表示，给定两个线性独立的向量a和b，叉积a×b的结果是一个向量，这个向量与a、b都正交，也就是正交于a、b所在的平面。为什么要强调线性独立呢，因为非线性独立的两个向量（同向或反向）的叉积为0。

叉积定义为：

a×b=∥a∥∥b∥sin(θ)n (4)

其中，θ表示a、b的夹角，0∘≤θ≤180∘，n正交于a、b所在的平面，方向通常由右手法则确定，如下图所示：　

通过右手法则确定叉积方向

性质
　　右手法则决定了叉积不符合交换律，而符合反交换律，即a×b=−b×a，如图4所示：

反交换律

由公式也可以看出当a、b的不线性独立时，即夹角为0∘或180∘时，叉积为零向量0。叉积随夹角θ的变化如图5所示。

叉积随夹角变化示意图

另外，叉积符合分配律，即a×(b+c)=a×b+a×c。如图6所示，左图向量b和c都被分解为vector projection和vector rejection两部分，右图则解释了分配律成立的原因，看图时要注意图中的平行四边形和正方形都表示了相等的关系。

图6 叉积分配律示意图

坐标表示
　　考虑右手法则定义的标准三维坐标系，三个坐标轴i、j、k如图7所示，并满足以下等式关系：

i×j=k
j×k=i
k×i=j
同样，由叉积的反交换律可得下面三个等式关系：

j×i=−k
k×j=−i
i×k=−j
由平行向量的叉积为零向量可得：i×i=j×j=k×k=0。
　　由图7也可得，任意一个三维向量都可以表示为三个基向量的线性组合，例如：

a=a1i+a2j+a3k
b=b1i+b2j+b2k
图7 三维坐标系基向量与向量a的表示

进而，可以用坐标表示叉积运算如下：
a×b=(a1i+a2j+a3k)×(b1i+b2j+b2k)
=(a2b3−a3b2)i+(a3b1−a1b3)j+(a1b2−a2b1)k
=∣i j k ∣ (5)
∣a1 a2 a3∣
∣b1 b2 b3∣

上式中，将括号展开分别进行叉积推导出第二个等号，而第三个等号则可通过行列式计算得出。
　　进一步，可将叉积表示为矩阵与向量相乘的形式，由于a×b=(a2b3−a3b2,a3b1−a1b3,a1b2−a2b1)，则叉积可表示为：

a×b=[a]×b=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥⎡⎣⎢b1b2b3⎤⎦⎥=[b]T×a=⎡⎣⎢0−b3b2b30−b1−b2b10⎤⎦⎥⎡⎣⎢a1a2a3⎤⎦⎥(6)
其中，[a]×（slam14讲书上记为a∧）表示由向量a得到的反对称矩阵，定义为：

[a]×=⎡⎣⎢0a3−a2−a30a1a2−a10⎤⎦⎥(7)
通过该反对称矩阵的定义可以将叉积表示为矩阵与向量的乘法。

罗德里格斯旋转公式
　　考虑v∈R3的三维旋转问题，旋转轴k是单位向量，旋转角为θ，按照右手法则（即逆时针）旋转。则可通过罗德里格斯旋转公式得出旋转后的向量vrot为：

vrot=cosθv+(1−cosθ)(k⋅v)k+sinθk×v(8)
推导过程
　　由上文中向量投影部分的知识我们知道，一个向量v可以分解为平行于k的分量v∥和正交于k的分量v⊥：

v=v∥+v⊥(9)

图8 向量的分解图示

如图8所示，因为k为单位向量，由向量投影部分知识可得

v∥=(v⋅k)k(10)
v⊥=v−v∥=v−(k⋅v)k=−k×(k×v)(11)

式(11)用于后面推导维基百科中罗德里格斯旋转公式的矩阵形式，其中，最后一个等号的推导如下：

回顾叉积的知识，k×v=k×(v∥+v⊥)=0+k×v⊥=k×v⊥，k×v可以看做将v⊥以k为旋转轴逆时针旋转了90∘(可参考图8理解)。正如图9所示，v分解为v∥和v⊥，用右手法则不难确定出k×v的方向，进而不难发现，k×(k×v)可以看做将v⊥以k为旋转轴逆时针旋转了180∘，图9中的(椭)圆正反映了k×(k×v)、k×v、v⊥三者“大小相等”的关系。最终，可知v⊥=−k×(k×v)。
图9 罗德里格斯旋转公式几何图示

从图8还可以看出，v的平行分量v∥不会因为旋转而改变，旋转后的向量vrot的平行分量依然等于v∥，即v∥rot=v∥。
　　而v的正交分量v⊥在旋转过程中大小不变，方向会发生变化，即

|v⊥rot|=|v⊥|v⊥rot
=cosθv⊥+sinθk×v⊥
=cosθv⊥+sinθk×v(12)

式(12)中第2个等式通过图9可以得出，将圆看做xOy坐标系平面，v⊥所在的直线看做x轴，k×v所在的直线看做y轴，结合三角函数，很容易用v⊥和k×v表示出v⊥rot。

到这已经得出罗德里格斯公式了：

vrot=v∥rot+v⊥rot
=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(13)
矩阵形式
　　在叉积部分提到过叉积可以表示为矩阵乘向量的形式，类似地，罗德里格斯旋转公式可以表示为旋转矩阵乘以向量的形式，vrot=Rv，其中R是旋转矩阵。在slam14讲[4]中的表示如下：

R=cosθI+(1−cosθ)kkT+sinθk∧(14)
其中，I表示单位矩阵，k表示旋转向量（书中用n表示旋转向量），k∧表示由k得到的反对称矩阵。从式(13)不难看出上式，另外，结合式(13)还可以得到下面这个式子：

vrot=v∥rot+v⊥rot=v∥+cosθv⊥+sinθk×v=v−v⊥+cosθv⊥+sinθk×v=v+(sinθ)k×v+(cosθ−1)×v⊥=v+(sinθ)k×v+(1−cosθ)k×(k×v)(15)
上式最后一个等号的推导用到了式(11)。从而，得出这个维基百科上的矩阵表示：

vrot=Rv=v+(sinθ)Kv+(1−cosθ)K2v(16)
其中，R=I+(sinθ)K+(1−cosθ)K2，K表示由旋转向量k生成的反对称矩阵。

参考：
[1] Rodrigues’ rotation formula
[2] Cross product
[3] Vector projection
[4] 视觉SLAM十四讲：从理论到实践