三维空间刚体运动2：旋转向量表示旋转

本篇继续参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动。博文将原第三讲分为四部分来讲解：1、旋转矩阵和变换矩阵；2、旋转向量表示旋转；3、欧拉角表示旋转；4、四元数表示变换。本文相对于原文会适当精简，同时为便于理解，会加入一些注解和补充知识点，本篇为第二部分：旋转向量表示旋转，另外三部分请参照博主的其他博文。

1.定义

对于矩阵表示方式至少有以下两个缺点：
1.$SO(3)$的旋转矩阵有9个量，但一次旋转只有3个自由度，因此这种表达方式是冗余的，同理$SE(4)$也是。那么，是否有更紧凑的表示呢？
2.旋转矩阵和变换矩阵自身带有约束：它必须是正交矩阵且行列式为。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变的更困难。
因此，希望有一种方式能够紧凑的描述旋转和平移。

旋转向量：事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画，于是，我们可以使用一个向量，其方向$n$与旋转轴一致，其长度等于旋转角$\Theta$，那么向量$\Theta n$也可以描述这个旋转，这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。同样，对于变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换，此时变量维数正好是六维。

2.罗德里格斯公式

2.1 定义

罗德里格斯公式：旋转向量和旋转矩阵有什么联系吗？事实上，从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigues’s Formula）表示。因为任意旋转都可以由一个旋转轴$n$和一个旋转角$\Theta$刻画，故罗德里格斯公式具体形式如下：$R= \cos \theta I+(1-\cos \theta )nn^{T}+\sin \theta n^{\wedge }\tag{1.1}$
符号$^{\wedge }$是向量到反对称矩阵的转换符，见第一篇博客的公式（1.4）。

2.2 推导

首先，理解图1。定义向量$k$是旋转轴的单位矢量，为表示方便，使用$k$代表$n$。图中初始向量$v$以$k$为轴旋转角$\theta$得到$v_{rot}$。向量$w$为$k\times v$的方向上的单位向量，向量$v_{\perp }$和$v_{rot\perp }$分别是$v$和$v_{rot}$的垂直于平面的向量。向量$a$和$b$分别是$v_{rot}$在$w$和$v_{\perp }$方向上的分量。
图1.1：旋转向量3D图（推荐数学绘图软件：geogebra）

所谓推导旋转方程，实则求出一个旋转矩阵，使得$V_{rot}=Rv$，所以我们要做的就是找出$v_{rot}$与$v$，并用矩阵来表示。
此公式有2种形式，故而也有2种推导方法，两者推导方法的不同主要在$v_{\perp }$的表示上。具体推导过程如下。

2.2.1 推导一

推导一使用单位向量$k$表示，推导过程如下：

1. 对$v$进行向量分解：$v = v_{\perp } + v_{||}$

2. 根据向量减法可得：$v_{\perp } = v - v_{||}$

3. 由旋转过程平行向量不变得：$v_{rot||} = v_{||}$

4. $v_{||}$其实就是$v$在$u$上的正交投影（Orthogonal Projection），根据正交投影公式：$\begin{aligned} v_{||} &= proj_{k}(v) &= \frac{k\cdot v}{k\cdot k}k &= \frac{k\cdot v}{||k||^{2}}k &= (k\cdot v)k \end{aligned}$
   其中$u\cdot k=||k||^{2}, ||k||=1,(v\cdot k)$为标量，所以再乘向量$k$得到一个矢量

5. 为计算方便，对$v_{rot\perp}$ 进行向量分解：$v_{rot\perp} =a+b$

6. 由图中的向量关系经推导可得：$a=\sin \theta k\times v,b=\cos \theta v_{\perp }$，$a,b$的推导在后边

7. 综上可得：
   $v_{rot}=v_{rot\perp}+v_{rot||}$
           $=a+b+v_{||}$
           $=\sin \theta k\times v +\cos \theta v_{\perp } + (v\cdot k)k$
           $=\sin \theta k\times v +\cos \theta (v - v_{||}) + (v\cdot k)k$
           $=\sin \theta k\times v +\cos \theta (v - (v\cdot k)k ) + (v\cdot k)k$
           $=\cos \theta v+(1-\cos \theta )(v\cdot k)k + \sin \theta k\times v$
   显然：到此步，我们还无法将其用矩阵来表示，所以需要对$(v\cdot k)k$ 和 $k\times v$ 进行矩阵转换，由点积的交换律和结合律得：$(v\cdot k)k=k\cdot(v\cdot k)=k\cdot (k^{T} v)=k\cdot k^{T} v$其中的向量都是列向量，点积展开规则为：$x\cdot y = [x, y] =x^{T}y$。
   对于$k\times v$可用叉乘矩阵来化简为$Kv$：$\begin{bmatrix} (k\times v)_{x}\\ (k\times v)_{y}\\ (k\times v)_{z} \end{bmatrix}= \begin{bmatrix} k_{y}v_{z}- k_{z}v_{y}\\ k_{z}v_{x}- k_{x}v_{z}\\ k_{x}v_{y}- k_{y}v_{x} \end{bmatrix}= \begin{bmatrix} 0 & -k_{z} & k_{y}\\ k_{z} & 0 & -k_{x}\\ k_{y} & k_{x} & 0 \end{bmatrix}\begin{bmatrix} v_{x}\\ v_{y}\\ v_{z} \end{bmatrix}= Kv$
   故$v_{rot}=\cos \theta v+(1-\cos \theta )(v\cdot k)k + \sin \theta k\times v$
              $=\cos \theta v+(1-\cos \theta )kk^{T}v + \sin \theta K v$
              $=(\cos \theta I +(1-\cos \theta )kk^{T} + \sin \theta K )v$
   故旋转矩阵$R= \cos \theta I +(1-\cos \theta )kk^{T} + \sin \theta K. \tag{1.2}$其中$I$为单位矩阵。

