预积分总结与公式推导20180827.pdf_预积分

需积分: 27 626 浏览量更新于2023-03-03 评论 1 收藏 475KB PDF 举报

身份认证购VIP最低享 7 折!

领优惠券(最高得80元）

资源详情

资源评论

资源推荐

邱笑晨 ares43490@126.com

1



本文作者为北航自动化学院在读博士邱笑晨，如有疑问和建议请发邮件至：ares43490@126.com

全文已授权泡泡机器人转载，如需转载请联系泡泡机器人（刘富强，liufuqiang_robot@hotmail.com）

〇、一些预备知识的科普

这部分内容根据在泡泡机器人的四期推送中正文前的絮叨以及与小伙伴们的互动整理

而来。

IMU 预积分技术最早由 T Lupton 于 12 年提出[1]，C Forster 于 15 年[2][3][4]将其进一

步拓展到李代数上，形成了一套优雅的理论体系。Forster 将 IMU 预积分在开源因子图优化

库 GTSAM 中进行了实现，并完成了和其另一大作 SVO 的组合。这套理论目前已经被广泛

的应用在基于 Bundle Adjustment 优化框架的 Visual Inertial Odometry 中。其中包括

VI-ORBSLAM，港科大 VINS，百度/浙大 ICE-BA 等。

本报告对 Foster 的 paper[3][4]中的公式进行了详尽的推导，试图将这套优雅的理论详细

地展现在读者面前，使读者对 IMU 预积分理论有更加完备的认识。

为了更好的理解本系列报告，推荐另外三本参考书目[5][6][7]。其中[6]可作为本文惯性

导航部分知识的详细参考。

下面先简单阐述一下传统捷联惯性导航的基本思想。

惯性导航的核心原理基于牛顿第二定律，即位置的导数等于速度，速度的导数等于加速

度。如果我们假设参考坐标系下载体的初始速度和初始位置已知，利用载体运动过程中参考

系下的加速度信息，就可以不断地进行积分运算，更新实时的速度和位置。这基本上是中学

的知识了，是不是很简单？

当然了，这是理想的理论情况，实际的情况是，加速度是由与载体固连的加速度计测量

得到的（会跟随载体转动），每一时刻的加速度都是在当前的载体系下得到的，进行速度和

位置的积分，前提是把这些加速度都统一到同一个坐标系下。更新姿态的作用就在于此——

通过实时更新姿态，我们可以求得当前载体系相对于参考系的姿态，从而将载体系下的加速

度测量投影到参考系下。

一个更麻烦的情况是，由于加速度计的测量原理，它敏感到的“加速度”实际上不是纯加

速度，而是包含了反向重力加速度的比力。举两个例子帮助大家理解：假如一个三轴加速度

计放在水平面上，那么它的输出将是铅垂线反向的 9.8m/s^2；如果一个加速度计做自由落体

运动，那么它的输出会是 0。总而言之，加速度计无法敏感到重力（所以静止时才会有反向

重力加速度输出）。因为这个麻烦的情况，捷联惯导中的姿态一般需要相对于水平地理系来

表示（也就是说将水平地理系选作参考系），这样才好补偿重力加速度（因为我们都知道水

平地理系下的正向重力加速度就是[0,0,-9.8]，以东北天系为例）。

最后，传感器总是存在噪声的，惯性导航这种积分运算，必然使得 IMU

器件中的测量

噪声不断的累积，从而造成定位和姿态误差。

好了，如果掌握了前面的内容，对于理解 IMU 预积分理论所需的惯导知识就已经足够

了。至于捷联导航的更多复杂内容，比如对地球自转的处理，以及地速与绝对速度等概念就

不做展开了，如果感兴趣可以去找本捷联惯导的书看看（比如[6]）。

下面介绍传统捷联导航算法与预积分算法的联系。

传统捷联惯性导航的递推算法，以初始状态为基础，利用 IMU 测量得到的比力和角速

度信息进行积分运算，实时更新载体的位姿及速度等状态（详见第二部分运动模型），根据

这种思路，如果知道上一帧图像采样时刻载体的位姿和速度，则可以根据两帧之间的 IMU

测量（角速度和比力）递推得到当前帧的位姿和速度。需要注意的是，传统的惯导解算中非

常重要的一个问题是处理重力，由于加计的测量特性，其测量值为包含反向重力的比力，而

不是纯加速度。这使得一旦姿态不准确，重力投影误差将对速度和位置积分产生严重影响。

在基于 BA 的视觉惯性融合算法中，各个节点的载体状态都是有待优化的量。IMU 预

积分的初衷，是希望借鉴纯视觉 SLAM 中图优化的思想，将帧与帧之间 IMU 相对测量信息

转换为约束节点（载体位姿）的边参与到优化框架中。IMU 预积分理论最大的贡献是对这

些 IMU 相对测量进行处理，使得它与绝对位姿解耦（或者只需要线性运算就可以进行校正），

从而大大提高优化速度。另外，这种优化架构还使得加计测量中不受待见的重力变成一个有

利条件——重力的存在将使整个系统对绝对姿态（指相对水平地理坐标系的俯仰角和横滚角，

不包括真航向）可观。要知道纯视觉 VO 或者 SLAM 是完全无法得到绝对姿态的。

邱笑晨 ares43490@126.com

2



此外，由于传感器测量误差的存在，无论是纯惯导还是纯 VO 解算，单纯依靠递推运算

不可避免的将带来累积误差（低精度 IMU 会极快发散）。将两种传感器融合可以利用冗余测

量（例如两种方式都可以求取相对位姿）来抑制累积误差。同时，IMU 和视觉这两种不同

源的测量，也使得 IMU 的 bias 可观，从而可以在优化中被有效估计。另外老生常谈的纯单

目视觉缺乏绝对尺度的问题，也可以由惯性信息的引入而得以解决。

接下来将进入推导部分，相比在泡泡机器人的推送，更正了 SO(3)的右 Jacobian 的逆的

表达式（括号中的“+”更正为“-”）；补充了“Adjoint 性质”的证明部分。

一、关于李群流形的一些基本概念和性质

由于预积分中的概念就是在李群流形上推导的，有必要对相关知识进行介绍。这部分介

绍一些预积分的描述和公式推导时会用到的，关于李群流形的基本概念和性质。

·李群要满足的基本性质等可参见高博的《视觉 SLAM 十四讲》，更进阶的知识可参考《State

Estimation for Robotics》。

·特殊正交群 SO(3)是李群的一种，其元素为旋转矩阵（方向余弦阵，3×3 正交阵），其对应

的李代数为 so(3)，其元素为 3×3 的反对称阵（注意并不是 3 维实向量，不过每个 3 维实向

量和一个 3×3 反对称阵一一对应，后面会看到有时也用 3 维实向量来指代 so(3)的元素）。

·hat 运算符



把 3 维实向量映射为 3×3 反对称阵；vee 运算符



把 3×3 反对称映射为 3 维实

向量；这对运算符构成了 3×3 反对称阵和 3 维实向量间的双射，如下式：

132

23 1

321

32 1

312

21 3



 



 



 







  

  

 

  



  















ω W

W ω

·关于 hat 运算符的一个性质（

为所有 3 维实向量的集合）：

, ,



   ab ba ab

·指数映射



exp  将 so(3)中的元素映射到 SO(3)上：













sin 1 cos

exp











 I







当





是小量时，有一阶近似如下：





exp







I



·对数映射



log  将 SO(3)中的元素映射到 so(3)上：









log

2sin







其中



cos

tr R













。有



log







Ra

，



为旋转角，

为旋转轴单位矢量，有



2sin















。

·当









（或





被限制在其它任意一个连续的



范围内）时，指数映射和对数映射

邱笑晨 ares43490@126.com

3



构成 SO(3)和 so(3)间的双射。



exp  和



log  是 SO(3)（3×3 正交阵）与 so(3)（3×3 反对称阵）之间的映射，为了后

续行文的符号简明，下面用 3 维实向量来指代 so(3)的元素（依据 3 维实向量和 3×3 反对称

一一对应），定义新的指数映射和对数映射：







Exp: exp (3)

Log :SO(3) log

RSO







 



RR



注意







exp Exp ，







log Log ，首字母大写表示 SO(3)和

之间的映射。

·对于 3 维实向量





和一个小量







，有下述近似性质：















  



Exp Exp Exp

Log Exp Exp



   



   



  



 

  



 

式 1 可以这样理解：

中的向量





加上一个小量







，对应到 SO(3)中则是



Exp





右乘一

个



Exp





J



；

式 2 则可这样理解：SO(3)中





Exp





右乘一个





Exp





，对应到

中则是





加上一项













J



。







是 SO(3)的右 Jacobian，将切空间的“加性项”和 SO(3)中的右“乘性项”联系在一

起。







及其逆









的表达式如下：













1 cos sin

1cos

2sin







 







 



 







  

















 





·指数映射的 Adjoint 性质：











  

Exp exp Exp

Exp Exp







 



RRRRR

RR R

 



下面证明第一行等式。首先，对于任意旋转矩阵

和三维矢量





，有如下等式成立：









RRR





该等式在《State Esitimation for Robotics》第 P227 页被引用。它的一个简单证明可参照郑帆

大佬的博客（https://fzheng.me/2017/12/10/Rvhat/）。

令





 a



，其中



为模值，

为单位矢量。参照《视觉 SLAM 十四讲》第 P70 页式(4.22)

有（即罗德里格斯公式）













Exp exp exp

cos 1 cos sin

 









 

Iaaa





则 Adjoint 性质的左边可以开始变形：

邱笑晨 ares43490@126.com

4





  



Exp

cos 1 cos sin

exp exp

TTTT





 









    



  

  



RI aa aR

RR Raa R Ra R

IRaRaRa

Ra R





此时，考虑









RRR



，则有













exp exp





RRR



；考虑





Exp: exp (3)RSO





 



，则有













exp Exp





RR



。

证毕。

二、IMU 器件测量模型（Sensor Model）和运动学模型（Kinetic Model）

·陀螺测量模型：















wb wb g g

ttttωωb η



其中

是随时间缓慢变化的 bias，

是白噪声。该模型利用了 Static World Assumption，后

文会介绍。

·加计测量模型：















bwTww

baa

tttfRagbη

其中

是随时间缓慢变化的 bias，

是白噪声。

·可以看到，这里对加计和陀螺的测量模型建模都比较简单，没有过细的考虑马尔科夫过程

等其他误差项的建模，这与 SLAM 中一般使用精度较低的 mems 器件有关。

· Static World Assumption：由于 SLAM 的运行场景一般比较小（重力变化不大），运行时

间也不会太长，且使用的 IMU 器件一般为 mems 器件（精度低，陀螺仪静置时无法敏感地

球自转）。因此不会像传统的捷联 INS 解算中，考虑地球自转、根据位置更新重力矢量等，

常常忽略地球自转（认为地球是个 static world），并假设运行区域水平面是个平面，重力矢

量

的指向固定且模值恒定。在此假设下的 VISLAM 或 VIO 中，世界坐标系 w 被认为是

一个惯性系，且一般会选择初始时刻水平地理系，前面的陀螺仪测量模型中即利用了 w 系

是惯性系的假设。下面的运动模型，都是基于这个 assumption 来进行推导的。

·运动模型的微分方程形式如下：



wwb

bbwb





RRω



（来源可参考秦永元《惯性导航》第一版P238）

使用欧拉积分（Euler Integration，三角形积分）可得到运动方程的离散形式如下：

 











   



Exp

wwb

bt t bt

www

www w

tt t tt

tt t tt tt





   

     

RRω

vva

ppv a

★【其中，对 Rotation 更新方程做详细解释如下（参考秦永元《惯性导航》第 1 版 P292。

9.2.2 节）：



tω 表示 t 时刻“角速度矢量”在 b 系下的坐标



邱笑晨 ares43490@126.com

5





ttω 表示“旋转矢量”在 b 系下的坐标，又有





Exp



相当于罗德里格斯公式

（Rodrigues’ Rotation Formula）















Exp

bt t



 ω R





  







Exp

wb w w

bt bt bt t bt t

 

  R ω RR R 】★

注意到，IMU 预积分的理论是建立在欧拉积分的基础上的，并不是捷联惯导中传统的 LK4

等积分方式。

·为了符号简明，下面省略一些上下标记号：



t RR



；





ttωω



；









ttff；









ttvv



；





ttpp



；





·将测量模型代入离散运动方程

 



   



   

    



 

 

   



Exp

ggd

aad

tt t tt

ttttt

tt t tt

ttt t ttt

tt t tt tt

ttt tt t t t

   

   

   

   

     



    





RRω

R ω b η

vva

vRfb η g

ppv a

pv Rfb η g



     



aad

ttt t tt t tt        pv g Rfb η



·上述公式中，注意到噪声项采用

和

（d 表示 discrete），它们与连续噪声项

和

是不同的，离散噪声和连续噪声的协方差有如下关系：



Cov Cov

gd g

ad a









ηη

预积分 paper 中给出该关系的参考文献为《Sigme-point Kalman filtering for integration GPS

and inertial navigation》。

·进一步假设

t

恒定（即采样频率不变），每个离散时刻由

0,1,2,...k



表示，前述三个离

散运动方程可进一步简化（符号简化）为：







Exp

ggd

kk kkk

aad

kkkkkk

aad

kkk kkkk

tt t



  

 

   

RR ω b η

vvRfbη g

ppv g Rfbη



三、基于前述性质、假设以及模型的 IMU 预积分（IMU Preintegration）

·根据欧拉积分，可以利用

ki

时刻到

1kj





时刻的所有 IMU 测量，来由

ki

时刻的

、

和

直接更新得到

kj

时刻的

、

和

：

B坐标系下的加速度

剩余24页未读，继续阅读

一抹烟霞

粉丝: 1172
资源: 27

会员权益专享

预积分总结与公式推导20180827.pdf

评论0

会员权益专享

最新资源

预积分总结与公式推导20180827.pdf

评论0

高等数学积分表147个不定积分公式证明推导.pdf

拓展卡尔曼滤波算法EKF公式推导.pdf

三角函数全公式及图解简明解推导.pdf

灰色预测gm(1、1)的模型python代码_python 实现 灰色预测 GM(1,1)模型 灰色系统 预测 灰色预测公式推导...

低秩稀疏求解推导过程.pdf

PX4 的 ECL EKF 公式推导及代码解析.pdf

轮速计与imu预积分公式推导

电磁场与电磁波公式总结.pdf

LLC串联谐振电路设计要点及公式推导.docx

BUCK电路的重要分析及公式推导.doc

高等数学常用公式总结.pdf

不定积分公式大全-含求积分通用方法及例题.pdf

Kalman滤波算法公式详细推导和Matlab代码实现.pdf

渗流力学机理数学模型及有限元推导知识梳理.pdf

高频电子线路公式大全.pdf

bsim430 manual .pdf

有关IP3的两个公式推导

高数王莉笔记.pdf

坐标系转换公式大全.pdf

会员权益专享

最新资源

灰色预测gm(1、1)的模型python代码_python 实现灰色预测 GM(1,1)模型灰色系统预测灰色预测公式推导...