惯性导航解算

惯性导航解算

一、惯性导航解算的目的

从IMU原始的角速度（陀螺仪采集）和加速度（加速度计采集）数据得到它的导航结果（位置、速度、姿态）。

1. 姿态描述

在导航定位领域，描述一个物体的姿态，常用的描述有欧拉角、旋转矩阵、四元数，这三个概念的理解难度逐渐递增。另外，还有一种表示方式，是旋转向量，它主要作为运算过程中的一个工具，而一般不用来对外作为姿态的描述输出

1.1 旋转矩阵

1) 内积

$$a.b=a^{T}b=\sum _{i=1}^3=|a||b|\cos <a,b>$$

内积可以描述向量间的投影关系。

2) 外积

$$a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_2{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b=a^{\wedge }b$$

外积的方向垂直于这两个向量,大小为$|a||b|sin⟨a,b⟩$,是两个向量张成的四边形的有向面积。对于外积,我们引入了${}^{\wedge }$符号，把 a 写成一个矩阵。事实上是一个反对称矩阵，你可以将${}^{\wedge }$记成一个反对称符号。这样就把外积$a\times b$,写成了矩阵与向量的乘法 $a^{\wedge }b$，把它变成了线性运算。

3) 旋转矩阵定义

旋转矩阵是一个非常常见的姿态描述和计算手段了，因为它可以很方便地对坐标系中的矢量进行计算。

假设旋转前，载体系(b系)的单位正交基为 $\left(\mathbf{e}{1}, \mathbf{e}{2}, \mathbf{e}_{3}\right) $，旋转后对应的单位正交基为 $\left({\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right)$，在世界坐标系(w系，不随载体的旋转而旋转)下有向量 $\mathbf{a}$ ，它在旋转前后两个坐标系中的坐标分别为${\left[a_1,a_2,a_3\right]}^{\mathrm{T}}$和 ${\left[a_1^{\prime },a_2^{\prime },a_3^{\prime }\right]}^{\mathrm{T}}$ ，那么有
$$\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
由此可以得到
$$\mathbf{a}=\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{lll}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{l}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\stackrel{\text{ def }}{=}{\mathbf{Ra}}^{\prime }$$
其中$\mathbf{R}$ 就是旋转矩阵，带上坐标系下标，可以写为${\mathbf{R}}_{wb}$，表示是b系到w系的旋转矩阵。

4) 旋转矩阵性质

旋转矩阵的主要特性是单位正交矩阵，即
$${\mathbf{R}}^{-1}={\mathbf{R}}^T$$

5) 旋转矩阵的微分方程

