logo资料库

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

发布时间：2022-06-08 发布人：admin 分类：说明书 资料大小：0.72M 资料格式：pdf 举报 版权申诉
第1页 / 共25页
第2页 / 共25页
第3页 / 共25页
第4页 / 共25页
第5页 / 共25页
第6页 / 共25页
第7页 / 共25页
第8页 / 共25页
资料共25页，剩余部分请下载后查看
    邱笑晨  ares43490@126.com  本文作者为北航自动化学院在读博士邱笑晨，如有疑问和建议请发邮件至：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 是完全无法得到绝对姿态的。   1 
    邱笑晨  ares43490@126.com  此外，由于传感器测量误差的存在，无论是纯惯导还是纯 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 维实向量间的双射，如下式：  ω   W   3      1        2        3 0     2  0     3 1 0     1 0    2 0    3 1 0    1              1      2      3   2 3 2  W  ω ·关于 hat 运算符的一个性质（ 3R 为所有 3 维实向量的集合）： ·指数映射    , a b  b a      a b , exp  将 so(3)中的元素映射到 SO(3)上： 1 cos                  exp sin     I R 3    2     2  当 是小量时，有一阶近似如下： ·对数映射     I   exp      log  将 SO(3)中的元素映射到 so(3)上：  R R      2sin  log R    T   1   tr R 2    。有  log R     a ，为旋转角， a 为旋转轴单位矢量，有        其中   1  cos  T a      R R  。   2sin    （或  ·当   被限制在其它任意一个连续的 2范围内）时，指数映射和对数映射   2 
    邱笑晨  ares43490@126.com  exp  和   构成 SO(3)和 so(3)间的双射。 ·   log  是 SO(3)（3×3 正交阵）与 so(3)（3×3 反对称阵）之间的映射，为了后 续行文的符号简明，下面用 3 维实向量来指代 so(3)的元素（依据 3 维实向量和 3×3 反对称 一一对应），定义新的指数映射和对数映射：     Exp : exp SO (3) R 3      R log    R      ，首字母大写表示 SO(3)和 3R 之间的映射。  R 3 exp   注意   Exp  ·对于 3 维实向量 Log :SO(3)    ，   Log log    和一个小量 ，有下述近似性质：         J Exp Exp                      式 1 可以这样理解： 3R 中的向量 加上一个小量 Log Exp Exp Exp  J  r               1  r  ，对应到 SO(3)中则是    Exp  右乘一 个 Exp  J     r    ；    Exp    Exp    ，对应到 3R 中则是 加上一项 式 2 则可这样理解：SO(3)中 右乘一个  。      J 1  r      r J 是 SO(3)的右 Jacobian，将切空间的“加性项”和 SO(3)中的右“乘性项”联系在一   起。  r J 1   及其逆  r J     J r 的表达式如下：     1 cos    2   I         J 1  r   I    1 2       1    2 2    sin  3   1 cos   sin           2            2        ·指数映射的 Adjoint 性质： Exp R          Exp   T R  exp  R R    Exp  R R     T   T  R  Exp   R    下面证明第一行等式。首先，对于任意旋转矩阵 R 和三维矢量 ，有如下等式成立：   R      R R   T    该等式在《State Esitimation for Robotics》第 P227 页被引用。它的一个简单证明可参照郑帆 大佬的博客（https://fzheng.me/2017/12/10/Rvhat/）。 令  a 有（即罗德里格斯公式） ，其中为模值，a 为单位矢量。参照《视觉 SLAM 十四讲》第 P70 页式(4.22)       exp Exp exp        aa I cos 1 cos T      则 Adjoint 性质的左边可以开始变形：     a  sin       a   3 
    R    T T   T R I    T    Exp     R aa cos 1 cos sin        RR Raa R cos 1 cos T             Ra Ra I 1 cos cos                Ra R exp    R R   ， 则 有    T  T     SO (3) ，则有 exp    此 时 ， 考 虑  exp Exp :     R 3 exp   R     邱笑晨  ares43490@126.com   a   sin sin   T  R   Ra R      Ra    T    R R   T   ； 考 虑 exp   R      R  Exp     exp   R     。  证毕。 二、IMU 器件测量模型（Sensor Model）和运动学模型（Kinetic Model） ·陀螺测量模型：  ω b wb   t  ω b wb   t  b g   t  η g   t a a b w w f  g a    b η R wT b   t   t   t 其中 gb 是随时间缓慢变化的 bias， gη 是白噪声。该模型利用了 Static World Assumption，后 文会介绍。 ·加计测量模型：   其中 ab 是随时间缓慢变化的 bias， aη 是白噪声。 ·可以看到，这里对加计和陀螺的测量模型建模都比较简单，没有过细的考虑马尔科夫过程 等其他误差项的建模，这与 SLAM 中一般使用精度较低的 mems 器件有关。 · Static World Assumption：由于 SLAM 的运行场景一般比较小（重力变化不大），运行时 间也不会太长，且使用的 IMU 器件一般为 mems 器件（精度低，陀螺仪静置时无法敏感地 球自转）。因此不会像传统的捷联 INS 解算中，考虑地球自转、根据位置更新重力矢量等， 常常忽略地球自转（认为地球是个 static world），并假设运行区域水平面是个平面，重力矢 量 wg 的指向固定且模值恒定。在此假设下的 VISLAM 或 VIO 中，世界坐标系 w 被认为是 一个惯性系，且一般会选择初始时刻水平地理系，前面的陀螺仪测量模型中即利用了 w 系 是惯性系的假设。下面的运动模型，都是基于这个 assumption 来进行推导的。 ·运动模型的微分方程形式如下： b wb   （来源可参考秦永元《惯性导航》第一版P238）  R w b  v w  p w    w b  R ω a w v w 使用欧拉积分（Euler Integration，三角形积分）可得到运动方程的离散形式如下： R w  b t  v w  p w  R Exp w      b t t    v t t t w       t    p t t w ω b wb a  w  w v   t   t   t  t   t   w a   t 2 t      t 1 2 ★【其中，对 Rotation 更新方程做详细解释如下（参考秦永元《惯性导航》第 1 版 P292。 9.2.2 节）：   tω 表示 t 时刻“角速度矢量”在 b 系下的坐标 b wb   4 
    邱笑晨  ares43490@126.com    b wb   t  f b v p w  w ；   b wb   t   b wb  t  f R w  b t w   b t   t p    t v    t R ω 】★   b t  t b t    t  ；   t ；   t   t  ω ；   ω t  ω b wb Exp  t     t Exp  相当于罗德里格斯公式   b t R    t b t   R R t w      b t 表示“旋转矢量”在 b 系下的坐标，又有 ω （Rodrigues’ Rotation Formula） Exp R w   b t 注意到，IMU 预积分的理论是建立在欧拉积分的基础上的，并不是捷联惯导中传统的 LK4 等积分方式。 ·为了符号简明，下面省略一些上下标记号：     t t R ·将测量模型代入离散运动方程     ω t t Exp        b ω t Exp   a t t w        f R t t   1    2 1 2 1 2  R t      v t      R t   R t   v t    v t   t 1 2    t t      t        ；   t wg g    ·上述公式中，注意到噪声项采用 gdη 和 adη （d 表示 discrete），它们与连续噪声项 gη 和 aη 是不同的，离散噪声和连续噪声的协方差有如下关系：  η  Cov Cov Cov Cov     t   t   t   t     η a         t   t   t   t   t   t   t   t   t   t   t   t   t   t   t   t   t t   t   t    b  b  f   g  η  η  η  η  p  p p  t  v  v  v b a w a R   t t η ad 2  η gd R  f      a  t g   ad  g g 2 t ad 2 t p ad a   g gd 2 t t 1 t  1 t  预积分 paper 中给出该关系的参考文献为《Sigme-point Kalman filtering for integration GPS and inertial navigation》。 ·进一步假设 t 恒定（即采样频率不变），每个离散时刻由 0,1,2,... 散运动方程可进一步简化（符号简化）为： g  k 表示，前述三个离  ω b k  R   1   k k k R Exp     v R f k k      v t  p k k k  η gd k     η ad k t 2 t t    g   R f k  t      1 2  k  a k g b  1 2 v p k 1  k 1    b a k  η ad k  2 t   三、基于前述性质、假设以及模型的 IMU 预积分（IMU Preintegration） ·根据欧拉积分，可以利用 k iv 和 ip 直接更新得到 k j  时刻的所有 IMU 测量，来由 k j 时刻的 jR 、 jv 和 jp ： i 时刻到 1 k i 时刻的 iR 、   5 
    邱笑晨  ares43490@126.com  g k  η gd k  t   Exp    ω b  k R j  R i  j 1   k i   v i     g t ij j 1   k i   b a k  η ad k  t   p i     t k g 2   t v p j j k    R f k i j  2 1 2 g 2    t 1 2  p i  v    t k 其中 t   ij t    j  i t j 1  v  k i  j 1      1   k i  j k i  j 1  1 2 k i     R f k  k   R f k  k  b a k  η ad k  b a k  η ad k  2 t   2 t       ·于是，为了避免每次更新初始的 iR 、 iv 和 ip 都需要重头积分求解 jR 、 jv 和 jp ，引出 预积分项（理想值）如下：  R  ij   v ij    p ij   j R R T i j 1   Exp k i   R v T i j 1  j    R ik   k i   v  i  f k T i  R p        k i  1  j v  ω b  k g k  gd k t    η   η ad k t    ij g  b a k  t    1 2   f k  p i  v i j    t ij g t   2 ij        t R 1 2  ik ik  b a k  η ad k  2 t      ·上述三个预积分公式中，前两个式子的推导是显而易见的，现对 ijp 的公式进行证明， 且为节省篇幅，令 ： ξ k  f k   b a k  p j  p i  v i   t ij t 2   ij ad k g η 1 2 j 1   k i  v k    t    1 2 g 2    t i  2  v k  v i  t    v k  v i  t   j     j  1  k i  1 2  R ξ k k 2 t       j 1   k i  v i   t  j 2 i   2 g t   2  j 2 i   2 g     2    t 1 2 j 1   k i  R ξ k k 2 t    k  i  g 2   t 1 2 j 1   k i  R ξ k k 2 t           j 1   k i  j 1   k i  j 1   k i  j 1   k i  j 1   k i            v k  v i   k  i  g     t t   1 2 R ξ k k 2 t        v k  v i t    ik g  t   1 2 R ξ k k 2 t      R v      t ik i 1 2 R ξ k k 2 t      6 
    其中利用了 i j  2  j  2 i   2  j  i       1   i  j   2   j 1   k  k i  邱笑晨  ares43490@126.com  i  （等差数列求和）。 前面等式左右两边同时左乘 T  v i    t ij 1 2 g t   2 ij    j i j 1  T i T i   iR ，有  p R p              k i  j 1  k i  j 1  k i  ik ik i    1 2 1 2 R R v      t ik 1 2 R R ξ k T i k 2 t     v      t R ξ ik 2 t   k    v      t R   f k  ik  b a k  η ad k  2 t      证毕。 四、预积分测量值及测量噪声 kη ）从预积分理想值中分离出来，使得预积分测量值（由 IMU ·下面尝试将噪声项（ gd 测量数据计算得到）具有理想值“加”（之所以打引号是因为旋转量 ijR 不具有加性形式， 它是乘性的）白噪声的形式。 这里做一个假设，认为预积分计算区间内（和视觉融合时，通常是两帧间）的 bias 相等， 即 （1） ijR 项： b 。 kη 和 ad b b g  i 1 b a  i 1 以及 b b     g i a i a j g j R  ij  ①  ② =    j 1   k i  j 1  Exp  k i   R    j ij Exp 1   k i   ω b  k  g i t   η gd k  t     ω b  k   t   g i Exp  Exp   R  T k  J η k r  gd k j 1    t   g i η gd k t    k     J ω b r  t  推导中的一些细节： ①处：  利用性质“当 ②处： 利用 Adjoint 性质“ 令 J    J ω b k  r r 再令   k 是小量时有 Exp         Exp      Exp  J r         ”。   T   R Exp ”；  R R         。  Exp  t  g i  R  ij j 1    k i     ij  Exp  。则可得：  Exp    ω b  k  g i t    j 1   k i  Exp   R  T k  J η k r  gd k t  1  j    7   R 注意到其中有 jj I  
    R  ij  R  ij  Exp  ij    邱笑晨  ares43490@126.com    （或   ij ijR 即旋转量预积分测量值，它由陀螺仪测量值和对陀螺 bias 的估计或猜测计算得到。认  Exp 为 ij （2） ijv 项： 将上式，即 ）即其测量噪声。  代入 ijv 的公式中，有： Exp R    ij ij  R    f k   v   ij R  ik b a i   R  ik  Exp    ik   η ad k  t    ij       f k  f k b   a i b a i  η ad k  t    η ad k  t    R  ik    I  R  ik  R  ik  R  ik        I       f k  f k     b a i b a i    ik    ik           f k  b a i      t  R η ik ad k t    t +    R    t     f k  ik  b a i    ik       t  R η ik ad k t      R  ik   f k   b a i       ik       t  R η ik j 1   k i  ad k t      是小量时，有一阶近似如下：  exp    ik kη ad 项）；     I     Exp     I ”; ，或 j 1   k i  j 1   k i  j 1   k i  j 1   k i  j 1   k i  j 1   k i   ①  ②  ③  = 推导中的一些细节： ①处：  利用性质“当 ②处： 忽略高阶小项（ ③处： 利用性质“     a b 再令 可得： b a ”。     v  ij v  ij  j 1   k i  j 1   k i        R  ik   f k   b a i  t    R η  ik ad k t     R ik      f k  b a i      ik t       v ij  v   ij v ij ijv 即速度增量预积分测量值，它由 IMU 测量值和对 bias 的估计或猜测计算得到。 ijv 即 其测量噪声。 （3） ijp 项 将 R  ij  R  ij  Exp  ij    v 以及 ij   v   ij v ij 代入 ijp 的公式中，有：   8 
    分享到：
    赞 收藏

    相关推荐

    资料库

    课程资源

    共收录17145份资料，累计13个分类，关注成员有19位，主要包括：PHP,网络管理,网页制作,Java,.Net,数据库,3G/移动开发,C/C++,游戏开发,嵌入式,讲义,软件测试,专业指导

    热门标签

    PHP 网络管理 网页制作 Java .Net 数据库 3G/移动开发 C/C++ 游戏开发 嵌入式 讲义 软件测试 专业指导

    最新资料