2.2.2 推导二

与推导一相比，推导二的不同主要在于用叉乘去表示一些数据。用叉乘来表示v⊥可得：$v_{\perp } =-k\times (k\times v)$。
所以联立推导一中各式得：
$v_{rot}=v_{rot\perp}+v_{rot||}$
        $=a+b+v_{||}$
        $=\sin \theta k\times v +\cos \theta v_{\perp } + v-v_{\perp }$
        $=\sin \theta k\times v -\cos \theta k\times (k\times v) + v + k\times (k\times v)$
        $=v + (1 - \cos \theta )k\times (k\times v) + \sin \theta k\times v$
        $=v + (1 - \cos \theta )K^{2}v+ \sin \theta Kv$(叉乘矩阵表示)
        $=(I + (1 - \cos \theta )K^{2}+ \sin \theta K)v$
        $=Rv$
从而得出第二种表达式$R= I +(1-\cos \theta )K^{2} + \sin \theta K. \tag{1.3}$显然，第二种表达式更为简便，在计算的过程中涉及的参数更少，所以这也是在进行旋转操作时常用的公式。

2.2.3 推导$a$

向量$a$和$b$是由$v_{rot\perp }$正交分解得到的矢量，既有大小又有方向，所以在求解时，我们要对其大小和方向分别求解。
大小
设$\theta_{1}= \pi -\theta$，$\theta_{2}$是向量$v$和$k$的夹角，$k$为单位向量，则对于$a$的大小$|a|$有：
$|a|=\sin \theta_{1}|v_{rot\perp }|=\sin \theta_{1}|v_{\perp }|$
      $=\sin (\pi -\theta)|v_{\perp }|=\sin \theta|v_{\perp }|$
      $=\sin \theta \sin \theta_{2}|v|$
      $=\sin \theta \sin \theta_{2}|v||k|$
      $=\sin \theta |k \times v|$
由公式$|a\times b| = \sin \theta |a||b|$知$\sin \theta_{2}|v||k|=|k\times v|$，所以：
$|a|=\sin \theta |k \times v|$

方向
由叉乘方向可得$a$的单位方向向量为：$k\times v /|k \times v|$，综上可得：
$a=(k\times v /|k \times v|)|a|$
    $=(k\times v /|k \times v|)\sin \theta |k \times v|$
    $=\sin \theta k\times v$

2.2.4 推导$b$

大小
由图得，$\theta_{1}$为$b$和$v_{rot\perp }$的夹角，则：
$|b|=\cos \theta_{1}|v_{rot\perp }|=\cos (\pi -\theta)|v_{\perp }|= -\cos \theta|v_{\perp }|$

方向
由于$b$的方向与$v_{\perp }$方向相反，可得$b$的单位方向向量为：$-v_{\perp }/|v_{\perp }|$，综上可得：
$b=(-v_{\perp }/|v_{\perp }|)|b|$
    $=\cos \theta v_{\perp }$

至此，罗德里格斯公式的证明全部结束。此外，如果读者希望获得关于Oxyz座标系的旋转变换关系，可以参考这篇博客：图形变换之旋转变换公式推导。
罗德里格斯公式反应的是旋转向量到旋转矩阵的转换关系，如果已知旋转矩阵$R$，如果推导旋转向量$v$呢？下边给出旋转矩阵到旋转向量的反向转换关系。

3.旋转矩阵到向量

这里计算从一个旋转矩阵到旋转向量的转换。对于旋转角$\theta$，取旋转矩阵$R$两边的迹，有：$\begin{aligned} tr(R) &= \cos \theta tr(I)+(1-\cos \theta )tr(nn^{T})+\sin \theta tr(n^{\wedge })\\ &=3\cos \theta + (1- \cos \theta) \\ &=1+2\cos \theta. \tag{1.4} \end{aligned}$因此：$\theta = \arccos \frac{tr(R)-1}{2} .\tag{1.5}$关于转轴$n$，旋转轴上的向量在旋转后不发生改变，说明：$Rn=n.\tag{1.6}$因此，转轴$n$是矩阵$R$特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴$n$。由$v=\theta n$得到旋转向量$v$。

至此，推导结束。实践部分代码放到第三部分一起演示。

参考：
1.《视觉SLAM十四讲：从理论到实践》，高翔、张涛等著，中国工信出版社
2. 罗德里格斯公式推导