三维空间刚体运动2：旋转向量与罗德里格斯公式

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

1.定义

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

旋转向量：事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画，于是，我们可以使用一个向量$u$（为方便表达，上下文统一采用符号$u$表示单位向量，其他符号$n,k$等也是可以的），其方向与旋转轴一致，其长度等于旋转角$\theta$，那么向量$\theta u$也可以描述这个旋转，这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转。

在三维空间中定义一个方向只需要用到两个量就可以了（与任意两个坐标轴之间的夹角，比如地球的经纬度就可以确定方向），因此第三个量可以用来定义长度，表示旋转角度。同样，对于变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换，此时变量维数正好是六维。

2.罗德里格斯公式

2.1 定义

罗德里格斯公式：旋转向量和旋转矩阵有什么联系吗？事实上，从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigues’s Formula）表示。因为任意旋转都可以由一个旋转轴$u$和一个旋转角$\theta$刻画，故罗德里格斯公式具体形式如下：$$R=\cos \theta I+(1-\cos \theta )u{u}^T+\sin \theta u^{\wedge }\text{\hspace{1em}(2.1)}$$符号${}^{\wedge }$是向量到反对称矩阵的转换符，见上一篇博客《三维空间刚体运动1：旋转矩阵与变换矩阵》的公式（1.4）。公式还可以写为：$$R=I+(1-\cos \theta )U^2+\sin \theta U.\text{\hspace{1em}(2.2)}$$其中U代表向量$u$转换的反对称矩阵$u^{\wedge }$。

2.2 推导

首先，理解下图。定义$u$是旋转轴的单位向量，$v$为旋转向量，$w$为$u\times v$方向上的单位向量。图中$v$绕$u$旋转角度$\theta$得到$v_{rot}$。将$v$分解为平行于旋转轴$u$以及正交于$u$的两个分量：$v_{\Vert }$和$v_{\perp }$。将$v_{rot}$分解为平行于旋转轴$u$以及正交于$u$的两个分量：$v_{rot||}$和$v_{rot\perp }$，其中$v_{rot||}=v_{||}$。向量$a$和$b$分别是$v_{rot\perp }$在$w$和$v_{\perp }$方向上的分量。
图2.1：旋转向量3D图（数学绘图软件推荐geogebra）

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

2.2.1 推导一

推导一推导过程如下：

1. $v$：对$v$进行向量分解：$v=v_{\perp }+v_{||}$，根据向量减法可得：$$v_{\perp }=v-v_{||}\text{\hspace{1em}(2.3)}$$对$v_{rot||}$进行向量分解$$v_{rot}=v_{rot||}+v_{rot\perp }\text{\hspace{1em}(2.4)}$$下边分别推导$v_{rot||}$和$v_{rot\perp }$。
2. $v_{rot||}$：由于旋转过程平行向量的不变性可得$v_{rot||}=v_{||}$，$v_{||}$其实就是$v$在$u$上的正交投影（Orthogonal Projection），根据正交投影公式：$$\begin{array}{rlrlr}{v}_{||}& =pro{j}_u(v)& =\frac{u\cdot v}{u\cdot u}u& =\frac{u\cdot v}{||u|{|}^2}u& =(u\cdot v)u\end{array}\text{\hspace{1em}(2.5)}$$其中$u\cdot u=||u|{|}^2$，$||u||=1$，点积$(u\cdot v)$为标量，所以再乘向量$u$得到一个矢量。
3. $v_{rot\perp }$：下面画出$v_{rot\perp }$的俯视图：
   图2.2：旋转向量俯视图
   

我们需要处理正交于$u$的$v_{\perp }$，因为这两个向量是正交的，这个旋转可以看做是平面内的一个旋转，因为旋转不改变$v_{\perp }$的长度，所以路径是一个圆。现在，3D的旋转被转化为了2D平面上的旋转。由于在这个平面上只有一个向量$v_{\perp }$，用它来表示一个旋转是不够的，我们还需要构造一个同时正交于$u$ 和$v_{\perp }$的向量$w$，这个可以通过叉乘来获得： $$w=u\times v_{\perp }\text{\hspace{1em}(2.6)}$$注意叉乘的顺序，因为我们使用的是右手坐标系统，按照右手定则你可以发现这 个新的向量$w$指向$v_{\perp }$逆时针旋转$(\pi /2)$后的方向，并且和$v_{\perp }$一样也处于正交于$u$ 的平面内。因为$\Vert u\Vert$ = 1，我们可以发现：$$\begin{array}{rl}||w||& =||u\times v_{\perp }||\\ & =||u||\cdot ||v_{\perp }||\cdot \sin (\frac{\pi }{2})\\ & =||v_{\perp }||\end{array}\text{\hspace{1em}(2.7)}$$也就是说，$w$和$v_{\perp }$的模长是相同的，所以，$w$也位于圆上。有了这个新的向量$w$， 就相当于在平面内有了两个坐标轴。我们现在可以把$v_{rot||}$投影到$w$和$v_{\perp }$上，将其分解为向量$a$和$b$，使用一些三角知识可以得到：$$\begin{array}{rl}{v}_{rot\perp }& =a+b\\ & =\sin \theta w+\cos \theta v_{\perp }\\ & =\sin \theta (u\times v_{\perp })+\cos \theta v_{\perp }\end{array}\text{\hspace{1em}(2.8)}$$因为叉乘遵守分配律，且$u$平行于$v_{||}$，故：$$\begin{array}{rl}u\times v_{\perp }& =u\times (v-v_{||})\\ & =u\times v-u\times v_{||}\\ & =u\times v\end{array}\text{\hspace{1em}(2.9)}$$另外，向量$a$和$b$还有另外一种证法，稍显繁琐，有兴趣的同学请参见后两小节。

