如何理解IMU以及其預積分

來自專欄 SLAM相關所學所想29 人贊了文章

從接觸IMU時，離不開IMU的預積分，那麼IMU的預積分是什麼呢，下面一步一步來介紹：

首先我們要知道預積分的作者是Christian Forster這位大神，感興趣的同學可以收集他的論文來看。

1.IMU是什麼：

IMU中文名叫慣性測量單元，英文名：Inertial measurement unit，簡稱 IMU。簡單理解就是這個東東可以測量兩個東西，加速度是沿三個軸 $a_{x},a_{y},a_{z},$ 方向的線加速度，而角速度就是這三個方向的角速度 $w_{x},w_{y},w_{z},$ 還有就是IMU的頻率比較高一般都在100HZ以上。在IMU內部，除了通常的白雜訊，還有個特別的量零偏bias，在這是感測器內部機械、溫度等各種物理因素產生的感測器內部誤差的綜合參數。IMU的加速度計和陀螺儀的每個軸都用彼此相互獨立的參數建模，一個角速度測量值和真值之間的連續域上的關係可以寫作：

$ilde{w}=w + b_{w} + n_{w}$

2.IMU能幹什麼：

在純視覺的slam或者vo中，由於圖像的運動模糊、遮擋、快速運動、純旋轉、尺度不確定性的一系列問題，導致僅靠一個攝像頭很難完成我們實際場景的應用需求，而IMU直接可以得到運動主體自身的角速度、加速度的測量數據，從而對運動有一個約束，或者說與視覺形成互補，可實現快速運動的定位和主體純旋轉的處理，從而進一步提高slam/vio的可靠性。

3.IMU怎麼用：

IMU每個時刻可實現自身主體加速度和角速度的測量，那麼有測量模型：

式1

又有運動學方程：

式2

，在區間裏有：

式3

將式1、2帶入式3可得：

式4

這裡面有幾個關於SO3上旋轉角度變化量的計算，這部分是有關李羣、李代數的內容，這裡先不講。從式4可以看到，IMU可以乾的事情就是由上個時刻狀態可以推算出下個時刻的狀態，這裡的狀態就是旋轉角度，速度和位置。知道了這個以後，就有同學可能就要問了，預積分到底是啥，怎麼用的？不要著急，接著往下看：

4.IMU 預積分的作用是什麼？

為了有個清晰的認識，首先說下預計分的作用。大家知道，用IMU的slam、vio演算法有很多，有濾波器的比如MSCKF，有基於圖優化的比如VINS，OKVIS，ORB-SLAM等。就拿ORB-SLAM來說吧，在bundle adjustment裏，參與對象是keyframe，比如有2個keyframe： $KF_{1}, KF_{2}$ , 他們的位姿分別為： $P_{1w},P_{2w}$ ,那麼他們的相對位姿 :

$P_{21} = P_{1w} *P_{2w}^{-1}$

我們可以認為 $P_{21}$ 為估計項，是由SLAM的位姿直接算出來的。如果要構成一個優化問題，我們還需要知道誤差項和測量項。沒錯，是IMU可以計算在這兩個KF間的測量項 $P_{imu}$ ，預計分乾的事情就是計算這個測量項。

$r = P_{21}*P_{imu}^{-1}$

在緊耦合的優化slam中，IMU就是提供了兩個關鍵幀的相對測量，從而構建誤差函數對關鍵幀姿態的迭代優化。當然實際應用中不會是這麼簡單的形式，這裡面要對各個變數分別求取誤差，然後求雅克比矩陣。

5.IMU 預積分怎麼計算？

這裡主要貼一些關鍵公式，詳細推倒有不懂得地方可以去閱讀作者的補充材料，這裡面可能需要補充些李羣，李代數的知識，後續我也會把相關常用的一些數學公式結論整理出來。

假設有兩個關鍵幀，分別是時刻，IMU的每個時刻間的間隔是，於是由離散的積分公式可以得到：

式 5

上式中 $Delta t_{ij} = Sigma_{k=i}^{j}Delta t$ ，可以認為 $R_{i} = R_{Wi}$ , 標識從幀到世界坐標系的旋轉。所以，很容易的由上式5 可以得到從時刻的相對變化:

式 6

上式中： $Delta R_{ik} = R_{i}^{T} R_{k}$ ， $Delta V_{ik} = V_{k} - V_{i}$ , 這裡有一個前提假設，就是我們認為零偏 $b_{i}$ 是已知的，即：

我們都知道，在g2o優化中，一般我們的KF的狀態量是由5量構成，其中是： $R,V,P,b_{a},b_{g}$ , 所以加速度和角速度的零偏也是需要估計的，也就是說我們每優化一次Bias就要變化一次。那麼問題來了，難道我們沒做一次local bundle adjustment 就需要重新再計算一遍式6 嗎？當然不需要，有方法來避免重複計算，我們只需要用新的Bias來更新一下預積分的值就行了。

對旋轉增量 $Delta R_{ij} ，Delta V_{ij}, Delta P_{ij}$ 可進一步近似，得到以下公式：

式7

上訴公式有不明白的地方可以返回原文找到對應公式即可容易推導得出，

結合式6，得到IMU預積分的誤差模型：

式8

接下來，介紹如何應用更新後的Bias來更新IMU的預積分誤差模型，如果bias的更新： , 上式8中的增量對bias進行一階展開得到：

式9

那麼，這個式9就最終解答了我們的疑惑，告訴了我們在BA優化得到新的bias之後怎麼去計更新預積分的狀態增量。至於其中如何去求雅克比，這裡原作者有個補充材料講的很清楚，這裡就不再進行介紹了。

結尾：由於本人水平有限，某些觀點可能不嚴謹，或者有表述錯誤的地方請大家見諒。

參考文獻：

Forster, Christian, et al. "IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation." Georgia Institute of Technology, 2015.

Forster, Christian, et al.Supplementary material to: IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation. Georgia Institute of Technology, 2015.