坐标变换(5)—用旋转轴和旋转角表示旋转

任何旋转，都可以用一个旋转轴$\hat{\omega }$和一个旋转角$\theta$来描述。

1. 坐标系的线速度和角速度

如上图，在旋转的刚体上，附加一个body frame { b } \{b\} {b}，记为 { x ^ , y ^ , z ^ } \{\hat{x},\hat{y},\hat{z}\} {x^,y^​,z^}。对于三个轴而言，绕着$\hat{\omega }$旋转的轨迹为圆。当然，上述坐标轴 { x ^ , y ^ , z ^ } \{\hat{x},\hat{y},\hat{z}\} {x^,y^​,z^}和$\hat{\omega }$是在fixed frame { S } \{S\} {S}坐标系下的，下面将$\hat{\omega }$记为${\hat{\omega }}_s$，

绕着轴$\hat{\omega }$的角速度为，
$$w_s=\hat{w}\dot{\theta }\text{\hspace{1em}(1)}$$
运动的线速度记为$\dot{\hat{x}}$，三个轴的线速度则为，
$$\begin{array}{rl}\dot{\hat{x}}& =w_s\times \hat{x}\\ \dot{\hat{y}}& =w_s\times \hat{y}\\ \dot{\hat{z}}& =w_s\times \hat{z}\end{array}\text{\hspace{1em}(2)}$$
将三个轴的线速度统一写为，
$$\dot{R}=\left[\begin{array}{ccc}{w}_s\times \hat{x}& w_s\times \hat{y}& w_s\times \hat{z}\end{array}\right]=w_s\times R\text{\hspace{1em}(3)}$$
为了简化公式(3)中的叉乘，特引入了$[]$符号，将$w\times R$可以记为矩阵的乘法$[w]R$，其中$[w]$的定义如下：
对于${\mathbb{R}}^3$中的向量$x=\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]$,定义$[x]$为一个反对称矩阵，
$$[x]=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right]\text{\hspace{1em}(4)}$$
$$[x]=-[x{]}^T\text{\hspace{1em}(5)}$$
上述所有$3\times 3$的反对称矩阵统称为$so(3)$，小的。前面说过，旋转矩阵属于$SO(3)$，大的。下面有一个两者结合起来有趣的性质，假定$r_i^T$为$R$的第$i$行，即$r_i$是$R^T$的第$i$列，则
$$\begin{array}{rl}R[w]R^T& =R\left[\begin{array}{ccc}{w}_s\times r_1& w_s\times r_2& w_s\times r_3\end{array}\right]\\ & =\left[\begin{array}{ccc}{r}_1^T(w_s\times r_1)& r_1^T(w_s\times r_2)& r_1^T(w_s\times r_3)\\ r_2^T(w_s\times r_1)& r_2^T(w_s\times r_2)& r_2^T(w_s\times r_3)\\ r_3^T(w_s\times r_1)& r_3^T(w_s\times r_2)& r_3^T(w_s\times r_3)\end{array}\right]\\ & =\left[\begin{array}{ccc}0& -r_3^T{w}_s& r_2^T{w}_s\\ r_3^T{w}_s& 0& -r_1^T{w}_s\\ -r_2^T{w}_s& r_1^T{w}_s& 0\end{array}\right]\\ & =[R{w}_s]\end{array}\text{\hspace{1em}(6)}$$

对于(6)中矩阵中的$r_1^T(w_s\times r_2)$，是三个向量$r_1,w_s,r_2$的混合积，也就是三个向量组成的六面体的体积，而我们知道矩阵的行列式的值的物理意义就是体积。根据下面的混合积的图，很容易得到矩阵中对应元素的反对称的关系。

下面我们将三个轴的线速度表示为$[w]$的写法，
$$\dot{R}=[w_s]R\text{\hspace{1em}(7)}$$

