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

时隔多日，开始填坑，催稿催的我心慌慌～～～

一、概述

惯性导航的解算是一个实现起来非常简单，但是理解起来要费一番功夫的东西，所谓“实现”就是把公式变成代码，所谓“理解”，就是要弄明白几个公式是怎么推导出来的。

鉴于这种情况，我们先把本篇文章的核心思路捋清楚，然后再去搞细节，不然很容易稀里糊涂地陷入繁琐的公式推导当中去，相信各位以前应该都有过这种体会，每一个公式都看得懂，但是连在一起就不知道它为什么要这么干，对于“理解”，“为什么”可比“是什么”要重要的多了。

在这里，“理解”就包含以下三点：

1）做什么：惯性导航解算的目的是什么？

2）怎么做：惯性导航解算的方法是什么？

3）为什么：这个方法是怎么来的？

二、惯性导航解算的目的

目的当然很简单，从IMU原始的角速度（陀螺仪采集）和加速度（加速度计采集）数据得到它的导航结果（位置、速度、姿态）呗。但如果我们深入下去，好像就不是这一句话能回答的了的了，比如导航信息是怎么描述的，位置和速度好说，姿态就显得略微复杂一些，这是一个long story，喝口水慢慢讲。

1.姿态描述

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

1.1 欧拉角

1）欧拉角定义

欧拉角是一个非常直观的概念，一般使用中，它包括俯仰角、横滚角、航向角。

a. 俯仰角

俯仰角描述载体“抬头”的角度大小，一般以抬头(向上)为正，低头(向下)为负

b. 横滚角

横滚角描述载体“侧身”的角度大小，一般以绕着正前方逆时针旋转为正，顺时针旋转为负。

c. 航向角

航向描述载体的朝向，一般以逆时针旋转为正，顺时针旋转为负。（传统组合导航领域与此相反，在此暂时不过多提及，只是让大家看资料时别觉得奇怪就行）

2）欧拉角与坐标轴的对应关系

我们常常把一个xyz直角坐标系放在载体上，这样才方便进行数学描述嘛，如果用前后左右上下来对应载体的方向的话，常见的载体坐标系的对应关系有“右(x)-前(y)-上(z)”、“前(x)-右(y)-下(z)”、“前(x)-左(y)-上(z)”，其中前两种更多的在传统组合导航领域使用，而第三种在机器人、自动驾驶领域使用比较多，由于我们的文章是以后者为主要目标的，所以后面所有的推导就直接使用“前(x)-左(y)-上(z)”坐标系。

在这种坐标系下，我们可以看到，俯仰角是绕y轴的顺时针为正，逆时针为负，横滚角绕x轴的逆时针为正，顺时针为负，航向角绕z轴的逆时针为正，顺时针为负。需要注意的是，一般我们习惯的坐标系表示中，都是逆时针为正，顺时针为负，而俯仰角在这里与y轴的对应关系是反的，这是因为欧拉角有明确的物理意义(比如俯仰角抬头为正、低头为负)，违反这种物理意义会对使用带来一些障碍，所以一般宁愿去改变与坐标轴的对应关系。当然，也有很多不愿意转变的，也是很常见，所以大家在使用时要擦亮眼睛，接触一个新的系统之前，要记得首先去核实对应关系。

1.2 旋转矩阵

1）旋转矩阵定义

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

$\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]\\$

由此可以得到

$\boldsymbol{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 }}{=} \boldsymbol{R a}^{\prime}\\$

其中 R 就是旋转矩阵，带上坐标系下标，可以写为 RωB。

2）旋转矩阵性质

旋转矩阵的主要特性是单位正交矩阵，即

$\mathbf{R}^{-1} = \mathbf{R}^T\\$

1.3 四元数

1）四元数定义

对很多接触过导航定位算法的同志们来讲，可能四元数是一个让人头疼的问题了，想把它彻底讲明白，要费很大力气，更何况我自己也不敢说完全把它搞透彻了，曾经尝试过，我记得有一本专门写四元数的书，有好几百页，直接把我劝退，后来退而求其次，觉得从物理意义上大致理解它就行。

$\mathbf{A} \cdot \mathbf{B} = (a+b\mathbf{i})(c+d\mathbf{i})=ac-bd+(ac+bd)\mathbf{i}\\$

这种性质让它具有了向量不具有的优势，复数域的直接运算带来的便利性是无可比拟的。

后来，三维空间的运动描述越来越迫切的时候，复数已经不能满足需求，因为它只有一个角度嘛，不是三维的，汉密尔顿想(可自行百度下汉密尔顿与四元数)，能不能再加一维来表示呢，比如A=a+bi+cj，后来发现不行，那就再加一维 A=a+bi+cj+dk，发现行了，于是就有了四元数。哈哈，他发明四元数的过程当然没有那么简单，因为这个“再加一维”牵扯到的东西太多太多，也是理解四元数的关键，不是拍一下脑袋这么简单。

