两列数值对比取公式_VIO中的IMU数值积分与IMU预积分

电玩女神 2023-01-07 15:26 258阅读 0赞

**1. 数值积分基础**

### **1.1 定义** ###

给定微分方程

![Image 1][] 以及初值

![Image 1][], 求

![Image 1][]时刻后的

![Image 1][]

举例：假设已知当前的位置

![Image 1][]和加速度曲线

![Image 1][]，则位置曲线为

![Image 1][]。实际过程我们并没有

![Image 1][]的表达式，只能获得传感器检测到的离散采样值，所以需要利用离散积分尽可能逼近真实的连续积分。

怎么逼近呢? 基本思路是：将

![Image 1][]时间段内的微分

![Image 1][]用一个恒定的值

![Image 1][]来近似（即假设曲线

![Image 1][]在

![Image 1][]时间段内近似为一条直线）则数值积分公式为：

![Image 1][]

既然是逼近, 那势必会有近似误差,如何让近似误差尽可能小呢?

*  首先是**让离散间隔**
    
    **![Image 1][]尽可能小**
    
    。如果
    
    ![Image 1][]趋近无穷小, 那么离散积分就趋近于真实连续积分.
    
    ![Image 1][]取决于传感器的采集频率. 所以, 提高传感器采样频率是提高数值积分精度的最直接的方法.
 *  其次是**计算准确的“斜率**
    
    **![Image 1][]**
    
    ” 。根据
    
    ![Image 1][]计算的不同, 工程上有三种数值积分方法: 欧拉积分, 中点积分和龙格－库塔(Runge-Kutta)积分.

### **1.2 数值积分方法** ###

**1.2.1 欧拉积分**

欧拉积分利用

![Image 1][]时刻的斜率来作为

![Image 1][]整段的斜率(前向欧拉)：

![Image 1][]

欧拉积分计算量很小，但近似误差会比较大。

**1.2.2 中点积分**

中点积分利用中点

![Image 1][]时刻的斜率

![Image 1][] 来近似整段斜率：

![Image 1][]

因为

![Image 1][]是未知的，所以先通过欧拉积分近似

![Image 1][]。用中点斜率来近似整段斜率显然比比欧拉积分用端点斜率来近似要更合理。

**1.2.3 龙格库塔积分**

常用的是4阶龙格-库塔积分, 即RK4积分. 它将多个斜率加权来近似整段斜率：

![Image 1][]

![Image 1][]是起始点的斜率，

![Image 1][]都是中点斜率的“近似”，

![Image 1][]是结束点斜率的“近似”。注意这里的近似，中点和结束点的真实斜率未知，这是由于中点和结束点的真实

![Image 1][]是未知的，

*  ![Image 1][]中近似为
    
    ![Image 1][], 相当于前向欧拉积分, 取的
    
    ![Image 1][]的前点斜率;
 *  ![Image 1][]中近似为
    
    ![Image 1][], 相当于后向欧拉积分, 取的
    
    ![Image 1][]的后点斜率(注意, 这里用
    
    ![Image 1][]来近似后点斜率);
 *  ![Image 1][]中近似为
    
    ![Image 1][], 相当于中点积分, 取的中点
    
    ![Image 1][]的斜率. (注意, 这里用
    
    ![Image 1][]来近似中点斜率).

RK4加权将中点斜率

![Image 1][]的权重设为2，两端的斜率

![Image 1][]设为1。它的近似精度更高，但显然计算量也更大。

*RK4中的k1就是欧拉法的斜率，k2就是中点法的斜率。*

### **1.3 对比实测** ###

取最简单的微分方程

![Image 1][], 积分时间为

![Image 1][]，该微分方程的解析解为

![Image 1][]。

*  **不同**
    
    **![Image 1][]的积分精度对比**

黑色为ground truth,

![Image 1][]时积分误差还是很大的, 随着

![Image 1][]越来越小, 积分曲线会越来越接近真实曲线,

![Image 1][]时已经比较接近了, 但仍可以看出一点误差, 但

![Image 1][]时, 基本上接近真实值. 值得一提的是，不论

![Image 1][]多小，当积分时间趋于无穷大时，积分误差也会趋于无穷大。

![d794f54ec98452aac3f95e2cf7d99f5c.png][]

不同Delta t的积分精度对比