$$[w_s]=\dot{R}{R}^{-1}\text{\hspace{1em}(8)}$$
前面我们提到的所有的向量和$R$都是在fixed frame { S } \{S\} {S}下描述的，下面我们将$w_s$在body frame { b } \{b\} {b}下进行描述，易得，
$$w_s=R_{sb}{w}_b\text{\hspace{1em}(9)}$$
则旋转轴在body frame { b } \{b\} {b}下，
$$w_b=R_{sb}^{-1}{w}_s=R^{-1}{w}_s=R^T{w}_s\text{\hspace{1em}(10)}$$
因此可以得到，
$$\begin{array}{rl}[w_b]& =[R^T{w}_s]\\ & =R^T[w_s]R\\ & =R^T(\dot{R}{R}^T)R\\ & =R^T\dot{R}\\ & =R^{-1}\dot{R}\end{array}\text{\hspace{1em}(11)}$$

需要注意的是$w_b$是在body frame { b } \{b\} {b}下的描述，所以它描述的角速度不是一个旋转的坐标系的角速度(例如 { b } \{b\} {b}相对于 { S } \{S\} {S}旋转)，而是在某一瞬时，$w_b$相对于body frame { b } \{b\} {b}的旋转。

2. 微分方程的解

给定下面一个简单的线性微分方程，其中$x(t)\in \mathbb{R}$，$a\in \mathbb{R}$，初始状态$x(t)=x(0)$，
$$\dot{x}(t)=ax(t)\text{\hspace{1em}(12)}$$
易得上述的解为，
$$x(t)=e^{at}x(0)\text{\hspace{1em}(13)}$$
对$e^{at}$在$0$附近进行泰勒展开，可得，
$$e^{at}=1+at+\frac{(at{)}^2}{2!}+\frac{(at{)}^3}{3!}+\cdots \text{\hspace{1em}(14)}$$
同理，当$a$为矩阵$A$时，$x(t)$为列向量，
$$\dot{x}(t)=Ax(t)\text{\hspace{1em}(15)}$$
可得解为,
$$x(t)=e^{At}x(0)\text{\hspace{1em}(16)}$$
其中，
$$e^{At}=1+At+\frac{(At{)}^2}{2!}+\frac{(At{)}^3}{3!}+\cdots \text{\hspace{1em}(17)}$$

3. 指数形式的旋转

任何旋转，都可以用一个旋转轴$\hat{\omega }$和一个旋转角$\theta$来描述。其中$\hat{\omega }\in {\mathbb{R}}^3,\Vert \hat{\omega }\Vert =1$，$\theta \in {\mathbb{R}}^3$。
下面我们来分析如何利用一根旋转轴和旋转角来描述旋转，

假设向量$p(t)$从$p(0)$绕着$\hat{\omega }$以恒定的角速度$1rad/s$旋转了$\theta$秒，最终到$p(\theta )$，定义$p(t)$间断的线速度为，
$$\dot{p}=\hat{\omega }\times p\text{\hspace{1em}(18)}$$
由前面的分析，引入$[\hat{\omega }]$，则
$$\dot{p}=[\hat{\omega }]p\text{\hspace{1em}(19)}$$
该微分方程如前面介绍为，
$$p(t)=e^{[\hat{\omega }]t}p(0)\text{\hspace{1em}(20)}$$

则，
$$p(\theta )=e^{[\hat{\omega }]\theta }p(0)\text{\hspace{1em}(21)}$$

容易得到$[\hat{\omega }]$两个计算性质，如下，
$$[\hat{\omega }][\hat{\omega }]=\hat{\omega }{\hat{\omega }}^T-I\text{\hspace{1em}(22)}$$

