基于旋轉(zhuǎn)平移解耦框架的視覺慣性初始化方法

來源：3D視覺工坊

1. 導(dǎo)讀

精確和魯棒的初始化對于視覺慣性里程計(jì)(VIO)至關(guān)重要，因?yàn)椴涣嫉某跏蓟瘯?huì)嚴(yán)重降低姿態(tài)精度。在初始化期間，估計(jì)諸如加速度計(jì)偏差、陀螺儀偏差、初始速度和重力等參數(shù)是至關(guān)重要的。IMU傳感器需要精確估計(jì)陀螺儀偏差，因?yàn)橥勇輧x偏差會(huì)影響旋轉(zhuǎn)、速度和位置。現(xiàn)有的VIO初始化方法大多采用運(yùn)動(dòng)結(jié)構(gòu)法來解決陀螺漂移問題。然而，SfM在快速運(yùn)動(dòng)或退化場景中不夠穩(wěn)定和有效。為了克服這些限制，我們通過添加新的不確定性參數(shù)和優(yōu)化模塊來擴(kuò)展旋轉(zhuǎn)-平移-解耦框架。首先，我們采用一種陀螺儀偏置優(yōu)化器，它結(jié)合了概率法向極線約束。其次，我們?nèi)诤螴MU和視覺測量來有效地求解速度、重力和比例。最后，我們設(shè)計(jì)了一個(gè)額外的細(xì)化模塊，有效地減少重力和比例誤差。在EuRoC數(shù)據(jù)集上的大量初始化測試表明，我們的方法將陀螺儀偏差和旋轉(zhuǎn)估計(jì)誤差分別平均降低了16%和4%。它還顯著降低了重力誤差，平均降低了29%。

2. 引言

視覺慣性里程計(jì)（VIO）旨在在各種未知環(huán)境中估算相機(jī)的位置。它通過融合來自相機(jī)的圖像信息和來自慣性測量單元（IMU）傳感器的IMU測量數(shù)據(jù)來實(shí)現(xiàn)這一目標(biāo)。相機(jī)可以估計(jì)視覺地圖并減少姿態(tài)漂移。IMU傳感器為相機(jī)運(yùn)動(dòng)提供度量尺度，并具備短期魯棒性。VIO具有諸多優(yōu)勢，如體積小、成本低、功耗低。這些特性使得VIO在虛擬現(xiàn)實(shí)、增強(qiáng)現(xiàn)實(shí)和自動(dòng)化機(jī)器人等領(lǐng)域的應(yīng)用日益廣泛。

為了有效運(yùn)行VIO系統(tǒng)，必須在初始化階段準(zhǔn)確估計(jì)一組參數(shù)。這些參數(shù)包括尺度、重力方向、初始速度和加速度計(jì)及陀螺儀的偏差。初始化不正確將導(dǎo)致收斂性差和系統(tǒng)其他參數(shù)估計(jì)不準(zhǔn)確。另一方面，快速初始化也很重要，因?yàn)閂IO系統(tǒng)必須在IMU正確初始化后才能工作。

基本上，以往的VIO初始化工作可分為緊耦合和松耦合方法。緊耦合方法假設(shè)相機(jī)的姿態(tài)可以通過IMU測量進(jìn)行近似。它們將視覺觀測與IMU積分相融合，并通過封閉形式的解來估計(jì)初始化參數(shù)，這可能會(huì)增加計(jì)算成本。此外，這些方法通常忽略陀螺儀偏差，這可能會(huì)影響準(zhǔn)確性。松耦合方法假設(shè)通過視覺結(jié)構(gòu)從運(yùn)動(dòng)恢復(fù)（SfM）獲得的相機(jī)軌跡非常準(zhǔn)確。首先，它們解決視覺SfM問題，并根據(jù)得出的相機(jī)姿態(tài)初始化慣性參數(shù)。因此，這些方法的準(zhǔn)確性在很大程度上依賴于視覺SfM的性能。然而，在相機(jī)快速運(yùn)動(dòng)或共同特征點(diǎn)太少的情況下，視覺SfM可能會(huì)變得不穩(wěn)定。

