為什么工程實(shí)踐中我們使用視覺(jué)與IMU融合的解決方案即視覺(jué)慣性里程計(VIO)來(lái)估計運動(dòng)而不是簡(jiǎn)單地使用視覺(jué)里程計(VO)。視覺(jué)慣性里程計的傳感器主要包括相機和慣性測量單元(IMU)兩種傳感器各有優(yōu)缺點(diǎn),VIO的優(yōu)勢就在于IMU與相機的互補性。
視覺(jué)傳感器在大多數紋理豐富的場(chǎng)景中效果很好,但是遇到玻璃或白墻這樣特征少的場(chǎng)景就很難有效工作了。盡管如此相機數據的優(yōu)點(diǎn)在于數據基本不會(huì )有漂移。如果將相機放在原地固定,那么估計的位姿也是固定不動(dòng)的。
IMU傳感器本身也是有自身缺點(diǎn)的,比如IMU長(cháng)時(shí)間使用就會(huì )有非常大的累積誤差。但是在短時(shí)間內,其相對位移數據又有很高的精度,所以當視覺(jué)傳感器失效時(shí),融合IMU數據,能夠提高定位的精度。IMU雖然可以測得角速度和加速度,但這些量都存在明顯的漂移,使得積分兩次得到的數據就會(huì )存在很大誤差。如果將IMU固定不動(dòng),其數據也會(huì )因為漂移的影響使得積分得到的位姿飄走。但是對于短時(shí)間內的快速運動(dòng),IMU能夠提供相對準確的估計,這又剛好彌補了相機的缺點(diǎn)。相機運動(dòng)過(guò)快時(shí)會(huì )出現模糊或者兩幀之間重疊區域太少無(wú)法匹配。
IMU和相機就這樣相輔相成,因此現在很多SLAM算法的研究都是融合相機和IMU數據進(jìn)行位姿估計。
IMU誤差部分
一提到VIO,相信很多人都會(huì )立刻想到一個(gè)名詞——預積分。這也是希望接觸VIO的研究者們最長(cháng)看到的詞匯。那么究竟預積分是怎么回事,為什么預積分在VIO中這么重要,就是接下來(lái)討論的重點(diǎn)了。
?
?
從上面的公式可以看出,我們讀的數據都不是客觀(guān)事實(shí),是在客觀(guān)事實(shí)的基礎上疊加上傳感器的誤差,也就是偏置和噪聲。
a 和 g 分別表示 acc 和 gyro,w 表示在世界坐標系,b 表示IMU體坐標系。在后面的公式中會(huì )有很多下標,很容易混淆。
位移,速度和姿態(tài)(position, velocity, quaternion, i.e. PVQ)對時(shí)間的導數可以寫(xiě)成
前兩個(gè)公式?jīng)]問(wèn)題,就是三個(gè)運動(dòng)量之間的積分微分。第三個(gè)關(guān)于四元數微分,在這里稍微推導一下,方便大家后續閱讀。
中間穿插了一段四元數推導,現在繼續。
從第 i 時(shí)刻的PVQ 對 IMU的測量值進(jìn)行積分得到第 j 時(shí)刻的PVQ:
因為IMU的采樣頻率高,通常為100Hz - 1000Hz,數據量非常大,在做優(yōu)化的時(shí)候,不可能將如此多的數據都放到狀態(tài)變量中,因此通常的做法是每隔一段時(shí)間提取一個(gè)數據,比如每隔1秒提取一個(gè)。也就是上式中假如 i 是第一秒提取的IMU數據,j 是第二秒提取的IMU數據?;具^(guò)程就是:已知第 i 秒的PVQ;第 i 秒和第 j 秒中間所有數據(如100個(gè))以及我們已知的運動(dòng)學(xué)知識積分,從第 i 秒一點(diǎn)一點(diǎn)積分得到第 j 秒的PVQ。但是這樣在做后端優(yōu)化的過(guò)程中,當我們進(jìn)行迭代求解計算來(lái)更新和調整PVQ的值時(shí),一旦(比如第 1 秒)的PVQ進(jìn)行了調整,每一個(gè)中間過(guò)程以及后面所有的軌跡都要重新再積分算一遍,如果是100Hz,兩秒之間有100個(gè)采集數據,就要計算100次積分。有沒(méi)有什么方法是可以不用如此牽一發(fā)而動(dòng)全身,擺脫冗繁的積分過(guò)程?預積分的目的就是嘗試將這100次積分過(guò)程變成只有1次積分,或者說(shuō)用1個(gè)值來(lái)代替100個(gè)值,通過(guò)預積分模型的應用可以大大節省了計算量。
一個(gè)簡(jiǎn)單的公式就可以將積分模型轉為預積分模型。
?
?
?
改叫 IMU預積分。分別對應了位置,速度和姿態(tài)。
預積分量?jì)H和IMU測量值有關(guān),它將一段時(shí)間內的IMU數據直接積分起來(lái)就得到了預積分量。
下圖為IMU和視覺(jué)傳感器的模型,可以在一定程度上幫助理解。
IMU相機模型
預積分的離散形式
?
IMU的預積分誤差
一段時(shí)間內IMU構建的預積分量作為測量值,對兩時(shí)刻之間的狀態(tài)量進(jìn)行約束。
誤差中的位移,速度和偏置都是直接相減得到的,也就是測量值減估計值。第二行是四元數的旋轉誤差。
通過(guò)將積分模型轉化為預積分模型有效地減小了計算量,但是我們同時(shí)丟失了一些東西。當我們用1個(gè)結果代替(如100個(gè))數據點(diǎn)的時(shí)候,我們就不知道這1個(gè)結果的不確定度了。在轉化之前,這100個(gè)數據點(diǎn)每一個(gè)數據點(diǎn)的不確定度我們是知道的(因為IMU數據作為測量值的噪聲方差我們能夠標定),但是這100個(gè)數據積分形成的預積分量的方差是多少呢?這就需要我們在得到IMU預積分的結果之后,還要推導預積分量的協(xié)方差,需要知道IMU噪聲和預積分量之間的線(xiàn)性遞推關(guān)系。
假設已知了相鄰時(shí)刻誤差的線(xiàn)性傳遞方程:
即誤差的傳遞分為兩部分:當前時(shí)刻的誤差傳遞給下一時(shí)刻,當前時(shí)刻測量噪聲傳遞給下一時(shí)刻。
協(xié)方差矩陣可以通過(guò)遞推計算得到:
其中
?
以上結果的推導利用了李群李代數的知識,過(guò)程過(guò)于繁復,因此沒(méi)有在這里展開(kāi)推導過(guò)程,以上結果與主流VIO開(kāi)源代碼(VINS-MONO)中寫(xiě)出的公式形式相同,讀者可以直接對應代碼中的部分來(lái)對照。如果有時(shí)間,我會(huì )再更新以上公式的推導過(guò)程。
以上就是預積分的簡(jiǎn)要介紹,在VIO開(kāi)源框架(如VINS-MONO)中,IMU預積分部分屬于前端部分,是在采集了IMU數據之后就立刻進(jìn)行的運算。在完整的VIO系統中除前端外還有很多其他的部分,包括:初始化部分(即IMU數據與相機數據的對齊)以及最重要的后端優(yōu)化求解部分。而后端優(yōu)化求解還包括很多內容,比如整體的目標函數如何構建,求解優(yōu)化的過(guò)程中運用了滑動(dòng)窗口的方法來(lái)保證狀態(tài)量的數量從而控制計算量等,隨著(zhù)我個(gè)人學(xué)習進(jìn)度的推進(jìn)還會(huì )進(jìn)行總結和整理。
審核編輯:黃飛
?
評論