三维反对称阵与Rodrigues公式

罗德里格斯变换

这将会是一篇比较无趣的博客,因为主题是数学,而且是枯燥繁琐的数学计算。如果你没有接触过线性代数,那么你可以关闭浏览器了。如果恰好你知道一点矩阵的知识,并且还有一点喜欢数学,那么你可以继续往下看。

这次的主题是Rodrigues公式,一种计算三维空间内的旋转矩阵的方法。三维空间\(\mathbb{R}^3\)上的旋转可以通过一个三维矩阵描述。一个转轴为\(\mathbf{r} = [x,y,z]^T\),旋转角为\(\theta\)的三维旋转\(\mathbf{R}\)可以通过Rodrigues公式得出:

$$\mathbf{R} = \mathbf{I}  + \sin{\theta}[\mathbf{r}]_{\times} + (1-\cos{\theta})[\mathbf{r}]_{\times}^2$$

其中,

$$[\mathbf{r}]_{\times} = \left[\begin{array}{ccc}0 & -z & y \\z & 0 & -x \\-y & x & 0 \\\end{array}\right].$$

嗯,忘了说明,上面的\(\mathbf{r}\)需要是一个单位向量。这是一个很神奇且有用的公式。在现实的三维世界中,刚体的运动就只有两种,即平移和旋转。旋转矩阵在导航定位、机械连杆、运动描述中经常被使用。我第一见到这个公式应该是在好几年前,但由于种种原因(其实就是懒),一直没有探究这个公式为什么是对的。如今互联网这么发达,很容易就可以找到这个公式。然而,很少会有人给出这个公式的详细说明,依然是知其然而不知其所以然。我也懒得去找这个公式的来源了。好歹我也是半个数学专业的(学过三年的法国预科),这点任务还是可以自己解决的。于是,用了几页草稿纸,给出这个公式的推导和证明。证明不是非常的严谨,请不要见怪。下面开始正题。

首先,说明一下这个公式是怎么算出来的。其实也挺简单就是

$$\mathbf{R} = \exp(\theta[\mathbf{r}]_{\times}) = \mathbf{I}  + \sin{\theta}[\mathbf{r}]_{\times} + (1-\cos{\theta})[\mathbf{r}]_{\times}^2$$

当然,不能就这么草草了事,否则太对不起各位看客了。接下来,还有三件事需要做

  1. 说明这个指数是怎么计算出来的
  2. 证明算出的结果是一个旋转矩阵
  3. 证明这个旋转是以\(\mathbf{r}\)为转轴,\(\theta\)为转角

先来算这个指数表达式。在此之前,先推导一个三维反对称阵幂指数的表达式。说推导可能有些不合适,其实就是找规律。假设\(\mathbf{A}\)是一个\(\mathbb{R}^3\)上的的反对称矩阵,为了方面计算,设右上角的三个元素分别为\(x,y,z\)。我们先计算\(\mathbf{A}\mathbf{A}\)。由于

$$\mathbf{A}\mathbf{A} = (-\mathbf{A}^T)(-\mathbf{A}^T) = (\mathbf{A}\mathbf{A})^T,$$所以计算一半就好了,

$$\mathbf{A}\mathbf{A} = \left[\begin{array}{ccc} -x^2-y^2 & yz & xz \\  & -x^2-z^2 & -xy \\  &  & -y^2-z^2 \\\end{array}\right].$$

接着算三次幂,与两次类似,$$(\mathbf{A}\mathbf{A}\mathbf{A})^T = \mathbf{A}^T\mathbf{A}^T\mathbf{A}^T = -\mathbf{A}\mathbf{A}\mathbf{A},$$也不用全部计算,这次算三个数就行。

$$\mathbf{A}\mathbf{A}\mathbf{A}= -(x^2+y^2+z^2)\left[\begin{array}{ccc} 0 & x & y \\  &  0 & z \\  &  & 0 \\\end{array}\right].$$

引入Frobenius范数,上式可以写成更加简洁的形式

$$\mathbf{A}\mathbf{A}\mathbf{A} = -\frac{1}{2}\|\mathbf{A}\|_F^2\mathbf{A}$$

通过上面的结果,我们已经可以写出\(\mathbf{A}^n\)的表达式。当然,计算\(\exp{\theta[\mathbf{r}]_{\times}}\)也就不再话下:

$$ \exp(\theta[\mathbf{r}]_{\times}) = \sum_{k=0}^{\infty}\frac{(\theta[\mathbf{r}]_{\times})^k}{k!}$$

由于我们设定\(\mathbf{r}\)是单位向量,所以\(\|\mathbf{r}\|_F^2 = 2\),所以可以对上面的等式继续进行化简:

