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

Posted 2021-03-09 wtyuan

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

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

向量投影（Vector projection）

　　向量a在非零向量b上的向量投影指的是a在平行于向量b的直线上的正交投影。结果是一个平行于b的向量，定义为(mathbf{a}_1=a_1hat{mathbf{b}})，其中，(a_1)是一个标量，称为a在b上的标量投影，(hat{mathbf{b}})是与b同向的单位向量。(a_1=leftVertmathbf{a} ightVertcos heta=mathbf{a}cdot hat{mathbf{b}}=mathbf{a}cdotfrac{mathbf{b}}{leftVertmathbf{b} ightVert})，其中(cdot)表示点积，(leftVertmathbf{a} ightVert)表示a的长度，( heta)表示a和b的夹角。标量投影有正负，正负号与夹角( heta)有关。

　　有了向量投影( extbf{a}_1)，向量a可以表示为(mathbf{a}=mathbf{a}_1+mathbf{a}_2)，其中(mathbf{a}_2)称为a from b的vector rejection（没找到比较官方的翻译），也即a向正交于b的超平面的正交投影，(mathbf{a}_2=mathbf{a}-mathbf{a}_1=mathbf{a}-(leftVertmathbf{a} ightVertcos heta)hat{mathbf{b}})。下图比较清晰地表示出(mathbf{a})、(mathbf{a}_1)、(mathbf{a}_2)的关系。

图1 Projection of a on b(a₁), rejection of a from b(a₂)

　　当(90^{circ}< hetale180^{circ})时，向量投影示意图如图2所示：

图2 大于90°时的向量投影示意图，此时a₁与b的方向相反

记号

　　向量a在b上的向量投影用加粗的(mathbf{a}_1)表示，标量投影用不加粗的(a_1)。有时向量投影和vector rejection分别用(mathbf{a}_{parallelmathbf{b}})和(mathbf{a}_{perpmathbf{b}})表示。

用a和b表示

　　当( heta)未知时，可通过a和b计算得出，(cos heta = frac{mathbf{a}cdotmathbf{b}}{leftVertmathbf{a} ightVertleftVertmathbf{b} ightVert})，从而标量投影、向量投影和vector rejection可以分别表示如下：

• 标量投影：
  

  
  egin{equation}
  a_1=leftVertmathbf{a} ightVertcos heta=leftVertmathbf{a} ightVertfrac{mathbf{a}cdotmathbf{b}}{leftVertmathbf{a} ightVertleftVertmathbf{b} ightVert}=frac{mathbf{a}cdotmathbf{b}}{leftVertmathbf{b} ightVert}
  end{equation}
  

• 向量投影：
  

  
  egin{equation}
  mathbf{a}_1=a_1hat{mathbf{b}}=frac{mathbf{a}cdotmathbf{b}}{leftVertmathbf{b} ightVert}frac{mathbf{b}}{leftVertmathbf{b} ightVert}=left(mathbf{a}cdothat{mathbf{b}} ight)hat{mathbf{b}}=frac{mathbf{a}cdotmathbf{b}}{mathbf{b}cdotmathbf{b}}mathbf{b}
  end{equation}
  

• vector rejection：
  

  
  egin{equation}
  mathbf{a}_2=mathbf{a}-mathbf{a}_1=mathbf{a}-frac{mathbf{a}cdotmathbf{b}}{mathbf{b}cdotmathbf{b}}mathbf{b}
  end{equation}
  

叉积

定义

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

　　叉积定义为：

egin{equation}
mathbf{a} imesmathbf{b}=leftVertmathbf{a} ightVertleftVertmathbf{b} ightVertsin( heta)mathbf{n}
end{equation}

其中，( heta)表示

a

、

b

的夹角，(0^circle hetale180^circ)，(mathbf{n})正交于

a

、

b

所在的平面，方向通常由右手法则确定，如下图所示：　

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

性质

　　右手法则决定了叉积不符合交换律，而符合反交换律，即(mathbf{a} imesmathbf{b}=-mathbf{b} imesmathbf{a})，如图4所示：

图4 反交换律

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

图5 叉积随夹角变化示意图

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

图6 叉积分配律示意图

坐标表示