總體而言，緊耦合和松耦合方法都未能充分利用相機(jī)和IMU之間的互補(bǔ)信息。具體而言，緊耦合方法未利用視覺觀測來估計(jì)陀螺儀偏差，這可能會(huì)導(dǎo)致數(shù)值穩(wěn)定性問題和準(zhǔn)確性降低。松耦合方法未使用IMU測量來增強(qiáng)視覺SfM的穩(wěn)定性，從而在具有挑戰(zhàn)性的運(yùn)動(dòng)場景中導(dǎo)致準(zhǔn)確性低或初始化失敗。受圖像觀測可直接用于優(yōu)化圖像幀之間旋轉(zhuǎn)的啟發(fā)[12]，提出了一種旋轉(zhuǎn)-平移解耦的VIO初始化方法。該方法增強(qiáng)了視覺觀測與IMU測量之間的聯(lián)系。然而，這種方法忽略了圖像特征匹配的質(zhì)量。在最終結(jié)果中，每個(gè)匹配都被賦予相同的權(quán)重。盡管已從特征匹配中去除異常值，但2D特征對應(yīng)點(diǎn)的誤差分布隨圖像內(nèi)容和具體匹配技術(shù)而變化。因此，考慮2D特征匹配的不確定性至關(guān)重要。

3. 效果展示

MH02數(shù)據(jù)集和MH03數(shù)據(jù)集的角速度和標(biāo)度誤差可視化。第一行圖像是MH02，第二行圖像是MH03。列(a)顯示了基于角速度著色的相應(yīng)數(shù)據(jù)集的軌跡。列(b)和(c)分別顯示了我們的方法和DRT-t方法基于相應(yīng)數(shù)據(jù)集上著色的尺度誤差的軌跡。刻度誤差在0和1之間。顏色越淺，誤差越小。

4. 主要貢獻(xiàn)

為了克服SfM的局限性并提高初始化準(zhǔn)確性和魯棒性，我們在考慮概率法正態(tài)極線約束（PNEC）的情況下估計(jì)陀螺儀偏差。該方法通過考慮特征點(diǎn)的異性和非均勻不確定性來提高陀螺儀偏差估計(jì)的準(zhǔn)確性?；谛D(zhuǎn)-平移解耦框架，我們首先解決旋轉(zhuǎn)問題。然后，我們使用線性全局平移約束（LiGT）解決平移問題。最后，我們通過求解最小二乘問題來估計(jì)速度、尺度和重力。為了進(jìn)一步提高尺度和重力的準(zhǔn)確性，我們還引入了一個(gè)改進(jìn)的尺度和重力細(xì)化模塊。推薦課程：徹底剖析激光-視覺-IMU-GPS融合SLAM算法：理論推導(dǎo)、代碼講解和實(shí)戰(zhàn)。

5. 方法

陀螺儀偏差的準(zhǔn)確估計(jì)在提高VIO系統(tǒng)軌跡準(zhǔn)確性方面起著核心作用。偏差會(huì)影響旋轉(zhuǎn)，進(jìn)而影響平移和速度的積分。在本節(jié)中，我們提出了一種能夠準(zhǔn)確求解初始化參數(shù)的方法，這些參數(shù)包括陀螺儀偏差、速度、重力和尺度。初始化過程分為以下四個(gè)步驟：（1）陀螺儀偏差估計(jì)，（2）旋轉(zhuǎn)和平移估計(jì)，（3）尺度、速度和重力估計(jì)，以及（4）尺度和重力細(xì)化。

正態(tài)極線約束（NEC）的幾何特性以及陀螺儀偏差與NEC之間的關(guān)系。法向量n1和n2垂直于包含f1i（f2i）和f1j（f2j）（紅色和綠色）的極平面，且所有法向量位于同一平面（黃色）內(nèi)，形成一個(gè)可用于求解旋轉(zhuǎn)Rcicj（橙色）的約束。通過將外部參數(shù)Rbc（藍(lán)色）引入，求解Rcicj的問題轉(zhuǎn)化為求解陀螺儀偏差（粉色）的問題。

6. 實(shí)驗(yàn)結(jié)果

7. 總結(jié) & 未來工作

我們提出了一種基于旋轉(zhuǎn)-平移解耦框架的魯棒且精確的視覺-慣性初始化方法。

通過建立概率正態(tài)極線約束，我們推導(dǎo)出了一個(gè)新的陀螺儀偏置估計(jì)公式，該公式直接利用視覺觀測結(jié)果并計(jì)算特征點(diǎn)位置的不確定性。然后，基于慣性測量單元（IMU）和視覺測量值建立最小二乘問題，以估計(jì)速度、尺度和重力。最后，我們引入了一個(gè)改進(jìn)的尺度-重力優(yōu)化模塊。

大量的初始化實(shí)驗(yàn)表明，我們的方法在保持高計(jì)算效率的同時(shí)，提高了準(zhǔn)確性和魯棒性。該方法的一個(gè)局限性在于沒有考慮加速度計(jì)偏置對速度和尺度的影響。未來，我們將研究一個(gè)涵蓋加速度計(jì)偏置的尺度-重力優(yōu)化模塊。