为了弄明白这个“再加一维”，我们可以换一个角度来看它，若另 A=a+bi, B=c+di，会发现

$\begin{aligned} \mathbf{Q} &= \mathbf{A+Bj} \\ &= (a+b\mathbf{i}) + (c+d\mathbf{i})\mathbf{j}\\ &=a+b\mathbf{i} + c\mathbf{j}+d\mathbf{i}\mathbf{j} \end{aligned}\\$

在这里，另k=ij，那上式不就是四元数了嘛

$\begin{aligned} \mathbf{Q} = a+b\mathbf{i} + c\mathbf{j}+d\mathbf{k} \end{aligned}\\$

因此，从 Q=A+Bj可以看出，四元数可以理解为“复数的复数”。

明白了这一点，再回到复数的定义，就可以严格点讲，复数不是在实数上加一维，而是“实数的复数”，看起来好像一句废话，但这样描述是有必要的。更抽象一点，可以总结为，把一类东西(假设类别为X)在复数维度升维，应该写成Xa+Xbj，而不应该是Xa+bj (这里的b是一个实数)。当X表示实数时，这两种理解方式并无差异，而当X已经是复数时，两种方式造成的结果就产生了差异。这就是刚才所提到的，为什么汉密尔顿一开始在描述三维空间的角度时，只在复数上加一维( A=a+bi+cj )会不行的原因。

由此扩展下去，四元数的复数是八元数，八元数的复数是十六元数，这不是我胡说八道，在数学领域确实是存在的，它们统称为超复数。由于每一次升维，都会损失一个性质（比如四元数没有了乘法交换性，而复数有），且在三维空间领域，四元数已经足够用来描述，因此其他超复数就没有用武之地了，大家也就见不到。

2）四元数性质

四元数的性质很多，我们挑一些后面能用到的说。首先把四元数按照常见的形式写成

$\boldsymbol{q}=q_{w}+\boldsymbol{q}_{v}=q_{w}+q_{x} \boldsymbol{i}+q_{y} \boldsymbol{j}+q_{z} \boldsymbol{k}\\$

其中

$q_{w}$ 是实部

$\mathbf{q}_v$ 是虚部。

a. 单位四元数

在姿态表示中，我们所用的四元数都是单位四元数，即

$||\boldsymbol{q}||=\sqrt{q_w^2 + q_x^2 + q_y^2 + q_z^2} = 1 \\$

b. 共轭四元数与四元数的逆

共轭四元数指的是实部相同，虚部相反的四元数

$\boldsymbol{q}^{*}=q_{w}-\boldsymbol{q}_{v}=q_{w}-q_{x} \boldsymbol{i}-q_{y} \boldsymbol{j}-q_{z} \boldsymbol{k}\\$

对于单位四元数来讲，它的逆与它的共轭四元数相等

$\boldsymbol{q}^{-1}=\boldsymbol{q}^{*}\\$

c. 四元数乘法

四元数乘法定义为

$\boldsymbol{p} \otimes \boldsymbol{q}=\left[\begin{array}{c} p_{w} q_{w}-\boldsymbol{p}_{v}^{\mathrm{T}} \boldsymbol{q}_{v} \\ p_{w} \boldsymbol{q}_{v}+q_{w} \boldsymbol{p}_{v}+\boldsymbol{p}_{v} \times \boldsymbol{q}_{v} \end{array}\right]\\$

如果你把上面的式子展开，会发现四元数乘法可以表示成矩阵与向量相乘的形式

$\boldsymbol{p} \otimes \boldsymbol{q}=[\boldsymbol{p}]_{L} \boldsymbol{q}\\$

其中