　　考虑右手法则定义的标准三维坐标系，三个坐标轴(mathbf{i})、(mathbf{j})、(mathbf{{k}})如图7所示，并满足以下等式关系：
[ mathbf{i} imesmathbf{j}=mathbf{k}mathbf{j} imesmathbf{k}=mathbf{i}mathbf{k} imesmathbf{i}=mathbf{j} ]
同样，由叉积的反交换律可得下面三个等式关系：
[ mathbf{j} imesmathbf{i}=-mathbf{k}mathbf{k} imesmathbf{j}=-mathbf{i}mathbf{i} imesmathbf{k}=-mathbf{j} ]
由平行向量的叉积为零向量可得：(mathbf{i} imesmathbf{i}=mathbf{j} imesmathbf{j}=mathbf{k} imesmathbf{k}=mathbf{0})。
　　由图7也可得，任意一个三维向量都可以表示为三个基向量的线性组合，例如：
[ mathbf{a}=a_1mathbf{i}+a_2mathbf{j}+a_3mathbf{k}mathbf{b}=b_1mathbf{i}+b_2mathbf{j}+b_2mathbf{k} ]

图7 三维坐标系基向量与向量a的表示

　　进而，可以用坐标表示叉积运算如下：

egin{equation} egin{split} mathbf{a} imesmathbf{b}&=(a_1mathbf{i}+a_2mathbf{j}+a_3mathbf{k}) imes(b_1mathbf{i}+b_2mathbf{j}+b_2mathbf{k})&=(a_2b_3-a_3b_2)mathbf{i}+(a_3b_1-a_1b_3)mathbf{j}+(a_1b_2-a_2b_1)mathbf{k}&=left|egin{array}{cccc} i & j & k a_1 & a_2 & a_3 b_1 & b_2 & b_3 end{array} ight| end{split} end{equation}

上式中，将括号展开分别进行叉积推导出第二个等号，而第三个等号则可通过行列式计算得出。
　　进一步，可将叉积表示为矩阵与向量相乘的形式，由于(mathbf{a} imesmathbf{b}=(a_2b_3-a_3b_2, a_3b_1-a_1b_3,a_1b_2-a_2b_1))，则叉积可表示为：

egin{equation} egin{split} mathbf{a} imesmathbf{b}=left[mathbf{a} ight]_ imesmathbf{b}=left[egin{array}{cccc} 0 & -a_3 & a_2a_3 & 0 & -a_1-a_2 & a_1 & 0 end{array} ight]left[ egin{array}{cc} b_1_2_3 end{array} ight]=left[mathbf{b} ight]^T_ imesmathbf{a}=left[egin{array}{cccc} 0 & b_3 & -b_2-b_3 & 0 & b_1_2 & -b_1 & 0 end{array} ight]left[ egin{array}{cc} a_1a_2a_3 end{array} ight] end{split} end{equation}

其中，(left[mathbf{a} ight]_ imes)（slam14讲书上记为(mathbf{a}^wedge)）表示由向量(mathbf{a})得到的反对称矩阵，定义为：

egin{equation} egin{split} left[mathbf{a} ight]_ imes=left[egin{array}{cccc} 0 & -a_3 & a_2a_3 & 0 & -a_1-a_2 & a_1 & 0 end{array} ight] end{split} end{equation}

通过该反对称矩阵的定义可以将叉积表示为矩阵与向量的乘法。

罗德里格斯旋转公式

　　考虑(mathbf{v}inmathbb{R}^3)的三维旋转问题，旋转轴(mathbf{k})是单位向量，旋转角为( heta)，按照右手法则（即逆时针）旋转。则可通过罗德里格斯旋转公式得出旋转后的向量(mathbf{v}_{rot})为：

egin{equation} mathbf{v}_{rot}=cos hetamathbf{v}+(1-cos heta)(mathbf{k}cdotmathbf{v})mathbf{k}+sin hetamathbf{k} imesmathbf{v} end{equation}

推导过程

　　由上文中向量投影部分的知识我们知道，一个向量(mathbf{v})可以分解为平行于(mathbf{k})的分量(mathbf{v}_parallel)和正交于(mathbf{k})的分量(mathbf{v}_{perp})：

egin{equation} mathbf{v}=mathbf{v}_{parallel}+mathbf{v}_perp end{equation}

图8 向量的分解图示

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

egin{equation} mathbf{v}_parallel=(mathbf{v}cdotmathbf{k})mathbf{k} end{equation}

egin{equation} mathbf{v}_perp=mathbf{v}-mathbf{v}_parallel=mathbf{v}-(mathbf{k}cdotmathbf{v})mathbf{k}=-mathbf{k} imes(mathbf{k} imesmathbf{v}) end{equation}

关于上式最后一个等号的推导如下：

