三维旋转&坐标系变换

旋转三维向量

图中$\mathbf{u}$为单位向量，表示转轴。将$\mathbf{x}$绕$\mathbf{u}$逆时针旋转角度$\varphi$得到${\mathbf{x}}^{\prime }$。可将$\mathbf{x}$分解为沿转轴的分量${\mathbf{x}}_{\parallel }$和垂直转轴的分量${\mathbf{x}}_{\perp }$。${\mathbf{x}}_{\parallel }$在转动时不变，${\mathbf{x}}_{\perp }$在平面上旋转角度$\varphi$得到${\mathbf{x}}_{\perp }^{\prime }$，可得

$$\begin{array}{}\text{(1)}& {\mathbf{x}}^{\prime }={\mathbf{x}}_{\parallel }+{\mathbf{x}}_{\perp }\cos \varphi +(\mathbf{u}\times \mathbf{x})\sin \varphi \end{array}$$

有

$$\mathbf{u}\times \mathbf{x}=[\mathbf{u}{]}_{\times }\mathbf{x}$$

其中

$$[\mathbf{u}{]}_{\times }\triangleq \left[\begin{array}{ccc}0& -u_3& u_2\\ u_3& 0& -u_1\\ -u_2& u_1& 0\end{array}\right]$$

为反对称矩阵。

那么式$\text{(1)}$可写成：

$$\begin{array}{rl}{\mathbf{x}}^{\prime }& =\mathbf{u}{\mathbf{u}}^T\mathbf{x}+(\mathbf{x}-\mathbf{u}{\mathbf{u}}^T\mathbf{x})\cos \varphi +[\mathbf{u}{]}_{\times }\mathbf{x}\sin \varphi \\ & =((1-\cos \varphi )\mathbf{u}{\mathbf{u}}^{\mathrm{\top }}+\cos \varphi \mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times })\mathbf{x}\end{array}$$

因此旋转作用可用一个矩阵表示。

旋转群

旋转是线性变换，将$\mathbf{v}$变为$r(\mathbf{v})$。其保持：

1. 向量长度
2. 两向量的内积
3. 相对方向 $\mathbf{u}\times \mathbf{v}=\mathbf{w}⟺r(\mathbf{u})\times r(\mathbf{v})=r(\mathbf{w})$

1和2是等价的。1推2可由$\Vert r(\mathbf{u}\mathbf{-}\mathbf{v})\Vert =\Vert \mathbf{u}\mathbf{-}\mathbf{v}\Vert$得到，2推1可由$r(\mathbf{u})\cdot r(\mathbf{u})=\mathbf{u}\cdot \mathbf{u}$得到。

因此可定义旋转群：

$$SO(3):\{{\mathbb{R}}^3\to {\mathbb{R}}^3\mid \mathrm{\forall }\mathbf{v},\mathbf{w}\in {\mathbb{R}}^3,\Vert r(\mathbf{v})\Vert =\Vert \mathbf{v}\Vert ,r(\mathbf{v})\times r(\mathbf{w})=r(\mathbf{v}\times \mathbf{w})\}$$

旋转矩阵

算符$r$是线性的，可用矩阵$R$表示，

$$r(\mathbf{v})=\mathbf{R}\mathbf{v}$$

由$\Vert \mathbf{R}\mathbf{v}\Vert =\Vert \mathbf{v}\Vert$，即

$$(\mathbf{R}\mathbf{v}{)}^{\mathrm{\top }}(\mathbf{R}\mathbf{v})={\mathbf{v}}^{\mathrm{\top }}{\mathbf{R}}^{\mathrm{\top }}\mathbf{R}\mathbf{v}={\mathbf{v}}^{\mathrm{\top }}\mathbf{v}$$

可得：

$${\mathbf{R}}^{\mathrm{\top }}\mathbf{R}=\mathbf{I}$$

即旋转矩阵是正交矩阵。

由旋转性质3，对于向量$\mathbf{u},\mathbf{v},\mathbf{w}$组成的六面体，旋转前后的有向体积应该相等，即

$$\left|\begin{array}{ccc}Ru& Rv& Rw\end{array}\right|=det(R)\left|\begin{array}{ccc}u& v& w\end{array}\right|=\left|\begin{array}{ccc}u& v& w\end{array}\right|$$

得$det(R)=1$。

这就构成了$SO(3)$群(Special Orthogonal group)，其中的special就是指$det(R)=1$。

欧拉定理

是说存在向量$\mathbf{u}$，使得旋转前后不变：

$$\mathbf{R}\mathbf{u}=\mathbf{u}$$

