自动驾驶融合定位系列教程四：惯性导航解算

一、概述

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

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

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

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）旋转矩阵定义

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

假设旋转前，载体系(b系)的单位正交基为 (𝑒1,𝑒2,𝑒3) ，旋转后对应的单位正交基为 (𝑒1′,𝑒2′,𝑒3′) ，在世界坐标系(w系，不随载体的旋转而旋转)下有向量 𝑎 ，它在旋转前后两个坐标系中的坐标分别为 [𝑎1,𝑎2,𝑎3]T 和 [𝑎1′,𝑎2′,𝑎3′]T ，那么有

2）旋转矩阵性质

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

1.3 四元数

1）四元数定义

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

先从复数开始讲起，可能更容易理解些。我们知道一个实数，可以对应一个坐标轴上的一个点，姑且写作 𝐴=𝑥 吧，后来又有了复数 𝐴=𝑎+𝑏𝑖 ，发现它不仅可以描述坐标，还可以描述方向了 𝜃=𝑎𝑟𝑐𝑡𝑎𝑛(𝑏𝑎) ，所以这种东西在信号处理里面经常用，这个角度就对应信号的相位。这里可能会自然而然产生一个疑问，我直接用向量描述不也是一样的吗，比如写作 𝐴=(𝑎,𝑏) ，不照样也能得到角度吗？这个问题的核心在于向量不可向复数那样运算，即复数有 𝑖2=−1 的性质，因此两个复数相乘，就有了

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

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

为了弄明白这个“再加一维”，我们可以换一个角度来看它，若另 𝐴=𝑎+𝑏𝑖 , 𝐵=𝑐+𝑑𝑖 ，会发现

在这里，另 𝑘=𝑖𝑗 ，那上式不就是四元数了嘛

因此，从 𝑄=𝐴+𝐵𝑗 可以看出，四元数可以理解为“复数的复数”。

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

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

2）四元数性质

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

a. 单位四元数

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

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

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

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

c. 四元数乘法

四元数乘法定义为

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

其中

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

1.4 等效旋转矢量

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

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

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

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

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

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

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

1）欧拉角与旋转矩阵

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

其中

2) 四元数与旋转矩阵

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

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

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

三、惯性导航解算的方法

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

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

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

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

1) 积分形式介绍

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

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

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

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

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

2）离散时间处理

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

a.欧拉法

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

b.中值法

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

c.龙哥库塔法

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

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

1）积分形式介绍

2) 离散时间处理

3.速度更新

4.位置更新

三、解算方法的来源

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

1.旋转矩阵微分方程

2.四元数微分方程

3.旋转矢量微分方程

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

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

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