罗德里格斯公式推导

Nov 16,2018   772 words   3 min

Tags: SLAM

在SLAM中介绍旋转向量的时候会提到一个公式叫罗德里格斯公式(Rodrigues’ Formula),它说明了旋转向量与旋转矩阵之间的关系。 假设某个旋转的旋转轴为向量,角度为θ。从旋转向量到选择矩阵可表示如下。

表示对向量取反对称矩阵。下面就对公式进行推导。

1.公式推导

本文的示意图来自Wiki上的罗德里格斯公式词条,点击查看 如上图所示(点击图片可查看svg矢量图),假设某个单位向量,某个向量为轴,旋转了θ角(逆时针)变成了。对于,可以将其分解为垂直和平行于旋转轴k的分量,且根据向量加法满足如下关系。

对于由于其大小可以看成是向量上的投影,即做点积,而其方向与平行,可以表示成的倍数,因此可以表示成如下形式:

对于,由上面的式子可以得到:

而又如上图所示(点击图片可查看svg矢量图),等于。在上图中得到的是一个垂直于所构成平面的一个向量(可以看作是绕k逆时针转90°),方向满足右手定则(从转到)。而再与做叉乘,得到的是一个垂直于平面的向量(可以看作是绕k逆时针转180°),方向满足右手定则(从转到)。所以大小相等,方向相反,前者加个负号,即相等。因此:

向量在绕旋转之后,其在旋转轴上的投影向量是不会变的(很容易理解,要不然也不叫旋转轴了),有:

对于垂直于轴的分量,只会改变方向,大小并不会变。

而由上图,根据三角函数知识,在直角三角形中可以计算出:

因为平行,所以它们做叉乘结果为向量,且根据与投影分量的关系,有:

所以可以写成:

因此对于旋转后的向量,有:

即:

以上为罗德里格斯的矢量运算表达式,下面推导矩阵运算形式。对于向量可以获得它对应的反对称矩阵K,

所以有如下式子成立

又因为,所以,带入有:

整理可得:

一般把等式右边括号中的部分叫做旋转矩阵R,即:

又由于

所以整理可得:

带入上式,整理可得:

这样便推导出了罗德里格斯公式。

本文作者原创,未经许可不得转载,谢谢配合

返回顶部