$\mathbf{u}$就是转轴方向。

证明：
只要证明$\mathbf{R}$有为1的特征值即可。

$$\begin{array}{rl}det(\mathbf{R}-\mathbf{I})=det((\mathbf{R}-\mathbf{I}{)}^{\mathsf{T}})& =det({\mathbf{R}}^{\mathsf{T}}-\mathbf{I})=det({\mathbf{R}}^{-1}-{\mathbf{R}}^{-1}\mathbf{R})\\ & =det({\mathbf{R}}^{-1}(\mathbf{I}-\mathbf{R}))=det\left({\mathbf{R}}^{-1}\right)det(-(\mathbf{R}-\mathbf{I}))\\ & =-det(\mathbf{R}-\mathbf{I})⟹det(\mathbf{R}-\mathbf{I})=0.\end{array}$$

指数映射

$$\frac{d}{dt}(R^{T}R)={\dot{R}}^{T}R+R^T\dot{R}=0$$

得：

$$R^T\dot{R}=-(R^T\dot{R}{)}^T$$

即$R^T\dot{R}$是反对称矩阵。三维的反对称矩阵集合用$\mathfrak{s}\mathfrak{o}(3)$表示，称为$SO(3)$的李代数。

那么有

$$R^T\dot{R}=[\mathit{\omega }{]}_{\times }$$

得到：

$$\dot{R}=R[\mathit{\omega }{]}_{\times }$$

当$R=I$时，$\dot{R}=[\mathit{\omega }{]}_{\times }$，即李代数是在幺元处的切空间。

如果$\mathit{\omega }$为常数，上述方程解得：

$$R(t)=R(0)e^{[\mathit{\omega }{]}_{\times }t}$$

其中矩阵的指数按泰勒级数定义。

这称为指数映射：

$$\exp :\mathfrak{s}\mathfrak{o}(3)\to SO(3);[\mathit{\varphi }{]}_{\times }\mapsto \exp ([\mathit{\varphi }{]}_{\times })=e^{[\mathit{\varphi }{]}_{\times }}$$

还可以定义"大写的"指数映射：

$$\text{Exp}:{\mathbb{R}}^3\to SO(3);\mathit{\varphi }\mapsto \text{Exp}(\mathit{\varphi })=e^{[\mathit{\varphi }{]}_{\times }}$$

如果绕转轴$\mathbf{u}$转了角度$\varphi$，那么旋转矩阵：

$$\mathbf{R}=e^{\varphi [\mathbf{u}{]}_{\times }}$$

上式按泰勒展开后得：

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

这就是Rodrigues旋转公式。推导过程用到了

$$[\mathbf{a}{]}_{\times }^2=\mathbf{a}{\mathbf{a}}^{\mathrm{\top }}-{\mathbf{a}}^{\mathrm{\top }}\mathbf{a}\mathbf{I}$$

根据这个式子，又可写成：

$$\begin{array}{}\text{(2)}& \mathbf{R}=\cos \varphi \mathbf{I}+\sin \varphi [\mathbf{u}{]}_{\times }+(1-\cos \varphi )\mathbf{u}{\mathbf{u}}^{\mathrm{\top }}\end{array}$$

这就是式$\text{(1)}$。

从$\mathbf{R}$得到$\varphi$和$\mathbf{u}$

由$\text{(2)}$可得：

$$tr(\mathbf{R})=2\cos \varphi +1$$

即：

$$\varphi =\arccos \left(\frac{tr(\mathbf{R})-1}{2}\right)$$

又有

$$\mathbf{R}-{\mathbf{R}}^T=2\sin \varphi [\mathbf{u}{]}_{\times }$$

因此

$$\mathbf{u}=\frac{(\mathbf{R}-{\mathbf{R}}^T{)}^{\vee }}{2\sin \varphi }$$

其中${\bullet }^{\vee }$是$[\bullet {]}_{\times }$的逆，即$([{\mathbf{v}}_{\times }{]}^{\vee })=\mathbf{v}$.

如果$\sin \varphi =0$，分两种情况：

1. $\cos \varphi =1$，这时候$\mathbf{R}=\mathbf{I}$，转轴未定义
2. $\cos \varphi =-1$，这时$\mathbf{R}+\mathbf{I}=2\mathbf{u}{\mathbf{u}}^T$，这个矩阵的每一列都平行于$\mathbf{u}$，只要对非0的一列归一化即可。这时候$-\mathbf{u}$也满足条件，但是不影响，因为绕$\pm \mathbf{u}$转180度效果是一样的。

四元数

四元数.md

旋转公式为：