*  **不同数值积分方法的积分精度对比(**
    
    **![Image 1][])**

为了让差距更明显, 将

![Image 1][], 欧拉法,中点法,RK法的精度依次提高.

![77e70918e804f999b81eb403ad5ffe44.png][]

不同数值积分方法的积分精度对比(Delta t = 1)

*  **不同数值积分方法的积分精度对比(**
    
    **![Image 1][])**

将

![Image 1][]缩小到0.1, 中点法和RK4的积分曲线已经基本接近真实值, 欧拉法仍会有明显偏差.

![8b3d5637d5b3512bc5b57027db0095bb.png][]

不同数值积分方法的积分精度对比(Delta t = 0.1)

*  **不同数值积分方法的积分精度对比(更复杂的微分方程)**

测试更复杂的微分方程

![Image 1][], 依然RK4的精度最高.

![467278c0539c34c60e26a0e610583660.png][]

不同数值积分方法的积分精度对比(更复杂的微分方程)

## **2 IMU数值积分** ##

### **2.1 姿态积分** ###

**2.1.1 理论推导**

姿态四元数定义如下：

![Image 1][]

其中

![Image 1][]为单位旋转向量,

![Image 1][]为旋转角度. 对于无穷小量, 有

![Image 1][]

其中

![Image 1][]为旋转向量.

角速度定义：

![Image 1][]

姿态微分方程：

![Image 1][]

姿态0阶积分公式：假设

![Image 1][]在

![Image 1][]时间内为常量

![Image 1][]

具体推导参见

Indirect Kalman Filter for 3D Attitude Estimationmars.cs.umn.edu

**2.1.2 测试结果**

陀螺仪检测的是角速度

![Image 1][], 真实的角速度需要减去陀螺仪bias和传感器误差

![Image 1][], 高斯白噪声

![Image 1][]的估计值为0，在积分过程中被忽略掉，但在协方差迭代中会考虑，

![Image 1][]通常无法忽略， 一般会放到滤波算法中进行估计。下图是EuRoC数据的姿态估计结果，假设

![Image 1][]恒定，用静止时刻100帧数据求均值作为

![Image 1][]，陀螺仪数据

![Image 1][]，可以看出姿态估计的偏移误差其实比较小，110多秒旋转误差角度不到10度，平均

![Image 1][]。

![7df46091e30c8ee2103065778394b8f4.png][]

姿态四元数积分结果(积分四元数曲线、真实四元数曲线、原始陀螺仪数据、姿态积分误差)

### **2.2 速度位置积分** ###

**2.2.1 理论推导**

IMU加速计获取的是加速度值，需要先积分得到速度，再对速度积分得到位置。

微分方程为：

![Image 1][]

速度积分(RK4)：假设

![Image 1][]时间内的加速度观测是恒定的

![Image 1][]

位置积分(RK4)：

![Image 1][]

**2.2.2 测试结果**

同样使用EuRoC数据进行测试，加速度bias用的EuRoC实时估计值， 陀螺仪bias用静止陀螺仪数据估的(因为EuRoC估的bg好像很不准)，由于速度和位置估计很容易积飞，短时间就能发散到特别大， 所以限制积分时间t，每隔t时间重新以ground truth为初值进行新的估计。统计每段时间内最后时间的位置、速度误差，并对所有段求平均。注意：位置和速度实时估计需要用到姿态，所以姿态也同时积分，具体方法同2.1。

![1b36f81cf0edc54afd914d8c19ec72d4.png][]

![d9469a90fce354d82539b7408abc2581.png][]

结论：

1.  姿态的累积误差最小，速度累积误差较大，位置累积误差最大。
2.  速度每秒偏移量相对恒定，平均每秒偏移0.1m/s，但随着积分时间增大，每秒偏移量也会变大。
3.  位置每秒偏移量随积分时间增大而增大(近似线性增长)，所有位置累积误差随时间成指数上升。积分时间1s的情况下，累积偏移0.05m，但积分时间增到10秒后，累积偏移量高达5.62m。
4.  姿态积分精度很高，积分时间1s和5s，甚至一直积分(见2.1.2)，积分误差都很小。
5.  尝试了欧拉法和中点法做IMU积分，发现误差相差不大，怀疑是积分间隔足够小(5ms)或者运动变化相对缓慢。