　　回顾叉积的知识，(mathbf{k} imesmathbf{v}=mathbf{k} imes(mathbf{v}_{parallel}+mathbf{v}_perp)=mathbf{0}+mathbf{k} imesmathbf{v}_perp=mathbf{k} imesmathbf{v}_perp)，(mathbf{k} imesmathbf{v})可以看做将(mathbf{v}_perp)以(mathbf{k})为旋转轴逆时针旋转了(90^circ)。正如图9所示，(mathbf{v})分解为(mathbf{v}_parallel)和(mathbf{v}_perp)，用右手法则不难确定出(mathbf{k} imesmathbf{v})的方向，进而不难发现，(mathbf{k} imes(mathbf{k} imesmathbf{v}))可以看做将(mathbf{v}_perp)以(mathbf{k})为旋转轴逆时针旋转了(180^circ)，图9中的(椭)圆正反映了(mathbf{k} imes(mathbf{k} imesmathbf{v}))、(mathbf{k} imesmathbf{v})、(mathbf{v}_perp)三者“大小相等”的关系。最终，可知(mathbf{v}_perp=-mathbf{k} imes(mathbf{k} imesmathbf{v}))。
　　

图9 罗德里格斯旋转公式几何图示

　　从图8还可以看出，v的平行分量(mathbf{v}_parallel)不会因为旋转而改变，旋转后的向量(mathbf{v}_{rot})的平行分量依然等于(mathbf{v}_parallel)，即(mathbf{v}_{parallel rot}=mathbf{v}_parallel)。
　　而v的正交分量(mathbf{v}_perp)在旋转过程中大小不变，方向会发生变化，即

egin{equation} egin{split} &|mathbf{v}_{perp rot}|=|mathbf{v}_perp|&mathbf{v}_{perp rot}=cos hetamathbf{v}_perp+sin hetamathbf{k} imesmathbf{v}_perp=cos hetamathbf{v}_perp+sin hetamathbf{k} imesmathbf{v} end{split} end{equation}

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

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

egin{equation} egin{split} mathbf{v}_{rot}&=mathbf{v}_{parallel rot}+mathbf{v}_{perp rot}&=mathbf{v}_parallel+cos hetamathbf{v}_perp+sin hetamathbf{k} imesmathbf{v}&=mathbf{v}_parallel+cos heta(mathbf{v}-mathbf{v}_parallel)+sin hetamathbf{k} imesmathbf{v}&=cos hetamathbf{v}+(1-cos heta)mathbf{v}_parallel+sin hetamathbf{k} imesmathbf{v}&=cos hetamathbf{v}+(1-cos heta)(mathbf{k}cdotmathbf{v})mathbf{k}+sin hetamathbf{k} imesmathbf{v} end{split} end{equation}

矩阵形式

　　在叉积部分提到过叉积可以表示为矩阵乘向量的形式，类似地，罗德里格斯旋转公式可以表示为旋转矩阵乘以向量的形式，(mathbf{v}_{rot}=mathbf{R}mathbf{v})，其中(mathbf{R})是旋转矩阵。在slam14讲(^{[4]})中的表示如下：
egin{equation}
mathbf{R}=cos hetamathbf{I}+(1-cos heta)mathbf{k}mathbf{k}^T+sin hetamathbf{k}^wedge
end{equation}
其中，(mathbf{I})表示单位矩阵，(mathbf{k})表示旋转向量（书中用(mathbf{n})表示旋转向量），(mathbf{k}^wedge)表示由(mathbf{k})得到的反对称矩阵。从式(13)不难看出上式，另外，结合式(13)还可以得到下面这个式子：

egin{equation} egin{split} mathbf{v}_{rot}&=mathbf{v}_{parallel rot}+mathbf{v}_{perp rot}&=mathbf{v}_parallel+cos hetamathbf{v}_perp+sin hetamathbf{k} imesmathbf{v}&=mathbf{v}-mathbf{v}_perp+cos hetamathbf{v}_perp+sin hetamathbf{k} imesmathbf{v}&=mathbf{v}+(sin heta)mathbf{k} imesmathbf{v}+(1-cos heta)mathbf{k} imesmathbf{k} imesmathbf{v} end{split} end{equation}

从而，得出这个维基百科上的矩阵表示：

egin{equation} egin{split} mathbf{v}_{rot}=mathbf{R}mathbf{v}=mathbf{v}+(sin heta)mathbf{K}mathbf{v}+(1-cos heta)mathbf{K}^2mathbf{v} end{split} end{equation}

其中，(mathbf{R}=mathbf{I}+(sin heta)mathbf{K}+(1-cos heta)mathbf{K}^2)，(mathbf{K})表示由旋转向量(mathbf{k})生成的反对称矩阵。

参考：

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