$$\begin{array}{}\text{(3)}& {\mathbf{x}}^{\prime }=\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }\end{array}$$

旋转要求

$$\Vert \mathbf{x}\Vert =\Vert {\mathbf{x}}^{\prime }\Vert =\Vert \mathbf{q}{\Vert }^2\Vert \mathbf{x}\Vert$$

因此$\Vert \mathbf{q}{\Vert }^2=1$，即$\mathbf{q}$是单位四元数：

$${\mathbf{q}}^{\ast }\otimes \mathbf{q}=1=\mathbf{q}\otimes {\mathbf{q}}^{\ast }$$

这与$R^{T}R=I=R{R}^T$的条件类似。

还可以看到，自动保持了相对方向：

$$\begin{array}{rl}r(v)\times r(w)& =(q\otimes v\otimes q^{\ast })\times (q\otimes w\otimes q^{\ast })\\ & =\frac{1}{2}((q\otimes v\otimes q^{\ast })\otimes (q\otimes w\otimes q^{\ast })-(q\otimes w\otimes q^{\ast })\otimes (q\otimes v\otimes q^{\ast }))\\ & =\frac{1}{2}(q\otimes v\otimes w\otimes q^{\ast }-q\otimes w\otimes v\otimes q^{\ast })\\ & =\frac{1}{2}(q\otimes (v\otimes w-w\otimes v)\otimes q^{\ast })\\ & =q\otimes (v\times w)\otimes q^{\ast }\\ & =r(v\times w)\end{array}$$

其中第2和第5个等号是因为：

$${\mathbf{p}}_v\otimes {\mathbf{q}}_v-{\mathbf{q}}_v\otimes {\mathbf{p}}_v=2{\mathbf{p}}_v\times {\mathbf{q}}_v$$

指数映射

$$\frac{d(q^{\ast }\otimes q)}{dt}={\dot{q}}^{\ast }\otimes q+q^{\ast }\otimes \dot{q}=0$$

得：

$$q^{\ast }\otimes \dot{q}=-({\dot{q}}^{\ast }\otimes q)=-(q^{\ast }\otimes \dot{q}{)}^{\ast }$$

即$q^{\ast }\otimes \dot{q}$是虚四元数。令：

$$q^{\ast }\otimes \dot{q}=\mathrm{\Omega }$$

两边左乘$q$，得到：

$$\dot{q}=q\otimes \mathrm{\Omega }$$

当$q=1$时，$\dot{q}=\mathrm{\Omega }$，可见虚四元数构成了单位四元数球$S^3$的切空间。

如果$\mathrm{\Omega }$为常数，上式解得$q(t)=q(0)\otimes e^{\mathrm{\Omega }t}$（可代回验证），这就引出了指数映射。

如果绕转轴$\mathbf{u}$转了角度$\varphi$，定义“大写的”指数映射：

$$\begin{array}{}\text{(4)}& \mathbf{q}\triangleq \text{Exp}(\varphi \mathbf{u})=e^{\varphi \mathbf{u}/2}=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}\end{array}$$

旋转作用

将式$\text{(4)}$代入式$\text{(3)}$， 推导可得式$\text{(1)}$，这就验证了$\text{(3)}$的正确性。

在证明中有一步$\mathbf{u}\otimes \mathbf{x}\otimes \mathbf{u}=\mathbf{x}({\mathbf{u}}^T\mathbf{u})-2\mathbf{u}({\mathbf{u}}^T\mathbf{x})$用到了$(a\times b)\times c=-a(c\cdot b)+b(c\cdot a)$

四元数到旋转矩阵的转换

由

$$\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }=[{\mathbf{q}}^{\ast }{]}_R[\mathbf{q}{]}_L\left[\begin{array}{c}0\\ \mathbf{x}\end{array}\right]=\left[\begin{array}{c}0\\ \mathbf{R}\mathbf{x}\end{array}\right]$$

可以得到：

$$\mathbf{R}=(q_w^2-{\mathbf{q}}_v^{\mathrm{\top }}{\mathbf{q}}_v)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\mathrm{\top }}+2q_w[{\mathbf{q}}_v{]}_{\times }$$

旋转合成

假设旋转2作用于旋转1之后。

对于旋转矩阵，

$${\mathbf{R}}_2({\mathbf{R}}_1\mathbf{x})=({\mathbf{R}}_2{\mathbf{R}}_1)\mathbf{x}$$

对于四元数，