## **3. IMU预积分** ##

IMU积分是指根据IMU传感器获取的加速度

![Image 1][] 和角速度

![Image 1][], 给定IMU bias

![Image 1][], 在初始的

![Image 1][]基础上不断积分得到

![Image 1][]. IMU积分公式如下:

![ee4bd66eb29d4f6230bd655a238bd240.png][]

这里假设加速度观测

![Image 1][]和旋转矩阵

![Image 1][]在

![Image 1][]时间内是不变的.

观察积分公式我们发现

![Image 1][]依赖于初值

![Image 1][]. 如果

![Image 1][]时刻的状态发生改变,

![Image 1][]时刻的状态必须重新计算. 在基于优化的VIO算法中, 状态每次迭代都会改变, 如果每次迭代都重新积分,那么计算量会非常大.

整个积分公式中最耗时的就是积分项, 预积分的目的就是想将这部分预先计算, 但这部分依赖于初始旋转

![Image 1][], 即

![Image 1][]时刻在World系下的姿态. 为了消除对

![Image 1][]的依赖, 将积分项乘上

![Image 1][], 积分项中的旋转矩阵变为了

![Image 1][], 而初值

![Image 1][]是单位阵. 这样就解除了初值依赖, 从而可以事先计算. 所以对积分公式的等式左右两边同时乘以

![Image 1][], 得到

![2f38bf54afc4665a135d6747cd80c4ce.png][]

![4833e3fcdee70c415313e6c5b191d805.png][]

![Image 1][]就是不依赖于初始状态的预积分项. 原先是在World系下进行

![Image 1][]的积分, 现在转成了

![Image 1][]系下进行

![Image 1][]积分. 优化过程中如果

![Image 1][]发生变化, 通过上述公式, 只需少量乘法和加法运算就可以快速求出

![Image 1][]

**注意：预积分是将迭代公式中的积分项拿出来预先计算，初值**

**![Image 1][]变化时,**

**![Image 1][]仍然需要重新计算。假设**

**![Image 1][]之间有N组IMU数据，原来积分需要N次迭代计算，现在只需要一次计算，计算量大大化简。**

预积分项的离散迭代公式如下, 初值

![Image 1][]都为0(或单位旋转).

![1a190994fbcb0d45941c064e7940ae28.png][]

## **参考** ##

1.  欧拉积分、中点积分与龙格－库塔积分
2.  IMU预积分: Tong Q, Li P, Shen S. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator\[J\]. IEEE Transactions on Robotics, 2017, PP(99):1-17.
3.  IMU RK4积分: KumarRobotics/msckf\_vio

[Image 1]: 
[d794f54ec98452aac3f95e2cf7d99f5c.png]: /images/20221119/6b560b923e984b2aab6e2bbcc9f3b77d.png
[77e70918e804f999b81eb403ad5ffe44.png]: /images/20221119/dd0fc442b52642389b906a126fcc188c.png
[8b3d5637d5b3512bc5b57027db0095bb.png]: /images/20221119/6d31b63ecdd54e398e146f1fd8ff56bc.png
[467278c0539c34c60e26a0e610583660.png]: /images/20221119/e7e9b6ecfdc44596970497a4470fafc6.png
[7df46091e30c8ee2103065778394b8f4.png]: /images/20221119/6318eeb839e24337b0f2b82a42aeb5f3.png
[1b36f81cf0edc54afd914d8c19ec72d4.png]: /images/20221119/fea09a97c3ec4e548599622d5dac6bf7.png
[d9469a90fce354d82539b7408abc2581.png]: /images/20221119/f6a2a20975454055b32e0ef8474a05d3.png
[ee4bd66eb29d4f6230bd655a238bd240.png]: /images/20221119/e4f11c306c5f42d6b1b63f0a2a3cb826.png
[2f38bf54afc4665a135d6747cd80c4ce.png]: https://img-blog.csdnimg.cn/img_convert/2f38bf54afc4665a135d6747cd80c4ce.png
[4833e3fcdee70c415313e6c5b191d805.png]: https://img-blog.csdnimg.cn/img_convert/4833e3fcdee70c415313e6c5b191d805.png
[1a190994fbcb0d45941c064e7940ae28.png]: https://img-blog.csdnimg.cn/img_convert/1a190994fbcb0d45941c064e7940ae28.png