$\begin{aligned} [\boldsymbol{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} \boldsymbol{I}+\left[\begin{array}{cc} 0 & -\boldsymbol{p}_{v}^{\mathrm{T}} \\ \boldsymbol{p}_{v} & {\left[\boldsymbol{p}_{v}\right]_{\times}} \end{array}\right] \end{aligned}\\$

其中 [·]x表示向量的反对称矩阵(不了解这个概念的麻烦自行去查一下)

四元数乘法也可以展开为

$\boldsymbol{p} \otimes \boldsymbol{q}=[\boldsymbol{q}]_{R} \boldsymbol{p}\\$

其中

$\begin{aligned} [\boldsymbol{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} \boldsymbol{I}+\left[\begin{array}{cc} 0 & -\boldsymbol{q}_{v}^{\mathrm{T}} \\ \boldsymbol{q}_{v} & -\left[\boldsymbol{q}_{v}\right]_{\times} \end{array}\right] \end{aligned}\\$

因此可以得到

$\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}\\$

这是两个很重要的性质，后面的推导中会多次用到

1.4 等效旋转矢量

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

等效旋转矢量可以用向量 ∅ ，并用单位向量u表示它的朝向， ∅ 表示它的大小，因此有

$\boldsymbol{\phi} = \phi \boldsymbol{u}\\$

等效旋转矢量的指数形式，是推导中重要的推导过程，因此先在这里给出

$\exp \left(\boldsymbol{\phi}^{\wedge}\right)=\exp \left(\phi \boldsymbol{u}^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\phi \boldsymbol{u}^{\wedge}\right)^{n}\\$

由于带着无穷项，因此需要化简才能用。由于反对称矩阵具有如下性质

$\left(\boldsymbol{\phi}^{\wedge}\right)^{i}=\left\{\begin{array}{ll} (-1)^{(i-1) / 2} \phi^{i-1}\left(\boldsymbol{\phi}^{\wedge}\right) & i=1,3,5, \cdots \\ (-1)^{(i-2) / 2} \phi^{i-2}\left(\boldsymbol{\phi}^{\wedge}\right)^{2} & i=2,4,6, \cdots \end{array}\right.\\$

因此，指数形式可以化简为

$\begin{aligned} \exp \left(\boldsymbol{\phi}^{\wedge}\right) &=I+\phi\left(\boldsymbol{u}^{\wedge}\right)+\frac{1}{2 !} \phi^{2}\left(\boldsymbol{u}^{\wedge}\right)^{2}+\frac{1}{3 !} \phi^{3}\left(\boldsymbol{u}^{\wedge}\right)^{3}+\frac{1}{4 !} \phi^{4}\left(\boldsymbol{u}^{\wedge}\right)^{4}+\cdots \\ &=I+\phi\left(\boldsymbol{u}^{\wedge}\right)+\frac{1}{2 !} \phi^{2}\left(\boldsymbol{u}^{\wedge}\right)^{2}-\frac{1}{3 !} \phi^{3}\left(\boldsymbol{u}^{\wedge}\right)-\frac{1}{4 !} \phi^{4}\left(\boldsymbol{u}^{\wedge}\right)^{2}+\cdots \\ &=I+\left(\boldsymbol{u}^{\wedge}\right)^{2}+\underbrace{\left(\phi-\frac{1}{3 !} \phi^{3}+\frac{1}{5 !} \phi^{5}-\cdots\right)}_{\sin \phi}\left(\boldsymbol{u}^{\wedge}\right)-\underbrace{\left(1-\frac{1}{2 !} \phi^{2}+\frac{1}{4 !} \phi^{4}-\cdots\right)}_{\cos \phi}\left(\boldsymbol{u}^{\wedge}\right)^{2} \\ &=I+\sin \phi\left(\boldsymbol{u}^{\wedge}\right)+(1-\cos \phi)\left(\boldsymbol{u}^{\wedge}\right)^{2} \\ &=I+\frac{\sin \phi}{\phi}\left(\boldsymbol{\phi}^{\wedge}\right)+\frac{(1-\cos \phi)}{\phi^{2}}\left(\boldsymbol{\phi}^{\wedge}\right)^{2} \end{aligned}\\$

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

既然各种方式之间是等价的，且实际使用中，各种形式都有可能出现，那么理清它们之间的转换关系就是必要的。不过由于很多推到过程过于复杂且无聊，这里就只能给出结论，对过程感兴趣的自己去查资料吧。

1）欧拉角与旋转矩阵

按照机器人前(x)-左(y)-上(z)的坐标系定义，并令横滚角为 α 、俯仰角为 β 、航向角为 γ ，假设旋转矩阵是按照z-y-x的顺序旋转得来，那么旋转矩阵可以表示为

$\boldsymbol{R}_{w b}=\left(\boldsymbol{R}_{x}(\alpha) \boldsymbol{R}_{y}(-\beta) \boldsymbol{R}_{z}(\gamma)\right)^{T}\\$

其中

$\boldsymbol{R}_{x}(\alpha)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos (\alpha) & \sin (\alpha) \\ 0 & -\sin (\alpha) & \cos (\alpha) \end{array}\right] \\$

$\boldsymbol{R}_{y}(-\beta)=\left[\begin{array}{ccc} \cos (\beta) & 0 & \sin (\beta) \\ 0 & 1 & 0 \\ -\sin (\beta) & 0 & \cos (\beta) \end{array}\right] \\$

$\quad \boldsymbol{R}_{z}(\gamma)=\left[\begin{array}{ccc} \cos (\gamma) & \sin (\gamma) & 0 \\ -\sin (\gamma) & \cos (\gamma) & 0 \\ 0 & 0 & 1 \end{array}\right]\\$

也可以直接写出结果形式

$\boldsymbol{R}_{w b}=\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 & \operatorname{coc} \beta \end{array}\right]\\$

其中 $s \bullet=\sin (\bullet) \quad c \bullet=\cos (\bullet)$

因此，观察该矩阵，可以得出由旋转矩阵得到欧拉角的方式

$\begin{array}{l} \alpha=\arctan 2\left(\boldsymbol{R}_{w b}(3,2), \boldsymbol{R}_{w b}(3,3)\right) \\ \beta=\arcsin \left(\boldsymbol{R}_{w b}(3,1)\right) \\ \gamma=\arctan 2\left(\boldsymbol{R}_{w b}(2,1), \boldsymbol{R}_{w b}(1,1)\right) \end{array}\\$

