罗德里格斯公式

罗德里格斯公式

如果v 是R^3中的向量，k 是描述旋转轴的单位向量，根据右手定则v 围绕该旋转轴旋转角度θ ，旋转向量的V_{rot}罗德里格斯公式为

​ V_{rot}= \cos\theta V + (1-\cos\theta)(V \cdot K)K + \sin\theta K\times V

推导

​ 图一

令k 为定义旋转轴的单位向量，令v 为围绕k 旋转角度 θ 的任意向量（右手定则，图中逆时针方向）。

使用点积和叉积，向量v 可以分解为平行和垂直于轴k 的分量

V = V_ \parallel + V_ \perp

其中与K平行的分量是:V_ \parallel = (V \cdot K)K ,

V_ \parallel看作向量v 在轴k 上的投影。

V_ \perp = V - V_ \parallel = V - (V \cdot K)K = -K\times(K\times V)

其中，最后一个等号的推导如下：

回顾叉积的知识，K \times V = K \times (V_\parallel + V_\perp) = 0 + K \times V_\perp = K \times V_\perp，K \times V可以看做将V_\perp以K为旋转轴逆时针旋转了90°（可参考图1来理解）。正如图2所示，V分解为V_\parallel和V_\perp，用右手法则不难确定出K \times V的方向，进而不难发现K\times(K\times V)可以看作将V_\perp以K为旋转轴逆时针旋转了180°，图中的圆正反映了K\times(K\times V)、K\times V、V_\perp 三者大小相等的关系，最终，可知 V_ \perp == -K\times(K\times V)。

从图1还可以看出，v 的平行分量V_\parallel不会因为旋转而改变，旋转后的向量V_{rot}的平行分量依然等于V_\parallel，即V_{\parallel rot} = V_\parallel。

而v 的正交分量V_\perp在旋转过程中大小不变，方向会发生变化，即

|V_{\perp rot}| =| V_\perp|

V_{\perp rot} = \cos\theta V_\perp + \sin\theta K\times V_\perp = \cos\theta V_\perp + \sin\theta K\times V

上式的解释，因为向量V_\perp和具K \times V有相同的长度，并且K \times V是围V_\perp绕K逆时针旋转了90°。使用三角函数sin和cos对V_\perp和K \times V进行适当的缩放，得到旋转的垂直分量。

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

V_{rot} = V_{\parallel rot} + V_{\perp rot}

​ = V_\parallel + \cos\theta V_\perp + \sin\theta K\times V

​ = V_\parallel + \cos\theta (V-V_\parallel) + \sin\theta K\times V

​ = \cos\theta V + (1-\cos\theta)V_\parallel + \sin\theta K\times V

​ = \cos\theta V + (1-\cos\theta)(V \cdot K)K + \sin\theta K\times V

矩阵形式

在叉积部分提到过叉积可以表示为矩阵乘向量的形式，类似地，罗德里格斯旋转公式可以表示为旋转矩阵乘以向量的形式，V_{rot} = R V，其中R是旋转矩阵。在slam14讲中的表示如下：

R = \cos\theta I +(1-\cos\theta)KK^T+\sin\theta K^{\wedge}

\cos\theta I +(1-\cos\theta)KK^T+\sin\theta K^{\wedge}$

其中，I表示单位矩阵，K表示旋转向量（书中用n表示旋转向量），K^{\wedge}表示由K得到的反对称矩阵。