自动驾驶融合定位_ IMU预积分模型及应用

自动驾驶融合定位: IMU预积分模型及应用

附赠自动驾驶学习资料和量产经验：链接

一、概述

在上一篇文章中，我们给出了下面的框图中实现框图对应的理论部分，但是也提到，虚线对应的部分太过复杂，当时没有介绍。

下面来分析一下具体的原因。带IMU的融合模型中的优化问题可以表示为

三种约束分别通过以下方式获得：

1) 激光里程计约束：使用激光里程计，计算每个关键帧位姿，进而得到相对位姿；

2) IMU约束：在上一个关键帧位姿基础上，进行惯性积分，从而得到两关键帧相对位姿；

3) RTK约束：直接测量得到。

此时融合流程可以用下面的流程图表示

这个流程图中，红色部分是本篇文章新增的IMU约束对应的环节，剩下的部分就是上一篇文章对应的流程了。所以从这个框图中也可以看出这两篇文章在内容上的分工了。

二、预积分问题引入

这种融合模型的核心问题在于，IMU的约束是通过惯性积分得到的，而惯性积分是以它前一时刻的位姿为初始值得到的，但是位姿每次优化后会发生变化，这导致其后的IMU惯性积分就要重新进行，显然运算量过大。

这个问题的解决思路就是直接计算两帧之间的相对位姿，而不依赖初始值影响，即所谓的预积分。本篇文章剩余的篇幅就几乎全在讨论预积分的问题。

三、预积分模型介绍

在前面的惯性导航文章中，已知导航的微分方程如下

根据该微分方程，可知从 i 时刻到 j 时刻 IMU的积分结果为

根据预积分的要求，需要求相对结果，而且不依赖于上一时刻位姿，因此需要对上式做转换。

以上三个式子中，积分符号里面的项，就是需要预积分的项，可见，他们已经完全和i时刻的状态无关了。

为了整理公式，把积分相关的项用下面的式子代替

实际使用中使用离散形式，而非连续形式，由于在解算中，一般采用中值积分方法，即

那么预积分的离散形式可以表示为

经过以上的推导，此时状态更新的公式可以整理为

需要注意的是，陀螺仪和加速度计的模型为

即认为bias是在变化的，这样便于估计不同时刻的bias值，而不是整个系统运行时间内都当做常值对待。这更符合低精度mems的实际情况。但在预积分时，由于两个关键帧之间的时间较短，因此认为i和j时刻的bias相等。

需要注意的一点是，预积分的结果中包含了bias，在优化过程中，bias作为状态量也会发生变化，从而引起预积分结果变化。为了避免bias变化后，重新做预积分，可以把预积分结果在bias处泰勒展开，表达成下面的形式，这样就可以根据bias的变化量直接算出新的预积分结果。

此处暂时只直接给出以上各雅可比的结果，它的推导放在后面进行。

四、预积分在优化中的使用

1.使用思路

1）凸优化回顾

即，在优化中使用一项信息，需要设计残差，并推导它的雅可比和方差

2）预积分的使用

按照优化的套路，在优化中使用预积分，需要：

a.设计残差

b.推导残差关于待优化变量的雅可比

c.计算残差的方差

除此以外，预积分中还多一项计算，即推导bias变化时，预积分量重新计算的方法(上面提到过的)。

2. 残差设计

在优化时，需要知道残差关于状态量的雅可比。由于已知姿态位姿更新的方法如下

因此，可以很容易写出一种残差形式如下：

但是和预积分相关的量，仍然与上一时刻的姿态有关，无法直接加减，因此，把残差修正为以下形式

待优化的变量是

但在实际使用中，往往都是使用扰动量，因此实际是对以下变量求雅可比

3. 残差雅可比的推导

此处只对几个比较复杂的雅可比进行推导（放在附录A中），其余比较简单，感兴趣的可自行完成。

4. 预积分方差计算

4.1 核心思路

4.2 连续时间下的微分方程

4.2 连续时间下的微分方程

推导方法已经在之前的文章中介绍过，此处直接给出结果

4.3 离散时间下的传递方程

由于推导较为复杂，此处给出结论（如下公式），推导过程放在附录B中。

其中

5. 预积分更新

回到前面遗留的问题，即bias变化时，预积分结果怎样重新计算的问题，现在是时候来解决它了。再次给出它的泰勒展开形式

这里，雅可比没有明确的闭式解，但是在推导方差的更新时，我们得到了

通过该递推形式，可以知道

即预积分结果的雅可比，可以通过这种迭代方式计算。

五、总结

可以看出，预积分的推导非常复杂，但只要耐心下来，把思路捋清楚，还是比较容易弄懂的，就是太费纸了。

==附录A====

附录A 残差雅可比推导

1. 姿态残差的雅可比

1)对i时刻姿态误差的雅可比

上式可以化简为

2)对j时刻姿态误差的雅可比

3)对i时刻陀螺仪bias误差的雅可比

2. 速度残差的雅可比

1)对i时刻姿态误差的雅可比

2)对i时刻速度误差的雅可比

3)对i时刻加速度bias误差的雅可比

3.位置残差的雅可比

位置残差的雅可比比较简单，不在这里推了，照猫画虎自己推一下就行。

==附录B====

附录B 离散时间递推方程推导