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

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

IMU傳感器本身也是有自身缺點(diǎn)的，比如IMU長(zhǎng)時(shí)間使用就會(huì)有非常大的累積誤差。但是在短時(shí)間內(nèi)，其相對(duì)位移數(shù)據(jù)又有很高的精度，所以當(dāng)視覺傳感器失效時(shí)，融合IMU數(shù)據(jù)，能夠提高定位的精度。IMU雖然可以測(cè)得角速度和加速度，但這些量都存在明顯的漂移，使得積分兩次得到的數(shù)據(jù)就會(huì)存在很大誤差。如果將IMU固定不動(dòng)，其數(shù)據(jù)也會(huì)因?yàn)槠频挠绊懯沟梅e分得到的位姿飄走。但是對(duì)于短時(shí)間內(nèi)的快速運(yùn)動(dòng)，IMU能夠提供相對(duì)準(zhǔn)確的估計(jì)，這又剛好彌補(bǔ)了相機(jī)的缺點(diǎn)。相機(jī)運(yùn)動(dòng)過快時(shí)會(huì)出現(xiàn)模糊或者兩幀之間重疊區(qū)域太少無法匹配。

IMU和相機(jī)就這樣相輔相成，因此現(xiàn)在很多SLAM算法的研究都是融合相機(jī)和IMU數(shù)據(jù)進(jìn)行位姿估計(jì)。

IMU誤差部分

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

?

?

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

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

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

前兩個(gè)公式?jīng)]問題，就是三個(gè)運(yùn)動(dòng)量之間的積分微分。第三個(gè)關(guān)于四元數(shù)微分，在這里稍微推導(dǎo)一下，方便大家后續(xù)閱讀。

中間穿插了一段四元數(shù)推導(dǎo)，現(xiàn)在繼續(xù)。

從第 i 時(shí)刻的PVQ 對(duì) IMU的測(cè)量值進(jìn)行積分得到第 j 時(shí)刻的PVQ：

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

一個(gè)簡(jiǎn)單的公式就可以將積分模型轉(zhuǎn)為預(yù)積分模型。

?

?

?

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

預(yù)積分量?jī)H和IMU測(cè)量值有關(guān)，它將一段時(shí)間內(nèi)的IMU數(shù)據(jù)直接積分起來就得到了預(yù)積分量。

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

IMU相機(jī)模型

預(yù)積分的離散形式

?

IMU的預(yù)積分誤差

一段時(shí)間內(nèi)IMU構(gòu)建的預(yù)積分量作為測(cè)量值，對(duì)兩時(shí)刻之間的狀態(tài)量進(jìn)行約束。

誤差中的位移，速度和偏置都是直接相減得到的，也就是測(cè)量值減估計(jì)值。第二行是四元數(shù)的旋轉(zhuǎn)誤差。

通過將積分模型轉(zhuǎn)化為預(yù)積分模型有效地減小了計(jì)算量，但是我們同時(shí)丟失了一些東西。當(dāng)我們用1個(gè)結(jié)果代替（如100個(gè)）數(shù)據(jù)點(diǎn)的時(shí)候，我們就不知道這1個(gè)結(jié)果的不確定度了。在轉(zhuǎn)化之前，這100個(gè)數(shù)據(jù)點(diǎn)每一個(gè)數(shù)據(jù)點(diǎn)的不確定度我們是知道的（因?yàn)镮MU數(shù)據(jù)作為測(cè)量值的噪聲方差我們能夠標(biāo)定），但是這100個(gè)數(shù)據(jù)積分形成的預(yù)積分量的方差是多少呢？這就需要我們?cè)诘玫絀MU預(yù)積分的結(jié)果之后，還要推導(dǎo)預(yù)積分量的協(xié)方差，需要知道IMU噪聲和預(yù)積分量之間的線性遞推關(guān)系。

假設(shè)已知了相鄰時(shí)刻誤差的線性傳遞方程：

即誤差的傳遞分為兩部分：當(dāng)前時(shí)刻的誤差傳遞給下一時(shí)刻，當(dāng)前時(shí)刻測(cè)量噪聲傳遞給下一時(shí)刻。

協(xié)方差矩陣可以通過遞推計(jì)算得到：

其中

?

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

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

審核編輯：黃飛

?