$$\begin{eqnarray*}
% \nonumber to remove numbering (before each equation)
\exp(\theta[\mathbf{r}]_{\times}) &=& \sum_{k=0}^{\infty}\frac{(\theta[\mathbf{r}]_{\times})^k}{k!} \\
&=& \mathbf{I} + \sum_{k=0}^{\infty}\frac{(\theta[\mathbf{r}]_{\times})^{2k+1}}{(2k+1)!} + \sum_{k=1}^{\infty}\frac{(\theta[\mathbf{r}]_{\times})^{2k}}{(2k)!}  \\
&=& \mathbf{I} +   \sum_{k=0}^{\infty}\frac{\theta^{2k+1}}{(2k+1)!}(-1)^k[\mathbf{r}]_{\times} +  \sum_{k=1}^{\infty}\frac{\theta^{2k}}{(2k)!}(-1)^{k+1}[\mathbf{r}]_{\times}^2\\
&=& \mathbf{I} + \sin{\theta}[\mathbf{r}]_{\times} + (1-\cos{\theta})[\mathbf{r}]_{\times}^2
\end{eqnarray*}$$

指数计算到此为止,接下来证明这个结果是一个旋转矩阵。这个很容易,如果\(\mathbf{A}\)是一个三维空间上反对称矩阵,按照定义,

$$\exp(\mathbf{A})\exp(\mathbf{A})^T = \exp(\mathbf{A})\exp(\mathbf{A^T}) = \exp(\mathbf{A})\exp(\mathbf{-A}) = \mathbf{I}$$

另一半证明也一样,所以\(\mathbf{R}\)是一个正交矩阵。一个正交矩阵是旋转矩阵的条件是其行列式为1。若\(\mathbf{A}\)是一个三维空间上反对称矩阵,则

$$\det{\mathbf{A}} = \exp{{\mathrm{tr}(\mathbf{A})}} = 1$$

因此\(\mathbf{R}\)是一个旋转矩阵。现在只差最后一步——验证转轴与转角。一个旋转矩阵的转轴可以直接通过计算特征向量得出,但这里已经知道转轴,只需代入验证即可。如果知道

$$[\mathbf{r}]_{\times}\mathbf{r} = \mathbf{r}\times \mathbf{r}= \mathbf{0}$$

那么验证得过程会很简单。当然,动手算一下也不是很难。验证的过程写出来就是:

$$\mathbf{R}\mathbf{r} = \mathbf{I}\mathbf{r}+\sin{\theta}\mathbf{r}\times\mathbf{r}+(1-\cos\theta)[\mathbf{r}]_{\times}(\mathbf{r}\times\mathbf{r}) = \mathbf{r}$$

旋转角的计算只需找一个与\(\mathbf{r}\)正交的向量进行旋转,进而通过旋转前后的内积就可得到夹角的余弦值。设\(\mathbf{e}\)是与\(\mathbf{r}\)不共线的单位向量,则

$$\mathbf{u} = \mathbf{r}\times\mathbf{e} = [\mathbf{r}]_{\times}\mathbf{e}$$

是一个与\(\mathbf{r}\)正交的向量,将旋转作用在\(\mathbf{u}\)上

$$\begin{eqnarray*}
\mathbf{R} \mathbf{u} &=& ( [\mathbf{r}]_{\times}\mathbf{e})[\mathbf{r}]_{\times}^2)[\mathbf{r}]_{\times}\mathbf{e} \\
&=& ([\mathbf{r}]_{\times} + \sin\theta [\mathbf{r}]_{\times}^2 +(1-\cos\theta)[\mathbf{r}]_{\times}^3)\mathbf{e}\\
&=& ([\mathbf{r}]_{\times} + \sin\theta [\mathbf{r}]_{\times}^2 -(1-\cos\theta)[\mathbf{r}]_{\times})[\mathbf{e}\\
&=&  (\sin\theta [\mathbf{r}]_{\times}^2 +\cos\theta[\mathbf{r}]_{\times})\mathbf{e}
\end{eqnarray*}$$

于是

$$\begin{eqnarray*}
\mathbf{R} \mathbf{u}\cdot \mathbf{u} &=& ( (\sin\theta [\mathbf{r}]_{\times}^2 +\cos\theta[\mathbf{r}]_{\times})\mathbf{e})([\mathbf{r}]_{\times}\mathbf{e}) \\
&=& \sin\theta (\mathbf{r}\times\mathbf{r}\times\mathbf{e})(\mathbf{r}\times\mathbf{e})+\cos\theta(\mathbf{r}\times\mathbf{e})(\mathbf{r}\times\mathbf{e})\\
&=& \cos\theta\|\mathbf{r}\times\mathbf{e}\|_2^2
\end{eqnarray*}$$

至此,Rodrigues公式算是证完了。

未经允许不得转载:Charlie小站 » 三维反对称阵与Rodrigues公式

赞 (6)
分享到:更多 ()