三维反对称阵与Rodrigues公式-Charlie小站

罗德里格斯变换

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

这次的主题是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$$

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

说明这个指数是怎么计算出来的
证明算出的结果是一个旋转矩阵
证明这个旋转是以$\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公式