2) 四元数与旋转矩阵

由四元数转旋转矩阵的公式为

$\boldsymbol{R}_{u b}=\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_{w b}(1,1)+R_{w b}(2.2)+R_{w b}(3.3)}}{2} \\ q_{x}=\frac{R_{w b}(3,2)-R_{w b}(2,3)}{4 q_{w}} \\ q_{y}=\frac{R_{u b}(1,3)-R_{w b}(3,1)}{4 q_{w}} \\ q_{z}=\frac{R_{w b}(2,1)-R_{w b}(1,2)}{4 q_{w}} \end{array}\\$

不过需要注意的是，此处需要满足

$q_{w} \neq 0,1+R_{w b}(1,1)+R_{w b}(2,2)+R_{w b}(3,3)>0\\$

不满足的时候也能转，不过太复杂，写不动了。。。。。。

3）旋转矩阵与等效旋转矢量

由旋转矢量得到旋转矩阵的公式为

$\boldsymbol{R}_{w b}=I+\frac{\sin \phi}{\phi}\left(\boldsymbol{\phi}^{\wedge}\right)+\frac{1-\cos \phi}{\phi^{2}}\left(\boldsymbol{\phi}^{\wedge}\right)^{2}\\$

可以发现，这跟等效旋转矢量的指数形式是一样的，这就是传说中的罗德里格斯公式。

由旋转矩阵得到旋转矢量的公式为

$\begin{array}{l} \phi=\arccos \frac{\operatorname{tr}\left(\boldsymbol{R}_{w b}\right)-1}{2} \\ \boldsymbol{u}=\frac{\left(\boldsymbol{R}_{w b}-\left(\boldsymbol{R}_{w b}\right)^{T}\right)^{\vee}}{2 \sin \phi} \end{array}\\$

4) 四元数与等效旋转矢量

由旋转矢量计算四元数的公式为

$\boldsymbol{q}=\cos \frac{\phi}{2}+\frac{\sin \frac{\phi}{2}}{\phi} \boldsymbol{\phi}\\$

由旋转矢量计算四元数的公式为

$\begin{array}{c} \phi=2 \arctan \left(\left\|\boldsymbol{q}_{v}\right\|, q_{w}\right) \\ \boldsymbol{u}=\boldsymbol{q}_{v} /\left\|\boldsymbol{q}_{v}\right\| \end{array}\\$

这两个公式里，有一个很有意思的问题，就是第一个公式里面有两处除以2，第二个公式里有一处乘以2，这个2怎么来的呢？

我们知道，在旋转矩阵表示法中，一个向量在两个坐标系之间转换的公式为

$\boldsymbol{a}^{\prime}=\boldsymbol{R} \boldsymbol{a}\\$

而四元数则不这么简单，它为

$\boldsymbol{a}^{\prime}=\boldsymbol{q} \otimes \boldsymbol{a} \otimes \boldsymbol{q}^{*}\\$

这个也可以从一个比较奇特的角度来解释，大家看一下，一个四元数和一个矢量相乘(其实乘的是矢量对应的纯虚四元数，即实部为0，虚步三个值对应这个三维矢量)，得到的是什么？已经忘记的可以网上翻去看一下四元数乘法。结论是乘出来的一个东西，实部不为0，这就尴尬了，一个三维矢量，乘出来变成了一个四维的东西(一个实部和三个虚部)，为了解决这个问题，就写成了上面的形式，左乘一个，右乘一个，实际就是对应两次旋转，自然每次就只旋转一半，即每一个四元数对应的角度只是想要的旋转角度的一半，这样改变之后，惊奇地发现，实部永远为0，等于三维矢量先是被第一个四元数给旋转到四维空间，又被第二个四元数给拉回到三维空间了，完美！

三、惯性导航解算的方法

经过上面啰嗦一大堆，我们解决了“做什么”、“怎么做”、“为什么”三个问题中的第一个，接下来解决第二个。

“怎么做”在这里就是怎么去解算的问题了，而解算就是“角速度->姿态”、“加速度->速度”、“速度->位置”(当然由于有坐标系问题，这种描述并不十分严谨，但更容易理解)。看到这些，我们很容易想到，就是个积分问题嘛，把这个积分过程用数学公式表示出来，就等于知道了“怎么做”的问题。

积分公式的形式其实很简单，但它的推导过程比较繁琐，因此这里先给出结论，过程是“为什么”这个环节要解决的问题。另外，积分与微分本是一家，因此本小节会从微分方程引入，讲“为什么”的时候就只讲微分方程怎么来的就好了，这样这两节就可以无缝衔接在一起。

1.基于旋转矩阵的姿态更新

1) 积分形式介绍

旋转矩阵的微分方程为