$${\mathbf{q}}_2\otimes ({\mathbf{q}}_1\otimes \mathbf{x}\otimes {\mathbf{q}}_1^{\ast })\otimes {\mathbf{q}}_2^{\ast }=({\mathbf{q}}_2\otimes {\mathbf{q}}_1)\otimes \mathbf{x}\otimes ({\mathbf{q}}_2\otimes {\mathbf{q}}_1{)}^{\ast }$$

因此，后旋转的都是乘在左边。

---

坐标系变换

方向余弦矩阵（DCM）

用G表示global，或者n系；L表示local，或者b系。

${\mathbf{r}}_G,{\mathbf{r}}_L$分别为向量$\mathbf{r}$在坐标系$G，L$中的坐标，由

$$\mathbf{r}=\left(\begin{array}{ccc}{\mathbf{i}}_G& {\mathbf{j}}_G& {\mathbf{k}}_G\end{array}\right){\mathbf{r}}_G=\left(\begin{array}{ccc}{\mathbf{i}}_L& {\mathbf{j}}_L& {\mathbf{k}}_L\end{array}\right){\mathbf{r}}_L$$

得：

$$\begin{array}{rl}{\mathbf{r}}_G& =\left(\begin{array}{c}{\mathbf{i}}_G^{⊺}\\ {\mathbf{j}}_G^{⊺}\\ {\mathbf{k}}_G^{⊺}\end{array}\right)\left(\begin{array}{ccc}{\mathbf{i}}_L& {\mathbf{j}}_L& {\mathbf{k}}_L\end{array}\right){\mathbf{r}}_L\\ & \triangleq R_{GL}{\mathbf{r}}_L\end{array}$$

表示从$L$到$G$的坐标变换。

由$R_{GL}$的定义还可得基变换：

$$\left(\begin{array}{ccc}{\mathbf{i}}_L& {\mathbf{j}}_L& {\mathbf{k}}_L\end{array}\right)=\left(\begin{array}{ccc}{\mathbf{i}}_G& {\mathbf{j}}_G& {\mathbf{k}}_G\end{array}\right)R_{GL}$$

对于一个与$L$系固连的向量$\mathbf{r}$，经过主动旋转$R$，得到${\mathbf{r}}^{\prime }$，那么：

$${\mathbf{r}}_G^{\prime }=R{\mathbf{r}}_G$$

而${\mathbf{r}}_L^{\prime }={\mathbf{r}}_G$，因此

$${\mathbf{r}}_G^{\prime }=R{\mathbf{r}}_L^{\prime }$$

而按定义：

$${\mathbf{r}}_G^{\prime }=R_{GL}{\mathbf{r}}_L^{\prime }$$

因此坐标变换矩阵与主动旋转矩阵$R$的关系是：

$$R_{GL}=R$$

欧拉角

intrinsic rotation是指绕当前坐标系（而不是某个固定坐标系）的轴转动。

$G$绕某轴逆时针旋转$\theta$，得到$L$。对于$R_{LG}$，根据定义得出如下结果1：

绕$X$轴旋转

$$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& c_{\theta }& s_{\theta }\\ 0& -s_{\theta }& c_{\theta }\end{array}\right]$$

绕$Y$轴旋转

$$R_y=\left[\begin{array}{ccc}{c}_{\theta }& 0& -s_{\theta }\\ 0& 1& 0\\ s_{\theta }& 0& c_{\theta }\end{array}\right]$$

绕$Z$轴旋转

$$R_z=\left[\begin{array}{ccc}{c}_{\theta }& s_{\theta }& 0\\ -s_{\theta }& c_{\theta }& 0\\ 0& 0& 1\end{array}\right]$$

如果按$Z\to Y\to X$的顺序转动，

$$R_{LG}=R_x{R}_y{R}_z=\left[\begin{array}{ccc}{c}_y{c}_z& c_y{s}_z& -s_y\\ s_x{s}_y{c}_z-c_x{s}_z& s_x{s}_y{s}_z+c_x{c}_z& s_x{c}_y\\ c_x{s}_y{c}_z+s_x{s}_z& c_x{s}_y{s}_z-s_x{c}_z& c_x{c}_y\end{array}\right]$$

可得欧拉角和DCM的转换关系：

$$\begin{gathered} \text{tan}({\theta }_z)=\frac{R_{12}}{R_{11}} \\ \text{sin}({\theta }_y)=-R_{13} \\ \text{tan}({\theta }_x)=\frac{R_{23}}{R_{33}} \end{gathered}$$

若是小角度转动，

$$\begin{array}{}\text{(5)}& R=\left[\begin{array}{ccc}1& {\theta }_z& -{\theta }_y\\ -{\theta }_z& 1& {\theta }_x\\ {\theta }_y& -{\theta }_x& 1\end{array}\right]=I-[\mathit{\theta }{]}_{\times }\end{array}$$