假设世界坐标系(w系)中有一个固定不动的矢量 ${\mathbf{r}}^w$ ，它在载体坐标系(b系)下的表示为${\mathbf{r}}^b$ ，则有
$${\mathbf{r}}^w={\mathbf{R}}_{wb}{\mathbf{r}}^b$$
两边同时微分，得到
$${\dot{\mathbf{r}}}^w={\mathbf{R}}_{wb}{\dot{\mathbf{r}}}^b+{\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$
由于${\dot{\mathbf{r}}}^w=0$（固定矢量求导为零，${\dot{\mathbf{r}}}^b=-{\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b$ （其中${\mathbf{\omega }}_{wb}^b$ 代表载体旋转角速度在b系下的表示，实际使用时，指的就是陀螺仪的角速度输出，且暂不考虑误差），因此有
$$0={\mathbf{R}}_{wb}\left(-{\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b\right)+{\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$
移项可得
$${\mathbf{R}}_{wb}\left({\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b\right)={\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$
即
$${\mathbf{R}}_{wb}\left({\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b\right)={\dot{\mathbf{R}}}_{wb}{\mathbf{r}}^b$$
因此有
$${\dot{\mathbf{R}}}_{wb}={\mathbf{R}}_{wb}{\left[{\mathbf{\omega }}_{wb}^b\right]}_{\times }$$

$${\left[{\mathbf{\omega }}_{wb}^b\right]}_{\times }=\left[\begin{array}{ccc}0& -w_3& w_2\\ w_3& 0& -w_1\\ -w_2& w_1& 0\end{array}\right]$$

1.2 四元数

四元数是超复数,即“复数的复数”
若有复数
$$\begin{gathered} \mathbf{A}=a+b\mathbf{i} \\ \mathbf{B}=c+d\mathbf{i} \end{gathered}$$
则复数的复数为:
$$\begin{array}{rl}\mathbf{Q}& =\mathbf{A}+\mathbf{Bj}\\ & =(a+b\mathbf{i})+(c+d\mathbf{i})\mathbf{j}\\ & =a+b\mathbf{i}+c\mathbf{j}+d\mathbf{ij}\end{array}$$
在这里，另 $\mathbf{k}=\mathbf{ij}$，则有
$$\begin{array}{r}\mathbf{Q}=a+b\mathbf{i}+c\mathbf{j}+d\mathbf{k}\end{array}$$
因此，从$\mathbf{Q}=\mathbf{A}+\mathbf{Bj}$可以看出，四元数可以理解为“复数的复数”。

一个四元数 q 拥有一个实部和三个虚部。这里把实部写在前面(也有地方把实部写在
后面)
$$\mathbf{q}=q_w+{\mathbf{q}}_v=q_w+q_x\mathbf{i}+q_y\mathbf{j}+q_z\mathbf{k}$$
其中$q_w$是实部，${\mathbf{q}}_v$是虚部。

其中 i, j, k 为四元数的三个虚部。这三个虚部满足关系式:
$$\begin{gathered} i^2=j^2=k^2=-1 \\ ij=k,ji=-k \\ jk=i,kj=-i \\ ki=j,ik=-j \end{gathered}$$

1) 单位四元数

在姿态表示中，我们所用的四元数都是单位四元数，即
$$||\mathbf{q}||=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2}=1$$

2) 四元数加减法

四元数$p,q$的加减运算为:
$$p+q=[p_w\pm q_w,p_v\pm q_v]$$

3) 共轭四元数与四元数的逆

共轭四元数指的是实部相同，虚部相反的四元数
$$\begin{gathered} {\mathbf{q}}^{\ast }=q_w-{\mathbf{q}}_v=q_w-q_x\mathbf{i}-q_y\mathbf{j}-q_z\mathbf{k} \\ {\mathbf{q}}^{\ast }\mathbf{q}=1 \end{gathered}$$
一个四元数的逆为:
$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }/\Vert \mathbf{q}\Vert$$
按此定义,四元数和自己的逆的乘积为实四元数的 1:
$$\mathbf{q}{\mathbf{q}}^{-1}=\mathbf{q}{\mathbf{q}}^{-1}=1$$
如果 $q$ 为单位四元数,逆和共轭就是同一个量。同时,乘积的逆有和矩阵相似的性
质:
$$\begin{gathered} {\mathbf{q}}^{-1}={\mathbf{q}}^{\ast } \\ ({\mathbf{q}}_a{\mathbf{q}}_b{)}^{-1}={\mathbf{q}}_b^{-1}{\mathbf{q}}_a^{-1} \end{gathered}$$

4) 四元数的旋转

假设一个空间三维点 $\mathbf{p}=\left[\begin{array}{ccc}x& y& z\end{array}\right]\in {\mathbb{R}}^3$ ,以及一个由轴角 $n,\theta$ 指定的旋转。三维点$p$ 经过旋转之后变成为 $p^{\prime }$ 。如果使用矩阵描述,那么有$p^{\prime }=Rp$。如果用四元数描述旋转,则如下：

首先,把三维空间点用一个虚四元数来描述:
$$\mathbf{p}=\left[\begin{array}{cccc}0& x& y& z\end{array}\right]=\left[\begin{array}{cc}0& v\end{array}\right]$$

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后 ，用四元数$q$表示这个旋转:
$$\mathbf{q}=\left[\begin{array}{cc}\cos \frac{\theta }{2}& n\sin \frac{\theta }{2}\end{array}\right]$$

那么,旋转后的点 $p^{\prime }$即可表示为这样的乘积:
$${\mathbf{p}}^{\prime }=\mathbf{q}\otimes \mathbf{p}\otimes {\mathbf{q}}^{\ast }$$

4) 四元数乘法