$\dot{\boldsymbol{R}}_{w b}=\boldsymbol{R}_{w b}[\boldsymbol{\omega}]_{\times}\\$

其中 ω 为IMU三个轴测量得到的角速度。

根据微分方程可以很容易写出它的积分形式为（此处用到的是高数中讲的求一阶微分方程通解的方法，基础知识不展开介绍了）

$\boldsymbol{R}_{w b_{k}}=\boldsymbol{R}_{w b_{k-1}} e^{\int_{t_{k-1}}^{t_{k}}[\boldsymbol{\omega}]_{\times} d \tau}\\$

这里给出的是由 k-1 时刻的姿态积分得到 k 时刻姿态的积分形式。仔细观察指数部分，它表示的是角速度矢量的积分，而角速度矢量的积分其实不就是 k-1 时刻到 k 时刻对应的等效旋转矢量嘛，因此，我们可以直接写成

$\boldsymbol{R}_{w b_{k}}=\boldsymbol{R}_{w b_{k-1}} e^{\phi \times}\\$

由于反对称矩阵的指数函数前面已经推导完毕(罗德里格斯公式)，因此上式又可以写为

$\boldsymbol{R}_{w b_{k}}=\boldsymbol{R}_{w b_{k-1}} \boldsymbol{R}_{b_{k-1} b_{k}}\\$

其中

$\boldsymbol{R}_{b_{k-1} b_{k}}=I+\frac{\sin \phi}{\phi}(\boldsymbol{\phi} \times)+\frac{1-\cos \phi}{\phi^{2}}(\boldsymbol{\phi} \times)^{2}\\$

可见，求旋转矩阵的问题，最终转化成了求等效旋转矢量的问题。咋求？接着用微分方程求呗。等效旋转矢量的微分方程为（方程怎么来的，同样放在“为什么”中去讲）

$\dot{\phi} \approx \omega+\frac{1}{2} \phi \times \omega\\$

这个方程看起来还是比较复杂的，可以也应当化简。在方程右边第二项，可以看到是两个矢量的叉乘，根据叉乘的定义，当两个矢量方向重合时，叉乘的结果为0。一般 imu 的采样频率不会低于 100hz，即 k-1 时刻到 k 时刻的时间间隔不会大于10ms，而载体一般自动驾驶车或者机器人也不会发生过于高频的运动，因此可以近似认为满足叉乘为0的条件，因此可以直接把上式简化为

$\dot{\phi} \approx \omega\\$

它对应的积分形式就很简单了

$\phi \approx \int_{t_{k-1}}^{t_{k}} \boldsymbol{\omega}(\tau) d \tau\\$

有了旋转矢量的积分形式，那么该旋转矩阵的形式就了然了，直接带进去就行。

但是，到这里会遇到一个问题，我们一直讨论的都是连续时间的，即 k-1 时刻到 k 时刻之间的所有值都是连续可测的，而实际数据都是离散的，只能得到 k-1 和 k 这两个时刻的值，那么必须把上面的连续时间形式高程离散形式，才能继续走下去了。

2）离散时间处理

这就牵扯到常见的三种方法：欧拉法、中值法、龙哥库塔法，三者按复杂程度逐渐递增，按精度也是逐渐递增。我们先从简单的开始说。

a.欧拉法

欧拉法很好理解，虽然我不知道 k-1 到 k 时刻的所有值，但是如果我假设 imu 在这个时间段内是匀速旋转的，那不就解决了吗，此时，对应的旋转矢量为

$\boldsymbol{\phi}=\boldsymbol{\omega}_{k-1}\left(t_{k}-t_{k-1}\right)\\$

b.中值法

欧拉法固然简单，但是匀速的假设还是有些太强了，实际使用中会发现精度不够，于是就有了中值法。中值法假设 imu 是匀加速运动的，这就比匀速更高级了一些，此时离散时间形式为

$\boldsymbol{\phi}=\frac{\boldsymbol{\omega}_{k-1}+\boldsymbol{\omega}_{k}}{2}\left(t_{k}-t_{k-1}\right)\\$

c.龙哥库塔法

中值法假设的匀加速运动，在imu运动更加剧烈的情况下仍然不够，那就继续把模型复杂化呗，龙哥库塔法就是这种，把运动模型搞成一个高阶曲线。由于这部分略显复杂，而且实际使用中，中值法多数情况下已经够用，因此在此处就不对这种方法做过多介绍了。

2.基于四元数的姿态更新

1）积分形式介绍

四元数的微分方程为

$\dot{\boldsymbol{q}}_{w b}=\boldsymbol{q}_{w b} \otimes \frac{1}{2}\left[\begin{array}{l} 0 \\ \boldsymbol{\omega} \end{array}\right]\\$

其矩阵形式为(可回顾四元数乘法那一小节的推导)

$\dot{\boldsymbol{q}}_{w b}=\frac{1}{2}\left[\begin{array}{l} 0 \\ \boldsymbol{\omega} \end{array}\right]_{R} \boldsymbol{q}_{w b}\\$

