為什么工程實踐中我們使用視覺與IMU融合的解決方案即視覺慣性里程計（VIO）來估計運動而不是簡單地使用視覺里程計（VO）。視覺慣性里程計的傳感器主要包括相機和慣性測量單元（IMU）兩種傳感器各有優(yōu)缺點，VIO的優(yōu)勢就在于IMU與相機的互補性。

視覺傳感器在大多數紋理豐富的場景中效果很好，但是遇到玻璃或白墻這樣特征少的場景就很難有效工作了。盡管如此相機數據的優(yōu)點在于數據基本不會有漂移。如果將相機放在原地固定，那么估計的位姿也是固定不動的。

IMU傳感器本身也是有自身缺點的，比如IMU長時間使用就會有非常大的累積誤差。但是在短時間內，其相對位移數據又有很高的精度，所以當視覺傳感器失效時，融合IMU數據，能夠提高定位的精度。IMU雖然可以測得角速度和加速度，但這些量都存在明顯的漂移，使得積分兩次得到的數據就會存在很大誤差。如果將IMU固定不動，其數據也會因為漂移的影響使得積分得到的位姿飄走。但是對于短時間內的快速運動，IMU能夠提供相對準確的估計，這又剛好彌補了相機的缺點。相機運動過快時會出現模糊或者兩幀之間重疊區(qū)域太少無法匹配。

IMU和相機就這樣相輔相成，因此現在很多SLAM算法的研究都是融合相機和IMU數據進行位姿估計。

IMU誤差部分

一提到VIO，相信很多人都會立刻想到一個名詞——預積分。這也是希望接觸VIO的研究者們最長看到的詞匯。那么究竟預積分是怎么回事，為什么預積分在VIO中這么重要，就是接下來討論的重點了。

?

?

從上面的公式可以看出，我們讀的數據都不是客觀事實，是在客觀事實的基礎上疊加上傳感器的誤差，也就是偏置和噪聲。

a 和 g 分別表示 acc 和 gyro，w 表示在世界坐標系，b 表示IMU體坐標系。在后面的公式中會有很多下標，很容易混淆。

位移，速度和姿態(tài)（position, velocity, quaternion, i.e. PVQ）對時間的導數可以寫成

前兩個公式沒問題，就是三個運動量之間的積分微分。第三個關于四元數微分，在這里稍微推導一下，方便大家后續(xù)閱讀。

中間穿插了一段四元數推導，現在繼續(xù)。

從第 i 時刻的PVQ 對 IMU的測量值進行積分得到第 j 時刻的PVQ：

因為IMU的采樣頻率高，通常為100Hz - 1000Hz，數據量非常大，在做優(yōu)化的時候，不可能將如此多的數據都放到狀態(tài)變量中，因此通常的做法是每隔一段時間提取一個數據，比如每隔1秒提取一個。也就是上式中假如 i 是第一秒提取的IMU數據，j 是第二秒提取的IMU數據?；具^程就是：已知第 i 秒的PVQ；第 i 秒和第 j 秒中間所有數據（如100個）以及我們已知的運動學知識積分，從第 i 秒一點一點積分得到第 j 秒的PVQ。但是這樣在做后端優(yōu)化的過程中，當我們進行迭代求解計算來更新和調整PVQ的值時，一旦（比如第 1 秒）的PVQ進行了調整，每一個中間過程以及后面所有的軌跡都要重新再積分算一遍，如果是100Hz，兩秒之間有100個采集數據，就要計算100次積分。有沒有什么方法是可以不用如此牽一發(fā)而動全身，擺脫冗繁的積分過程？預積分的目的就是嘗試將這100次積分過程變成只有1次積分，或者說用1個值來代替100個值，通過預積分模型的應用可以大大節(jié)省了計算量。

一個簡單的公式就可以將積分模型轉為預積分模型。

?

?

?

改叫 IMU預積分。分別對應了位置，速度和姿態(tài)。

預積分量僅和IMU測量值有關，它將一段時間內的IMU數據直接積分起來就得到了預積分量。

下圖為IMU和視覺傳感器的模型，可以在一定程度上幫助理解。

IMU相機模型

預積分的離散形式

?

IMU的預積分誤差

一段時間內IMU構建的預積分量作為測量值，對兩時刻之間的狀態(tài)量進行約束。

誤差中的位移，速度和偏置都是直接相減得到的，也就是測量值減估計值。第二行是四元數的旋轉誤差。

通過將積分模型轉化為預積分模型有效地減小了計算量，但是我們同時丟失了一些東西。當我們用1個結果代替（如100個）數據點的時候，我們就不知道這1個結果的不確定度了。在轉化之前，這100個數據點每一個數據點的不確定度我們是知道的（因為IMU數據作為測量值的噪聲方差我們能夠標定），但是這100個數據積分形成的預積分量的方差是多少呢？這就需要我們在得到IMU預積分的結果之后，還要推導預積分量的協方差，需要知道IMU噪聲和預積分量之間的線性遞推關系。

假設已知了相鄰時刻誤差的線性傳遞方程：

即誤差的傳遞分為兩部分：當前時刻的誤差傳遞給下一時刻，當前時刻測量噪聲傳遞給下一時刻。

協方差矩陣可以通過遞推計算得到：

其中

?

以上結果的推導利用了李群李代數的知識，過程過于繁復，因此沒有在這里展開推導過程，以上結果與主流VIO開源代碼（VINS-MONO）中寫出的公式形式相同，讀者可以直接對應代碼中的部分來對照。如果有時間，我會再更新以上公式的推導過程。

以上就是預積分的簡要介紹，在VIO開源框架（如VINS-MONO）中，IMU預積分部分屬于前端部分，是在采集了IMU數據之后就立刻進行的運算。在完整的VIO系統(tǒng)中除前端外還有很多其他的部分，包括：初始化部分（即IMU數據與相機數據的對齊）以及最重要的后端優(yōu)化求解部分。而后端優(yōu)化求解還包括很多內容，比如整體的目標函數如何構建，求解優(yōu)化的過程中運用了滑動窗口的方法來保證狀態(tài)量的數量從而控制計算量等，隨著我個人學習進度的推進還會進行總結和整理。

審核編輯：黃飛

?