乘法是把 $p$的每一项与 $q$ 每项相乘,最后相加,虚部要虚部满足关系式进行整理，
可得:
$$\begin{gathered} p=p_w+p_{x}i+p_{y}j+p_{z}k \\ q=q_w+q_{x}i+q_{y}j+q_{z}k \\ p\otimes q=p_w{q}_w-p_x{q}_x-p_y{q}_y-p_z{q}_z \\ +(p_w{q}_x+p_x{q}_w+p_y{q}_z-p_z{q}_y)i \\ +(p_q{q}_y+p_y{q}_w+p_z{q}_x-p_x{q}_z)j \\ +(p_w{q}_z+p_z{q}_w+p_x{q}_y-p_y{q}_x)k \end{gathered}$$
虽然稍为复杂,但形式上是整齐有序的。如果写成向量形式并利用内外积运算,该表
达会更加简洁:
$$\mathbf{p}\otimes \mathbf{q}=\left[\begin{array}{c}{p}_w{q}_w-{\mathbf{p}}_v^{\mathrm{T}}{\mathbf{q}}_v\\ p_w{\mathbf{q}}_v+q_w{\mathbf{p}}_v+{\mathbf{p}}_v\times {\mathbf{q}}_v\end{array}\right]$$
如果你把上面的式子展开，会发现四元数乘法可以表示成矩阵与向量相乘的形式
$$\mathbf{p}\otimes \mathbf{q}=[\mathbf{p}{]}_L\mathbf{q}$$
其中
$$\begin{array}{rl}[\mathbf{p}{]}_L& =\left[\begin{array}{cccc}{p}_w& -p_x& -p_y& -p_z\\ p_x& p_w& -p_z& p_y\\ p_y& p_z& p_w& -p_x\\ p_z& -p_y& p_x& p_w\end{array}\right]\\ & =p_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{p}}_v^{\mathrm{T}}\\ {\mathbf{p}}_v& {\left[{\mathbf{p}}_v\right]}_{\times }\end{array}\right]\end{array}$$
其中$[\cdot {]}_{\times }$表示向量的反对称矩阵。

四元数乘法也可以展开为
$$\mathbf{p}\otimes \mathbf{q}=[\mathbf{q}{]}_R\mathbf{p}$$
其中
$$\begin{array}{rl}[\mathbf{q}{]}_R& =\left[\begin{array}{cccc}{q}_w& -q_x& -q_y& -q_z\\ q_x& q_w& q_z& -q_y\\ q_y& -q_z& q_w& q_x\\ q_z& q_y& -q_x& q_w\end{array}\right]\\ & =q_w\mathbf{I}+\left[\begin{array}{cc}0& -{\mathbf{q}}_v^{\mathrm{T}}\\ {\mathbf{q}}_v& -{\left[{\mathbf{q}}_v\right]}_{\times }\end{array}\right]\end{array}$$
因此可以得到
$$\begin{array}{l}(\mathbf{q}\otimes \mathbf{x})\otimes \mathbf{p}=[\mathbf{p}{]}_R[\mathbf{q}{]}_L\mathbf{x}\\ \mathbf{q}\otimes (\mathbf{x}\otimes \mathbf{p})=[\mathbf{q}{]}_L[\mathbf{p}{]}_R\mathbf{x}\end{array}$$
这是两个很重要的性质，后面的推导中会多次用到。

5) 四元数的微分方程