同样按照微分方程的解法，得到其积分形式为

$\dot{\boldsymbol{q}}_{w b}=\mathrm{e}^{\frac{1}{2} \boldsymbol{\Theta}} \boldsymbol{q}_{w b}\\$

其中

$\boldsymbol{\Theta}=\left[\begin{array}{cccc} 0 & -\phi_{x} & -\phi_{y} & -\phi_{z} \\ \phi_{x} & 0 & \phi_{z} & -\phi_{y} \\ \phi_{y} & -\phi_{z} & 0 & \phi_{x} \\ \phi_{z} & \phi_{y} & -\phi_{x} & 0 \end{array}\right]\\$

为了求解该方程，要对指数函数进行泰勒展开，同样包含高次幂，展开方法与前面旋转矢量指数形式展开的方法类似，可以自行推导，此处直接给出结论

$\mathrm{e}^{\frac{1}{2} \Theta(T)}=I \cos \frac{\phi}{2}+\frac{\Theta}{\phi} \sin \frac{\phi}{2}\\$

因此有

$\boldsymbol{q}_{w b_{k}}=\left[I \cos \frac{\phi}{2}+\frac{\boldsymbol{\Theta}}{\phi} \sin \frac{\phi}{2}\right] \boldsymbol{q}_{w b_{k-1}}\\$

由于

$\left[\begin{array}{c} \cos \frac{\phi}{2} \\ \frac{\phi}{\phi} \sin \frac{\phi}{2} \end{array}\right]_{R}=\left[I \cos \frac{\phi}{2}+\frac{\Theta}{\phi} \sin \frac{\phi}{2}\right]\\$

另

$\boldsymbol{q}_{b_{k-1} b_{k}}=\left[\begin{array}{c} \cos \frac{\phi}{2} \\ \frac{\phi}{\phi} \sin \frac{\phi}{2} \end{array}\right]\\$

则

$\boldsymbol{q}_{w b_{k}}=\boldsymbol{q}_{w b_{k-1}} \otimes \boldsymbol{q}_{b_{k-1} b_{k}}\\$

2) 离散时间处理

3.速度更新

相比来讲，速度更新就显得很简单，速度的微分方程为

$\dot{\boldsymbol{v}}=\boldsymbol{R}_{w b} \boldsymbol{a}-\boldsymbol{g}\\$

其中

$\boldsymbol{a}=\left[\begin{array}{lll} a_{x} & a_{y} & a_{z} \end{array}\right]$ 为测量加速度

$\boldsymbol{g}=\left[\begin{array}{lll} 0 & 0 & g_{0} \end{array}\right]$ 为重力加速度。

该微分方程的通解形式为

$\Delta \boldsymbol{v}=\left(\boldsymbol{R}_{w b} \boldsymbol{a}-\boldsymbol{g}\right) \Delta t\\$

对应的基于中值法的速度更新形式为