可见小角度转动与转动顺序无关（即只与绕XYZ各轴转过的角度有关。如果只有俩轴，比如Z->X->Z，那么未出现的轴角度为0，出现两次的轴的角度相加），等效于一次转动：角度为$\theta =\sqrt{{\theta }_x^2+{\theta }_y^2+{\theta }_z^2}$，转轴为$\frac{1}{\theta }\left(\begin{array}{c}{\theta }_x\\ {\theta }_y\\ {\theta }_z\end{array}\right)$，也可以从Rodrigues旋转公式做小角近似看出来：

$${\mathbf{R}}_{LG}={\mathbf{R}}^T\approx \mathbf{I}-\varphi [\mathbf{u}{]}_{\times }$$

Extrinsic rotation

设固定坐标系为$A$。坐标系$B$绕$A$的$\mathbf{u}$轴转动角度$\varphi$，这个转动用$R_{\varphi \mathbf{u}}$表示，得坐标系$C$。求$R_{CA}$。

首先

$$R_{CA}=R_{BD}$$

其中$D$为$A$绕$\mathbf{u}$轴转动角度$-\varphi$得到。这个结论对于二维转动很直观，对于三维其实也容易看出。按定义，只要证明两对坐标系的基的内积相等即可。把基分解到平行于转轴和垂直于转轴。平行部分的内积显然不变，垂直部分由二维情况可知也不变，且平行于垂直部分内积为0。因此得证。

$$R_{BD}=R_{BA}{R}_{AD}=R_{BA}{R}_{DA}^{-1}=R_{BA}{R}_{-\varphi \mathbf{u}}^{-1}=R_{BA}{R}_{\varphi \mathbf{u}}$$

因此得到结论，绕固定坐标系的轴转动，$R$是乘在右边的。

四元数

和DCM类比有

$$\begin{array}{}\text{(6)}& {\mathbf{x}}_G={\mathbf{q}}_{GL}\otimes {\mathbf{x}}_L\otimes {\mathbf{q}}_{GL}^{\ast }\end{array}$$

其中${\mathbf{q}}_{GL}$等于主动旋转$\mathbf{q}$。

运动方程

推导的要点是从当前$L$系做扰动得到新的$L$系，而陀螺仪的测量值刚好是在角速度在$L$系的值。

四元数

由$\text{(6)}$可得，${\mathbf{q}}_{G{L}_2}={\mathbf{q}}_{G{L}_1}{\mathbf{q}}_{L_1{L}_2}$，即扰动是乘在右边的（L表示local）：

$$q(t+\mathrm{\Delta }t)=q(t)\otimes \mathrm{\Delta }{q}_L=q(t)\otimes \text{Exp}(\mathrm{\Delta }{\mathit{\varphi }}_L)$$

$$\begin{array}{rl}\dot{q}& \triangleq \underset{\mathrm{\Delta }t\to 0}{lim}\frac{q(t+\mathrm{\Delta }t)-q(t)}{\mathrm{\Delta }t}\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes \mathrm{\Delta }q-q}{\mathrm{\Delta }t}\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes (\left[\begin{array}{c}\cos \frac{\mathrm{\Delta }\varphi }{2}\\ \mathbf{u}\sin \frac{\mathrm{\Delta }\varphi }{2}\end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right])}{\mathrm{\Delta }t}\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes (\left[\begin{array}{c}1\\ \mathrm{\Delta }\mathit{\varphi }/2\end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right])}{\mathrm{\Delta }t}(小角近似)\\ & =\underset{\mathrm{\Delta }t\to 0}{lim}\frac{q\otimes \left[\begin{array}{c}0\\ \mathrm{\Delta }\mathit{\varphi }/2\end{array}\right]}{\mathrm{\Delta }t}\\ & =\frac{1}{2}q\otimes {\mathit{\omega }}_L\end{array}$$

方向余弦矩阵

由$\text{(5)}$得：

$$R_{L_2{L}_1}=I-[\mathit{\theta }{]}_{\times }$$

因此

$$R_{L_1{L}_2}=R_{L_2{L}_1}^T=I+[\mathit{\theta }{]}_{\times }$$

由$R_{G{L}_2}=R_{GL1}{R}_{L_1{L}_2}$，推导可得：

$${\dot{R}}_{GL}=R_{GL}[{\mathit{\omega }}_L{]}_{\times }$$

参考资料

Quaternion kinematics for the error-state Kalman filter

---

1. $s_{\theta }$放置位置记忆方法：放在转动轴的上一列（循环）。 ↩