${\mathbf{r}}^w$ 和${\mathbf{r}}^b$两个矢量之间可以用四元数转换如下（这是转换的定义）
$${\mathbf{r}}^w={\mathbf{q}}_{wb}\otimes {\mathbf{r}}^b\otimes {\mathbf{q}}_{wb}^{\ast }$$
re等式两边同时右乘${\mathbf{q}}_{wb}$ ，可得
$${\mathbf{r}}^w\otimes {\mathbf{q}}_{wb}={\mathbf{q}}_{wb}\otimes {\mathbf{r}}^b$$
上式两边同时求微分，可得
$$\begin{array}{rl}{\dot{\mathbf{r}}}^w\otimes {\mathbf{q}}_{wb}+{\mathbf{r}}^w& \otimes {\dot{\mathbf{q}}}_{wb}={\dot{\mathbf{q}}}_{wb}\otimes {\mathbf{r}}^b+{\mathbf{q}}_{wb}\otimes {\dot{\mathbf{r}}}^b\end{array}$$
由于（其中$\times$代表向量叉乘 ，$\otimes$代表两个四元数相乘）
$$\begin{array}{l}{\dot{\mathbf{r}}}^b=-{\mathbf{\omega }}_{wb}^b\times {\mathbf{r}}^b=-{\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b\\ {\dot{\mathbf{r}}}^w=0\end{array}$$
则有
$$\begin{array}{rl}& {\mathbf{r}}^w\otimes {\dot{\mathbf{q}}}_{wb}\\ =& \left({\mathbf{q}}_{wb}\otimes {\mathbf{r}}^b\otimes {\mathbf{q}}_{wb}^{\ast }\right)\otimes {\dot{\mathbf{q}}}_{wb}\\ =& {\dot{\mathbf{q}}}_{wb}\otimes {\mathbf{r}}^b-{\mathbf{q}}_{wb}\otimes {\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b\end{array}$$
等式两边同时左乘 ${\mathbf{q}}_{wb}^{\ast }$ ，可得
$$\begin{array}{r}{\mathbf{r}}^b\otimes {\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}={\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\otimes {\mathbf{r}}^b-{\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b\end{array}$$
移项可得
$${\mathbf{\omega }}_{wb}^b\otimes {\mathbf{r}}^b=\left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)\otimes {\mathbf{r}}^b-{\mathbf{r}}^b\otimes \left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)$$
利用前面四元数相乘展开成矩阵与向量相乘的公式，将上式展开，可得
$$\left[\begin{array}{cc}0& 0_{1\times 3}\\ 0_{3\times 1}& {\left[{\mathbf{\omega }}_{wb}^b\right]}_{\times }\end{array}\right]\left[\begin{array}{l}0\\ {\mathbf{r}}^b\end{array}\right]=\left[\begin{array}{cc}0& 0_{1\times 3}\\ 0_{3\times 1}& 2{\left[{\left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)}_v\right]}_{\times }\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{r}}^b\end{array}\right]$$
因此有
$${\left({\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}\right)}_v=\frac{1}{2}{\mathbf{\omega }}_{wb}^b$$
以上只是把虚部求解出来了，实部还得搞一搞，如下

根据四元数与旋转矢量的关系，有
$${\mathbf{q}}_{wb}^{\ast }=\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ -\mathbf{u}\sin \frac{\varphi }{2}\end{array}\right]$$

$${\dot{\mathbf{q}}}_{wb}=\left[\begin{array}{c}-\frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}\\ \dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\varphi }{2}\cos \frac{\varphi }{2}\end{array}\right]$$

因此有
$$\begin{array}{rl}{\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}& =\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ -\mathbf{u}\sin \frac{\varphi }{2}\end{array}\right]\otimes \left[\begin{array}{c}-\frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}\\ \dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\varphi }{2}\cos \frac{\varphi }{2}\end{array}\right]\\ & =\left[\begin{array}{c}-\frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}+{\left(\mathbf{u}\sin \frac{\varphi }{2}\right)}^{\mathrm{T}}\left(\dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\Phi }}{2}\cos \frac{\varphi }{2}\right)\\ \cos \frac{\dot{\varphi }}{2}\left(\dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}\cos \frac{\varphi }{2}\right)+\mathbf{u}\sin \frac{\varphi }{2}\cdot \frac{\dot{\varphi }}{2}\sin \frac{\varphi }{2}-\left(\mathbf{u}\sin \frac{\varphi }{2}\right)\times \left(\dot{\mathbf{u}}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}\cos \frac{\varphi }{2}\right)\end{array}\right]\\ & =\left[\begin{array}{c}0\\ \mathbf{u}\cos \frac{\varphi }{2}\sin \frac{\varphi }{2}+\mathbf{u}\frac{\dot{\varphi }}{2}-\mathbf{u}\sin \frac{\varphi }{2}\times \dot{\mathbf{u}}\sin \frac{\varphi }{2}\end{array}\right]\end{array}$$
可以看出实部为0。