$$[\hat{\omega }][\hat{\omega }][\hat{\omega }]=[\hat{\omega }{]}^3=-[\hat{\omega }]\text{\hspace{1em}(23)}$$
所以公式21可以化简为，
$$\begin{array}{rl}{e}^{[\hat{\omega }]\theta }& =I+[\hat{\omega }]\theta +[\hat{\omega }{]}^2\frac{{\theta }^2}{2!}+[\hat{\omega }{]}^3\frac{{\theta }^3}{3!}+\cdots \\ & =I+\left(\theta -\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\cdots \right)[\hat{\omega }]+\left(\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\frac{{\theta }^6}{6!}-\cdots \right)[\hat{\omega }{]}^2\\ & =I+sin(\theta )[\hat{\omega }]+(1-cos(\theta ))[\hat{\omega }{]}^2\end{array}\text{\hspace{1em}(24)}$$
上式就是著名的罗德里格斯公式，即指数形式的旋转，
$$\mathrm{Rot}(\hat{\omega },\theta )=e^{[\hat{\omega }]\theta }=I+\sin \theta [\hat{\omega }]+(1-\cos \theta )[\hat{\omega }{]}^2\in SO(3)\text{\hspace{1em}(25)}$$
经过指数映射，将$so(3)$和旋转的角度$\theta$通过指数映射为$SO(3)$，即三维的旋转矩阵。

在前面文章中介绍过，旋转矩阵左乘和右乘的区别，这里也是类似的，假设body frame { b } \{b\} {b}在fixed frame { S } \{S\} {S}中的描述为$R_{sb}$，则$\mathrm{Rot}(\hat{\omega },\theta )R_{sb}$，左乘，表示将$R_{sb}$顺着 { S } \{S\} {S}中的$\hat{\omega }$旋转$\theta$。而$R_{sb}\mathrm{Rot}(\hat{\omega },\theta )$，右乘，表示将$R_{sb}$顺着 { b } \{b\} {b}中的$\hat{\omega }$旋转$\theta$。

4. 旋转矩阵的对数

上面描述的是从$\hat{\omega }\theta$到旋转矩阵$R$的过程，下面介绍从旋转矩阵$R$求$\hat{\omega }\theta$的过程，也就是求得旋转向量和具体的旋转角度，求$R$矩阵的“对数”。可以将两个对应的过程描述成下面的形式，

$$\begin{array}{cllc}\exp :& [\hat{\omega }]\theta \in so(3)& \to & R\in SO(3)\\ \log :& R\in SO(3)& \to & [\hat{\omega }]\theta \in \mathrm{so}(3)\end{array}$$

下面将公式(25)展开，如下，
$$\left[\begin{array}{ccc}{c}_{\theta }+{\hat{\omega }}_1^2\left(1-c_{\theta }\right)& {\hat{\omega }}_1{\hat{\omega }}_2\left(1-c_{\theta }\right)-{\hat{\omega }}_3{\mathrm{s}}_{\theta }& {\hat{\omega }}_1{\hat{\omega }}_3\left(1-c_{\theta }\right)+{\hat{\omega }}_2{\mathrm{s}}_{\theta }\\ {\hat{\omega }}_1{\hat{\omega }}_2\left(1-{\mathrm{c}}_{\theta }\right)+{\hat{\omega }}_3{\mathrm{S}}_{\theta }& c_{\theta }+{\hat{\omega }}_2^2\left(1-c_{\theta }\right)& {\hat{\omega }}_2{\hat{\omega }}_3\left(1-{\mathrm{c}}_{\theta }\right)-{\hat{\omega }}_1{\mathrm{s}}_{\theta }\\ {\hat{\omega }}_1{\hat{\omega }}_3\left(1-{\mathrm{c}}_{\theta }\right)-{\hat{\omega }}_2{\mathrm{S}}_{\theta }& {\hat{\omega }}_2{\hat{\omega }}_3\left(1-{\mathrm{c}}_{\theta }\right)+{\hat{\omega }}_1{\mathrm{s}}_{\theta }& {\mathrm{c}}_{\theta }+{\hat{\omega }}_3^2\left(1-{\mathrm{c}}_{\theta }\right)\end{array}\right]\text{\hspace{1em}(26)}$$
其中，$\hat{\omega }=\left[\begin{array}{c}\widehat{w_1}\\ \widehat{w_2}\\ \widehat{w_3}\end{array}\right]$，${\mathrm{c}}_{\theta }=cos(\theta )$，${\mathrm{s}}_{\theta }=sin(\theta )$。
记旋转矩阵$R$为$r_{ij}$，则可以得到，
$$\begin{array}{rl}{r}_{32}-r_{23}& =2{\hat{\omega }}_{1}sin(\theta )\\ r_{13}-r_{31}& =2{\hat{\omega }}_{2}sin(\theta )\\ r_{21}-r_{12}& =2{\hat{\omega }}_{3}sin(\theta )\end{array}\text{\hspace{1em}(27)}$$
上式在$sin(\theta )\ne 0$的情况下，可以得到，
$$\begin{array}{r}{\hat{\omega }}_1=\frac{1}{2sin(\theta )}(r_{32}-r_{23})\\ {\hat{\omega }}_2=\frac{1}{2sin(\theta )}(r_{13}-r_{31})\\ {\hat{\omega }}_3=\frac{1}{2sin(\theta )}(r_{21}-r_{12})\end{array}\text{\hspace{1em}(28)}$$
上式也可以写成，
$$[\hat{\omega }]=\left[\begin{array}{ccc}0& -{\hat{\omega }}_3& {\hat{\omega }}_2\\ {\hat{\omega }}_3& 0& -{\hat{\omega }}_1\\ -{\hat{\omega }}_2& {\hat{\omega }}_1& 0\end{array}\right]=\frac{1}{2\sin \theta }\left(R-R^{\mathrm{T}}\right)\text{\hspace{1em}(29)}$$