$\boldsymbol{v}_{k}=\boldsymbol{v}_{k-1}+\left(\frac{\boldsymbol{R}_{w b_{k}} \boldsymbol{a}_{k}+\boldsymbol{R}_{w b_{k}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_{k}-t_{k-1}\right)\\$

4.位置更新

位置更新就更加简单，微分形式为

$\dot{\boldsymbol{p}}=\boldsymbol{v}\\$

其通解形式为

$\Delta \boldsymbol{p}=\boldsymbol{v} \Delta t\\$

基于中值法的离散形式为

$\boldsymbol{p}_{k}=\boldsymbol{p}_{k-1}+\frac{\boldsymbol{v}_{k}+\boldsymbol{v}_{k-1}}{2}\left(t_{k}-t_{k-1}\right)\\$

另外，如果把速度的通解代入，则位置的通解形式也可以写为

$\Delta \boldsymbol{p}=\boldsymbol{v} \Delta t+\frac{1}{2} \boldsymbol{a} \Delta t^{2}\\$

相应的基于中值法的离散形式为

$\begin{aligned} \boldsymbol{p}_{k}= \boldsymbol{p}_{k-1}+\boldsymbol{v}_{k-1}\left(t_{k}-t_{k-1}\right) +\frac{1}{2}\left(\frac{\boldsymbol{R}_{w b_{k}} \boldsymbol{a}_{k}+\boldsymbol{R}_{w b_{k-1}} \boldsymbol{a}_{k-1}}{2}-\boldsymbol{g}\right)\left(t_{k}-t_{k-1}\right)^{2} \end{aligned}\\$

三、解算方法的来源

这一部分就是要解决“为什么”的问题，其实就是推导刚才用到的那些微分方程怎么来的。由于速度、位置的微分方程过于简单，此处就不介绍了，只介绍旋转矩阵、四元数、旋转矢量的微分方程。

1.旋转矩阵微分方程

$\boldsymbol{r}^{w}=\boldsymbol{R}_{w b} \boldsymbol{r}^{b}\\$

两边同时微分，得到

$\dot{\boldsymbol{r}}^{w}=\boldsymbol{R}_{w b} \dot{\boldsymbol{r}}^{b}+\dot{\boldsymbol{R}}_{w b} \boldsymbol{r}^{b}\\$

$\mathbf{0}=\boldsymbol{R}_{w b}\left(-\boldsymbol{\omega}_{w b}^{b} \times \boldsymbol{r}^{b}\right)+\dot{\boldsymbol{R}}_{w b} \boldsymbol{r}^{b}\\$

移项可得

$\boldsymbol{R}_{w b}\left(\boldsymbol{\omega}_{w b}^{b} \times \boldsymbol{r}^{b}\right)=\dot{\boldsymbol{R}}_{w b} \boldsymbol{r}^{b}\\$

即

$\boldsymbol{R}_{w b}\left(\boldsymbol{\omega}_{w b}^{b} \times \boldsymbol{r}^{b}\right)=\dot{\boldsymbol{R}}_{w b} \boldsymbol{r}^{b}\\$

因此有

$\dot{\boldsymbol{R}}_{w b}=\boldsymbol{R}_{w b}\left[\boldsymbol{\omega}_{w b}^{b}\right]_{\times}\\$

2.四元数微分方程

$\boldsymbol{r}^{w}=\boldsymbol{q}_{w b} \otimes \boldsymbol{r}^{b} \otimes \boldsymbol{q}_{w b}^{*}\\$

$\boldsymbol{r}^{w} \otimes \boldsymbol{q}_{w b}=\boldsymbol{q}_{w b} \otimes \boldsymbol{r}^{b}\\$

上式两边同时求微分，可得

$\begin{aligned} \dot{\boldsymbol{r}}^{w} \otimes \boldsymbol{q}_{w b}+\boldsymbol{r}^{w} & \otimes \dot{\boldsymbol{q}}_{w b} =\dot{\boldsymbol{q}}_{w b} \otimes \boldsymbol{r}^{b}+\boldsymbol{q}_{w b} \otimes \dot{\boldsymbol{r}}^{b} \end{aligned}\\$

由于

$\begin{array}{l} \dot{\boldsymbol{r}}^{b}=-\boldsymbol{\omega}_{w b}^{b} \times \boldsymbol{r}^{b}=-\boldsymbol{\omega}_{w b}^{b} \otimes \boldsymbol{r}^{b} \\ \dot{\boldsymbol{r}}^{w}=\mathbf{0} \end{array}\\$

则有

$\begin{aligned} & \boldsymbol{r}^{w} \otimes \dot{\boldsymbol{q}}_{w b} \\ =&\left(\boldsymbol{q}_{w b} \otimes \boldsymbol{r}^{b} \otimes \boldsymbol{q}_{w b}^{*}\right) \otimes \dot{\boldsymbol{q}}_{w b} \\ =& \dot{\boldsymbol{q}}_{w b} \otimes \boldsymbol{r}^{b}-\boldsymbol{q}_{w b} \otimes \boldsymbol{\omega}_{w b}^{b} \otimes \boldsymbol{r}^{b} \end{aligned}\\$

$\begin{aligned} \boldsymbol{r}^{b} \otimes \boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b} =\boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b} \otimes \boldsymbol{r}^{b}-\boldsymbol{\omega}_{w b}^{b} \otimes \boldsymbol{r}^{b} \end{aligned}\\$

移项可得

$\boldsymbol{\omega}_{w b}^{b} \otimes \boldsymbol{r}^{b}=\left(\boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b}\right) \otimes \boldsymbol{r}^{b}-\boldsymbol{r}^{b} \otimes\left(\boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b}\right)\\$

利用前面四元数相乘展开成矩阵与向量相乘的公式，将上式展开，可得

$\left[\begin{array}{cc} 0 & \mathbf{0}_{1 \times 3} \\ \mathbf{0}_{3 \times 1} & {\left[\boldsymbol{\omega}_{w b}^{b}\right]_{\times}} \end{array}\right]\left[\begin{array}{l} 0 \\ \boldsymbol{r}^{b} \end{array}\right]=\left[\begin{array}{cc} 0 & \mathbf{0}_{1 \times 3} \\ \mathbf{0}_{3 \times 1} & 2\left[\left(\boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b}\right)_{v}\right]_{\times} \end{array}\right]\left[\begin{array}{c} 0 \\ \boldsymbol{r}^{b} \end{array}\right]\\$

因此有

$\left(\boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b}\right)_{v}=\frac{1}{2} \boldsymbol{\omega}_{w b}^{b}\\$

以上只是把虚部求解出来了，实部还得搞一搞，如下

根据四元数与旋转矢量的关系，有

$\boldsymbol{q}_{w b}^{*}=\left[\begin{array}{c} \cos \frac{\phi}{2} \\ -\boldsymbol{u} \sin \frac{\phi}{2} \end{array}\right]\\$