那么最终就可以得到
$${\mathbf{q}}_{wb}^{\ast }\otimes {\dot{\mathbf{q}}}_{wb}=\frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_{wb}^b\end{array}\right]$$
即
$${\dot{\mathbf{q}}}_{wb}={\mathbf{q}}_{wb}\otimes \frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_{wb}^b\end{array}\right]$$

1.3 旋转向量

旋转向量，从物理意义上比较好理解，借用欧拉角的定义，它表示物体绕坐标轴分别按三个欧拉角旋转三次，就可以得到它现在所处的姿态。而旋转向量的定义是，物体绕空间某一个轴旋转一次，就可以达到现在的姿态。二者的区别在于，欧拉角的旋转是绕xyz这样的直角坐标轴的，而旋转向量所绕的轴可能朝向空间任意一个方向，这个轴取决于载体的姿态。

等效旋转矢量可以用向量$\mathbf{\varphi }$ ，并用 单位向量$\mathbf{u}$表示它的朝向， $\varphi$ 表示它的大小，因此有
$$\mathbf{\varphi }=\varphi \mathbf{u}$$
等效旋转矢量的指数形式，是推导中重要的推导过程，因此先在这里给出
$$\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\exp \left(\varphi {\mathbf{u}}^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\varphi {\mathbf{u}}^{\wedge }\right)}^n$$
由于带着无穷项，因此需要化简才能用。由于反对称矩阵具有如下性质
$${\left({\mathbf{\varphi }}^{\wedge }\right)}^i=\{\begin{array}{ll}(-1{)}^{(i-1)/2}{\varphi }^{i-1}\left({\mathbf{\varphi }}^{\wedge }\right)& i=1,3,5,\cdots \\ (-1{)}^{(i-2)/2}{\varphi }^{i-2}{\left({\mathbf{\varphi }}^{\wedge }\right)}^2& i=2,4,6,\cdots \end{array}$$
因此，指数形式可以化简为
$$\begin{array}{rl}\exp \left({\mathbf{\varphi }}^{\wedge }\right)& =I+\varphi \left({\mathbf{u}}^{\wedge }\right)+\frac{1}{2!}{\varphi }^2{\left({\mathbf{u}}^{\wedge }\right)}^2+\frac{1}{3!}{\varphi }^3{\left({\mathbf{u}}^{\wedge }\right)}^3+\frac{1}{4!}{\varphi }^4{\left({\mathbf{u}}^{\wedge }\right)}^4+\cdots \\ & =I+\varphi \left({\mathbf{u}}^{\wedge }\right)+\frac{1}{2!}{\varphi }^2{\left({\mathbf{u}}^{\wedge }\right)}^2-\frac{1}{3!}{\varphi }^3\left({\mathbf{u}}^{\wedge }\right)-\frac{1}{4!}{\varphi }^4{\left({\mathbf{u}}^{\wedge }\right)}^2+\cdots \\ & =I+{\left({\mathbf{u}}^{\wedge }\right)}^2+\underset{\sin \varphi }{\underbrace{\left(\varphi -\frac{1}{3!}{\varphi }^3+\frac{1}{5!}{\varphi }^5-\cdots \right)}}\left({\mathbf{u}}^{\wedge }\right)-\underset{\cos \varphi }{\underbrace{\left(1-\frac{1}{2!}{\varphi }^2+\frac{1}{4!}{\varphi }^4-\cdots \right)}}{\left({\mathbf{u}}^{\wedge }\right)}^2\\ & =I+\sin \varphi \left({\mathbf{u}}^{\wedge }\right)+(1-\cos \varphi ){\left({\mathbf{u}}^{\wedge }\right)}^2\\ & =I+\frac{\sin \varphi }{\varphi }\left({\mathbf{\varphi }}^{\wedge }\right)+\frac{(1-\cos \varphi )}{{\varphi }^2}{\left({\mathbf{\varphi }}^{\wedge }\right)}^2\end{array}$$
最后一项是罗德里格斯公式。

1）旋转向量微分方程

旋转向量微分方程的推导就显得更加复杂，这里就借用严恭敏老师《惯性导航与组合导航算法》中的一个中间结论吧，直接给出旋转矢量微分方程的完整形式
$$\dot{\mathbf{\varphi }}={\mathbf{\omega }}_{wb}^b+\frac{1}{2}\mathbf{\varphi }\times {\mathbf{\omega }}_{wb}^b+\frac{1}{{\varphi }^2}\left(1-\frac{\varphi }{2}\cot \frac{\varphi }{2}\right)(\mathbf{\varphi }\times {)}^2{\mathbf{\omega }}_{wb}^b$$
如果你愿意把它泰勒展开，那么去除高阶项会得到如下简化形式
$$\dot{\mathbf{\varphi }}={\mathbf{\omega }}_{wb}^b+\frac{1}{2}\mathbf{\varphi }\times {\mathbf{\omega }}_{wb}^b$$
这就是前面看到的结果啦。

1.4 各种表示方式之间的转换

既然各种方式之间是等价的，且实际使用中，各种形式都有可能出现，那么理清它们之间的转换关系就是必要的这里就只能给出结论。

1）欧拉角与旋转矩阵

按照机器人前(x)-左(y)-上(z)的坐标系定义，并令横滚角为$\alpha$ 、俯仰角为 $\beta$ 、航向角为 $\gamma$ ，假设旋转矩阵是按照z-y-x的顺序旋转得来，那么旋转矩阵可以表示为
$${\mathbf{R}}_{wb}={\left({\mathbf{R}}_x(\alpha ){\mathbf{R}}_y(-\beta ){\mathbf{R}}_z(\gamma )\right)}^T$$
其中
$${\mathbf{R}}_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\alpha )& \sin (\alpha )\\ 0& -\sin (\alpha )& \cos (\alpha )\end{array}\right]$$
$${\mathbf{R}}_y(-\beta )=\left[\begin{array}{ccc}\cos (\beta )& 0& \sin (\beta )\\ 0& 1& 0\\ -\sin (\beta )& 0& \cos (\beta )\end{array}\right]$$
$${\mathbf{R}}_z(\gamma )=\left[\begin{array}{ccc}\cos (\gamma )& \sin (\gamma )& 0\\ -\sin (\gamma )& \cos (\gamma )& 0\\ 0& 0& 1\end{array}\right]$$
也可以直接写出结果形式
$${\mathbf{R}}_{wb}=\left[\begin{array}{ccc}c\beta c\gamma & -s\alpha s\beta c\gamma -c\alpha s\gamma & s\alpha s\gamma -c\alpha s\beta c\gamma \\ c\beta s\gamma & c\alpha c\gamma -s\alpha s\beta s\gamma & -c\alpha s\beta s\gamma -s\alpha c\gamma \\ s\beta & s\alpha c\beta & \mathrm{coc}\beta \end{array}\right]$$
其中 $s\bullet =\sin (\bullet )c\bullet =\cos (\bullet )$