此外，由式(26)可以得到另外一个计算$\theta$的公式，
$$\mathrm{tr}R=r_{11}+r_{22}+r_{33}=1+2\cos \theta \text{\hspace{1em}(30)}$$
至此，$sin(\theta )\ne 0$的情况下，利用旋转矩阵$R$，我们计算出了$\hat{\omega }$和$\theta$。接下来讨论$sin(\theta )=0$的情况：

1. 当$\theta =k\pi$，且$k$是偶数的情况下，此时相当于没有旋转，回到了原位置，$R=I$；
2. 当$\theta =k\pi$，且$k$是奇数的情况下，此时有，
   $$R=e^{[\hat{\omega }]\pi }=I+2[\hat{\omega }{]}^2\text{\hspace{1em}(31)}$$
   因为式(31)三个矩阵都是对角矩阵，所以可以得到下面的结果(利用$R$对角元素)
   $${\hat{\omega }}_i=\pm \sqrt{\frac{r_{ii}+1}{2}},i=1,2,3\text{\hspace{1em}(32)}$$
   利用$R$非对角元素，可得，
   $$\begin{array}{rl}2{\hat{\omega }}_1{\hat{\omega }}_2& =r_{12}\\ 2{\hat{\omega }}_2{\hat{\omega }}_3& =r_{23}\\ 2{\hat{\omega }}_1{\hat{\omega }}_3& =r_{13}\end{array}\text{\hspace{1em}(33)}$$
   利用式(32)和式(33)我们就能计算出$\hat{\omega }$，同时此时旋转的角为$\theta =\pm \pi ,\pm 3\pi ,\dots$。
   从上面的计算过程很容易看出来，旋转角度是以$2\pi$为周期，其实也是符合物理意义的，旋转$\pi$和旋转$3\pi$的效果是一样的，因此我们可以将旋转的角度限定在$[-\pi ,\pi ]$。此时计算的$\hat{\omega }\theta$的长度是$\le \pi$的。因此我们可以把$SO(3)$想象为一个半径为$\pi$的实心球，如下图所示，

当给定球中的一点$r\in {\mathbb{R}}^3$，我们可以将$\hat{\omega }=\frac{r}{\Vert r\Vert }$作为单位长度的旋转轴，$\Vert r\Vert$作为$\theta$。和$r$相对应的旋转矩阵$R$可以被看作是绕着$\hat{\omega }$旋转了$\theta$角。对于$R\in SO(3)$，同时$trR\ne -1$，此时在实心球中总能找到一个唯一的$r$，使得$e^{[r]}=R$。当$trR=-1$时，此时$\Vert r\Vert =\pi$，在实心球的表面有一对正好相反的一对点，两者的效果是一样的，$r$和$-r$都对应了同一个$R$。