$\dot{\boldsymbol{q}}_{w b}=\left[\begin{array}{c} -\frac{\dot{\phi}}{2} \sin \frac{\phi}{2} \\ \dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\phi}{2} \cos \frac{\phi}{2} \end{array}\right]\\$

因此有

$\begin{aligned} \boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b} &=\left[\begin{array}{c} \cos \frac{\phi}{2} \\ -\boldsymbol{u} \sin \frac{\phi}{2} \end{array}\right] \otimes\left[\begin{array}{c} -\frac{\dot{\phi}}{2} \sin \frac{\phi}{2} \\ \dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\phi}{2} \cos \frac{\phi}{2} \end{array}\right] \\ &=\left[\begin{array}{c} -\frac{\dot{\phi}}{2} \sin \frac{\phi}{2} \cos \frac{\phi}{2}+\left(\boldsymbol{u} \sin \frac{\phi}{2}\right)^{\mathrm{T}}\left(\dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\Phi}}{2} \cos \frac{\phi}{2}\right) \\ \cos \frac{\dot{\phi}}{2}\left(\dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2} \cos \frac{\phi}{2}\right)+\boldsymbol{u} \sin \frac{\phi}{2} \cdot \frac{\dot{\phi}}{2} \sin \frac{\phi}{2}-\left(\boldsymbol{u} \sin \frac{\phi}{2}\right) \times\left(\dot{\boldsymbol{u}} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2} \cos \frac{\phi}{2}\right) \end{array}\right] \\ &=\left[\begin{array}{c} 0 \\ \boldsymbol{u} \cos \frac{\phi}{2} \sin \frac{\phi}{2}+\boldsymbol{u} \frac{\dot{\phi}}{2}-\boldsymbol{u} \sin \frac{\phi}{2} \times \dot{\boldsymbol{u}} \sin \frac{\phi}{2} \end{array}\right] \end{aligned}\\$

可以看出实部为0。

那么最终就可以得到

$\boldsymbol{q}_{w b}^{*} \otimes \dot{\boldsymbol{q}}_{w b}=\frac{1}{2}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_{w b}^{b} \end{array}\right]\\$

即

$\dot{\boldsymbol{q}}_{w b}=\boldsymbol{q}_{w b} \otimes \frac{1}{2}\left[\begin{array}{c} 0 \\ \boldsymbol{\omega}_{w b}^{b} \end{array}\right]\\$

3.旋转矢量微分方程

旋转矢量微分方程的推导就显得更加复杂，这里就借用严恭敏老师《惯性导航与组合导航算法》中的一个中间结论吧，直接给出旋转矢量微分方程的完整形式

$\dot{\boldsymbol{\phi}}=\boldsymbol{\omega}_{w b}^{b}+\frac{1}{2} \boldsymbol{\phi} \times \boldsymbol{\omega}_{w b}^{b}+\frac{1}{\phi^{2}}\left(1-\frac{\phi}{2} \cot \frac{\phi}{2}\right)(\boldsymbol{\phi} \times)^{2} \boldsymbol{\omega}_{w b}^{b}\\$

如果你愿意把它泰勒展开，那么去除高阶项会得到如下简化形式

$\dot{\boldsymbol{\phi}}=\boldsymbol{\omega}_{w b}^{b}+\frac{1}{2} \boldsymbol{\phi} \times \boldsymbol{\omega}_{w b}^{b}\\$

这就是前面看到的结果啦。

四、总结

搞公式累到吐，等想填了再来啰嗦几句。。。。。。

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

任乾

时隔多日，开始填坑，催稿催的我心慌慌～～～

一、概述

二、惯性导航解算的目的

1.姿态描述

三、惯性导航解算的方法

1.基于旋转矩阵的姿态更新

2.基于四元数的姿态更新

3.速度更新

4.位置更新

三、解算方法的来源

1.旋转矩阵微分方程

2.四元数微分方程

3.旋转矢量微分方程

四、总结

为你推荐

我手写了个SLAM算法！

PCL中常用的高级采样方法

r2live论文阅读

ORB_SLAM2安装调试过程

机器人导航必备的栅格地图数学模型及使用

ubuntu16.04下用笔记本摄像头和ROS编译运行ORB_SLAM2的单目AR例程

评论（0）

关于作者

任乾

65

0

6

3

DSO源码解读(三)：后端优化

语义SLAM | 深度学习用于特征提取 : SuperPoint（三）

多传感器融合定位理论基础（二）：惯性器件综述

相关推荐

论文精读 《CaDNN: Categorical Depth Distribution Network for Monocular 3D Object Detection》

Autoware 笔记No.7, CNN障碍物检测（CNN LiDAR Baidu Object Segmenter）

从零开始搭二维激光SLAM --- 前言

占据栅格地图构建（Occupancy Grid Map）

常见的3d bounding box标注工具

机器人环境感知研究现状简述

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

论文精读《CaDNN: Categorical Depth Distribution Network for Monocular 3D Object Detection》

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板