因此，观察该矩阵，可以得出由旋转矩阵得到欧拉角的方式
$$\begin{array}{l}\alpha =\arctan 2\left({\mathbf{R}}_{wb}(3,2),{\mathbf{R}}_{wb}(3,3)\right)\\ \beta =\arcsin \left({\mathbf{R}}_{wb}(3,1)\right)\\ \gamma =\arctan 2\left({\mathbf{R}}_{wb}(2,1),{\mathbf{R}}_{wb}(1,1)\right)\end{array}$$

2) 四元数与旋转矩阵

由四元数转旋转矩阵的公式为
$${\mathbf{R}}_{ub}=\left[\begin{array}{ccc}{q}_u^2+q_x^2-q_y^2-q_z^2& 2\left(q_x{q}_y-q_w{q}_z\right)& 2\left(q_x{q}_z+q_w{q}_y\right)\\ 2\left(q_x{q}_y+q_w{q}_z\right)& q_w^2-q_x^2+q_y^2-q_z^2& 2\left(q_y{q}_z-q_w{q}_x\right)\\ 2\left(q_x{q}_z-q_w{q}_y\right)& 2\left(q_y{q}_z+q_w{q}_x\right)& q_w^2-q_x^2-q_y^2+q_z^2\end{array}\right]$$
由旋转矩阵转四元数的公式为
$$\begin{array}{l}{q}_u=\frac{\sqrt{1+R_{wb}(1,1)+R_{wb}(2.2)+R_{wb}(3.3)}}{2}\\ q_x=\frac{R_{wb}(3,2)-R_{wb}(2,3)}{4q_w}\\ q_y=\frac{R_{ub}(1,3)-R_{wb}(3,1)}{4q_w}\\ q_z=\frac{R_{wb}(2,1)-R_{wb}(1,2)}{4q_w}\end{array}$$
不过需要注意的是，此处需要满足
$$q_w\ne 0,1+R_{wb}(1,1)+R_{wb}(2,2)+R_{wb}(3,3)>0$$