4. 综上可得：
   $$\begin{array}{rl}{v}_{rot}& =v_{rot\perp }+v_{rot||}\\ & =a+b+v_{||}\\ & =\sin \theta u\times v_{\perp }+\cos \theta v_{\perp }+(u\cdot v)u\\ & =\sin \theta u\times v+\cos \theta (v-v_{||})+(u\cdot v)u\\ & =\sin \theta u\times v+\cos \theta (v-(u\cdot v)u)+(u\cdot v)u\\ & =\cos \theta v+(1-\cos \theta )(u\cdot v)u+\sin \theta u\times v\end{array}\text{\hspace{1em}(2.10)}$$
5. 显然：到此步，我们还无法将其用矩阵来表示，所以需要对$(u\cdot v)u$ 和 $u\times v$ 进行矩阵转换，由点积的交换律和结合律得：$$(u\cdot v)u=u\cdot (u\cdot v)=u\cdot (u^{T}v)=u\cdot u^{T}v\text{\hspace{1em}(2.11)}$$其中的向量都是列向量，点积展开规则为：$x\cdot y=[x,y]=x^{T}y$。
   对于$u\times v$可用叉乘矩阵来化简为$Uv$：$$u\times v=\left[\begin{array}{c}(u\times v{)}_x\\ (u\times v{)}_y\\ (u\times v{)}_z\end{array}\right]=\left[\begin{array}{c}{u}_y{v}_z-u_z{v}_y\\ u_z{v}_x-u_x{v}_z\\ u_x{v}_y-u_y{v}_x\end{array}\right]=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ u_y& u_x& 0\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]=Uv\text{\hspace{1em}(2.12)}$$其中$$U=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ u_y& u_x& 0\end{array}\right]\text{\hspace{1em}(2.13)}$$
6. 将$(u\cdot v)u$ 和 $u\times v$ 转换的矩阵代入式（2.10）得：$$\begin{array}{rl}{v}_{rot}& =\cos \theta v+(1-\cos \theta )(v\cdot u)u+\sin \theta u\times v\\ & =\cos \theta v+(1-\cos \theta )u{u}^{T}v+\sin \theta Uv\\ & =(\cos \theta I+(1-\cos \theta )u{u}^T+\sin \theta U)v\end{array}\text{\hspace{1em}(2.14)}$$故旋转矩阵$$R=\cos \theta I+(1-\cos \theta )u{u}^T+\sin \theta U.\text{\hspace{1em}(2.15)}$$其中$I$为单位矩阵。

2.2.2 推导二

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

2.2.3 推导向量$a$和$b$

此处单独推导罗德里格斯公式的向量$a$和$b$，仅做参考，也可以忽略不看。$a$和$b$是由$v_{rot\perp }$正交分解得到的矢量，既有大小又有方向，所以在求解时，我们要对其大小和方向分别求解。
$a$大小
设${\theta }_1=\pi -\theta$，${\theta }_2$是向量$v$和$u$的夹角，$u$为单位向量，则对于$a$的大小$|a|$有：
$$\begin{array}{rl}|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||u|\end{array}\text{\hspace{1em}(2.19)}$$由三角公式$|a\times b|=\sin \theta |a||b|$知：$\sin {\theta }_2|v||u|=|u\times v|$，所以：
$$|a|=\sin \theta |u\times v|\text{\hspace{1em}(2.20)}$$

$a$方向
由叉乘方向可得$a$的单位方向向量为：$$u\times v/|u\times v|\text{\hspace{1em}(2.21)}$$

综上可得：
$$\begin{array}{rl}a& =(u\times v/|u\times v|)|a|\\ & =(u\times v/|u\times v|)\sin \theta |u\times v|\\ & =\sin \theta u\times v\end{array}\text{\hspace{1em}(2.22)}$$

$b$大小
由图得，${\theta }_1$为$b$和$v_{rot\perp }$的夹角，则：
$$\begin{array}{rl}|b|& =\cos {\theta }_1|v_{rot\perp }|\\ & =\cos (\pi -\theta )|v_{\perp }|\\ & =-\cos \theta |v_{\perp }|\end{array}\text{\hspace{1em}(2.23)}$$

$b$方向
由于$b$的方向与$v_{\perp }$方向相反，可得$b$的单位方向向量为：$$-v_{\perp }/|v_{\perp }|\text{\hspace{1em}(2.24)}$$

综上可得：
$$\begin{array}{rl}b& =(-v_{\perp }/|v_{\perp }|)|b|\\ & =\cos \theta v_{\perp }\end{array}\text{\hspace{1em}(2.25)}$$

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

3.旋转矩阵到向量

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

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

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