3) 旋转矩阵与旋转向量

由旋转向量得到旋转矩阵的公式为
$${\mathbf{R}}_{wb}=I+\frac{\sin \varphi }{\varphi }\left({\mathbf{\varphi }}^{\wedge }\right)+\frac{1-\cos \varphi }{{\varphi }^2}{\left({\mathbf{\varphi }}^{\wedge }\right)}^2$$
可以发现，这跟旋转向量的指数形式是一样的，这就是传说中的罗德里格斯公式。

由旋转矩阵得到旋转向量的公式为
$$\begin{array}{l}\varphi =\arccos \frac{\mathrm{tr}\left({\mathbf{R}}_{wb}\right)-1}{2}\\ \mathbf{u}=\frac{{\left({\mathbf{R}}_{wb}-{\left({\mathbf{R}}_{wb}\right)}^T\right)}^{\vee }}{2\sin \varphi }\end{array}$$

4) 四元数与旋转向量

由旋转向量计算四元数的公式为
$$\mathbf{q}=\cos \frac{\varphi }{2}+\frac{\sin \frac{\varphi }{2}}{\varphi }\mathbf{\varphi }$$
由旋转向量计算四元数的公式为
$$\begin{array}{c}\varphi =2\arctan \left(\Vert {\mathbf{q}}_v\Vert ,q_w\right)\\ \mathbf{u}={\mathbf{q}}_v/\Vert {\mathbf{q}}_v\Vert \end{array}$$
这两个公式里，有一个很有意思的问题，就是第一个公式里面有两处除以2，第二个公式里有一处乘以2，这个2怎么来的呢？

我们知道，在旋转矩阵表示法中，一个向量在两个坐标系之间转换的公式为
$${\mathbf{a}}^{\prime }=\mathbf{R}\mathbf{a}$$
而四元数则不这么简单，它为
$${\mathbf{a}}^{\prime }=\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }$$
这个也可以从一个比较奇特的角度来解释，大家看一下，一个四元数和一个矢量相乘(其实乘的是矢量对应的纯虚四元数，即实部为0，虚步三个值对应这个三维矢量)，得到的是什么？已经忘记的可以往上翻去看一下四元数乘法。结论是乘出来的一个东西，实部不为0，这就尴尬了，一个三维矢量，乘出来变成了一个四维的东西(一个实部和三个虚部)，为了解决这个问题，就写成了上面的形式，左乘一个，右乘一个，实际就是对应两次旋转，自然每次就只旋转一半，即每一个四元数对应的角度只是想要的旋转角度的一半，这样改变之后，惊奇地发现，实部永远为0，等于三维矢量先是被第一个四元数给旋转到四维空间，又被第二个四元数给拉回到三维空间了，完美！

原文链接：多传感器融合定位理论基础（五）：惯性导航解算