一種自適應(yīng)初始化的視覺慣性SLAM方法

付 鵬，李帥鑫，喻天佑，姚 雯，陳小前

一種自適應(yīng)初始化的視覺慣性SLAM方法

付 鵬，李帥鑫，喻天佑，姚 雯，陳小前

（軍事科學院國防科技創(chuàng)新研究院，北京 100071）

針對傳統(tǒng)單目視覺慣性系統(tǒng)初始化階段魯棒性差、適用條件有限、收斂速度慢等問題，提出了一種基于不確定運動狀態(tài)的單目視覺慣性同時定位與建圖（SLAM）初始化方法。該方法基于視覺慣性導航系統(tǒng)（VINS）非線性優(yōu)化理論，建立了包含全要素初值估計的自適應(yīng)初始化框架，并且提供了一種初始狀態(tài)由靜止/懸停、直線運動轉(zhuǎn)入非退化運動的初始參數(shù)估計方案。通過狀態(tài)模式選擇算法判斷相應(yīng)的初始狀態(tài)及對應(yīng)的初始化模塊，從而對初始狀態(tài)信息充分利用，實現(xiàn)精準估計，以提高視覺慣性聯(lián)合在線初始化的魯棒性、速度和精度。通過公開數(shù)據(jù)集和實際場景測試，并與目前代表性算法對比表明，所提方法魯棒性較好，在參數(shù)估計收斂性能上有較大提升，軌跡誤差也明顯減少。

同時定位與建圖；不確定狀態(tài)；自適應(yīng)；狀態(tài)模式選擇；初始化模塊

0 引言

近年來，小型無人機（small unmanned aerial vehicle，sUAV）因其優(yōu)越的機動性能，廣泛應(yīng)用于環(huán)境勘測、搜索救援、測繪攝影、電力巡檢等領(lǐng)域。然而，在諸如室內(nèi)、城市、地下、峽谷等環(huán)境下工作時，無法接收全球衛(wèi)星導航系統(tǒng)（global navigation satellite system, GNSS）信號，只能依賴自身攜帶的傳感器實現(xiàn)自主飛行。機載單目相機和慣性測量單元（inertial measurement unit, IMU）從性能上來說具有天然的互補性，且因成本低、重量輕、尺寸小的特點，成為極佳的實現(xiàn)sUAV自主飛行和環(huán)境感知的傳感器套件。因此，單目視覺慣性系統(tǒng)得到了廣泛的研究。

由于視覺慣性導航系統(tǒng)（visual-inertial navigation system, VINS）系統(tǒng)的非線性，其性能在很大程度上依賴于重力矢量、1MU偏置、速度等初始值的精度和收斂速度。較差初始化會導致收斂速度過慢甚至估計錯誤，特別是對于空中飛行的無人機來說，精確的初始值和快速的初始化是至關(guān)重要的。文獻[1]引入了一種閉式解，通過在短時間獲取慣性和視覺測量值，簡單地對一個線性系統(tǒng)求逆，得到絕對比例、滾動和俯仰角、初始速度，以及到3維（three-dimensional, 3D）特征的距離。之后文獻[2]中推導了聯(lián)合求解尺度、重力、加速度計偏差、初始速度和視覺特征深度更直觀的表達方式。文獻[3]改進了文獻[2]的方法，增加了陀螺儀偏差校準，但忽略了IMU的噪聲特性。這幾項研究僅僅是理論上的描述，在大多數(shù)實際情況下，并不能提供足夠好的初始化效果。文獻[4]擴展了前人的工作，采用文獻[5-6]中提出的預(yù)積分方法，通過添加2輪視覺慣性約束提高準確性，但該方法在初始化時僅跟蹤20%的軌跡點，對于系統(tǒng)需要立即啟動的無人機來說，可能會估計失敗。文獻[7]在文獻[8]的基礎(chǔ)上，提出了一種能夠計算所需初始參數(shù)的IMU初始化算法，然而為了獲得準確的估計，該方法需要15 s左右。文獻[9]為確?？焖俪跏蓟?，忽略了加速度偏置，該方法在初始階段需要滿足一定加速度激勵條件，而且對視覺特征匹配有所依賴，如果在短時間內(nèi)特征點跟蹤失敗，后續(xù)的聯(lián)合初始化則無法完成。文獻[10]在文獻[7]基礎(chǔ)上改進了一種包含多要素的同步估計加速度計偏置和關(guān)鍵幀速度的初始化方法，通過提升加速度計偏置的收斂速度和準確性來提升初始化的速度。文獻[11-12]提出的方法只能在已知初始參數(shù)或者靜止狀態(tài)下啟動系統(tǒng)。文獻[13]中的研究需要無人機在開始時幾乎水平起飛，以完成初始化過程。在文獻[14]中提出的初始化方法要求初始姿態(tài)應(yīng)與重力方向?qū)R。如果沒有先驗信息，這幾種方法都不適合實時初始化。

由此，本文考慮無人機執(zhí)行任務(wù)過程系統(tǒng)的實際運行情況，以單目視覺慣性導航系統(tǒng)（visual-inertial navigation system-monocular, VINS-Mono）算法為框架，提出一種更完備的單目VINS初始化方法，充分利用初始啟動階段的有效信息，通過對初始狀態(tài)進行判斷，自主地選擇初始化模式，以期在提高單目視覺慣性系統(tǒng)魯棒性的同時，實現(xiàn)VINS的快速初始化。

1 IMU模型、預(yù)積分及位姿關(guān)系轉(zhuǎn)換

1.1 位姿關(guān)系轉(zhuǎn)換

圖1 坐標系關(guān)系轉(zhuǎn)換

考慮單目相機的尺度因子，本體坐標系與世界坐標系之間的位姿約束關(guān)系可表示為：

1.2 IMU測量模型

IMU慣性測量單元通常包括3軸加速度計和陀螺儀，其測量值誤差主要包括系統(tǒng)誤差和隨機誤差，考慮到可以實時在線估計的誤差參數(shù)有限，僅認為測量值受到高斯白噪聲和隨時間變化的偏置的影響，其加速度和角速度測量模型表示為：

1.3 預(yù)積分處理

2 視覺慣性自適應(yīng)初始化

本文算法基于VINS-Mono重點對初始化部分進行改進，流程框架如圖2所示。

圖2 初始化流程

2.1 無人機初始狀態(tài)模式檢測

在單目VINS系統(tǒng)中，初始值估計很大程度上決定了系統(tǒng)的性能；而初始值的計算又和運動狀態(tài)有關(guān)，在一些退化運動（如勻速運動）下，是難以或無法進行初始狀態(tài)估計的。其中VINS-Mono系統(tǒng)采用的動態(tài)初始化方式便是要求無人機必須處于非退化的運動狀態(tài)，即具有一定的加速度激勵才會進入視覺慣性初始化階段，從而限制了無人機的啟動方式，并不完全符合無人機在執(zhí)行任務(wù)中實際運動情況，而且也不能保證初始化成功。

因此本文考慮實際情況，提出一種狀態(tài)模式選擇算法，通過獲得的傳感器信息判斷起始運動狀態(tài)，進而決定初始化方式，達到更魯邦更精準的效果，具體算法如圖3所示。

圖3 狀態(tài)模式選擇算法示意

首先將IMU數(shù)據(jù)與圖像數(shù)據(jù)同步對齊，得到相鄰圖像幀中匹配特征點的平均像素差為

若平均像素差小于閾值，則判斷為靜止或懸停。否則分別計算相鄰圖像幀間加速度數(shù)據(jù)方差和平均角速度，對變速直線運動和非退化運動2種初始狀態(tài)進行判斷。

2.2 視覺慣性初始化方法

本文根據(jù)無人機初始運動狀態(tài)的不同，劃分不同的初始化方式，主要針對圖2所示的3種初始運動模式進行判斷分析，進而估計相應(yīng)模式下的初始速度、重力、加速度偏置、陀螺儀偏置、尺度因子等初值。文中討論的3種初始運動狀態(tài)如圖4所示。

2.2.1 模式一（靜止/懸停到非退化運動狀態(tài)）

無人機在實際執(zhí)行任務(wù)或?qū)嶒灂r，難免會由靜止開始啟用。為不失一般性，我們將懸停和靜止劃分為一類，此模式下運動狀態(tài)如圖4（a）所示，無人機由靜止/懸停轉(zhuǎn)入非退化運動，即轉(zhuǎn)入非勻速運動、直線運動等不會導致額外不可觀方向的運動狀態(tài)。其初始參數(shù)估計方法討論如下。

首先利用靜止/懸停狀態(tài)數(shù)據(jù)估計IMU偏置，此階段無人機初始速度、初始位置分別為零。IMU坐標系下測得的重力加速度方向為

世界坐標系軸方向矢量在IMU坐標系下的投影為

因而可以求解出加速度計偏置為

陀螺儀偏置由角速度觀測值為

然后在無人機運動后，通過將圖像數(shù)據(jù)和加速度數(shù)據(jù)對齊以恢復尺度。

考慮連續(xù)3個圖像幀數(shù)據(jù)并消除中間速度，得到關(guān)于尺度的等式為

2.2.2 模式二（直線運動到非退化運動狀態(tài)）

此模式如圖4（b）所示，無人機開始處于直線運動狀態(tài)，然后轉(zhuǎn)入非退化運動。在這一運動過程中，陀螺儀偏置估計使用變速直線運動階段的數(shù)據(jù)，由式（13）獲得。在進入非退化運動后，利用圖像信息和IMU數(shù)據(jù)之間的約束關(guān)系，采用視覺慣性松耦合的方式對速度、重力、加速度偏置和尺度因子的初值進行估計。

考慮到重力向量的大小已知，只有2個自由度，且為了保持求解模型的線性特性，在其切線空間上用2個變量來參數(shù)化重力為

因此定義要求解的狀態(tài)變量為

式中表示單位矩陣。

最后構(gòu)建最小二乘問題為

2.2.3 模式三（非退化運動狀態(tài)）

無人機在此模式下一直處于非退化運動狀態(tài)，如圖4（c）所示。因此通過圖像數(shù)據(jù)和IMU數(shù)據(jù)之間的關(guān)系獲得初始參數(shù)值。陀螺儀偏置求解通過最小化IMU旋轉(zhuǎn)同相機旋轉(zhuǎn)之間的誤差為

2.3 滑動窗口緊耦合非線性優(yōu)化

通過以上聯(lián)合初始化過程獲得狀態(tài)量初值后，繼續(xù)采用文獻[9]中基于非線性優(yōu)化的滑動窗口法進行優(yōu)化，進一步估計各參數(shù)值。將滑動窗口中要優(yōu)化的狀態(tài)定義為

最后通過最小化誤差量獲得所需參數(shù)為

3 實驗與結(jié)果分析

本文在VINS-Mono框架基礎(chǔ)上對所提出的算法進行了驗證，分別在公開數(shù)據(jù)集和真實實驗室場景中進行測試，并同VINS-Mono算法進行對比分析。公開數(shù)據(jù)集采用尤·羅·奇（EuRoC）數(shù)據(jù)集[15]進行測試，實驗中采用的處理平臺為Intel i7-5500U處理器、主頻1.8 GHz、8 GB內(nèi)存和256 GB固態(tài)硬盤。

3.1 EuRoC數(shù)據(jù)集測試

EuRoC數(shù)據(jù)集由微型飛行器（micro air vehicle,MAV）在一間廠房和2個不同的室內(nèi)場景中飛行采集而得，其中包括200 Hz的IMU測量數(shù)據(jù)和 20 Hz的雙目圖像信息，以及由激光跟蹤掃描儀徠卡（Leica） MS50、運動捕捉系統(tǒng)維康（VICON）提供的真實數(shù)據(jù)。在數(shù)據(jù)集測試中，本文使用左目相機獲得的圖像信息。

3.1.1 IMU初始化參數(shù)收斂性分析

我們在V1_02_medium序列上對IMU初始化參數(shù)收斂性及精度進行測試。此序列中，MAV位于室內(nèi)并且由靜止開始起飛。實驗得到的加速度偏置、陀螺儀偏置估計值隨時間變化情況分別如圖5、圖6所示。

圖5 加速度計偏置估計過程

圖6 陀螺儀偏置估計過程

從陀螺儀偏置估計情況來看，VINS-Mono因只能在運動過程中才可以進行初始化化，且估計的初始值遠離真實值，導致收斂速度較慢。而本文提出的方法因有效利用了靜止階段的數(shù)據(jù)，提供了較為精確的初始值，在后端優(yōu)化時很快就達到了收斂，且估計過程波動較小，收斂速度有明顯提升，估值精度提高了約22.4%。

3.1.2 軌跡誤差分析

為進一步驗證本文所提出算法的整體性能，我們分別在V1_02_medium、V2_02_medium 2個序列上對軌跡誤差進行了測試，并同VINS-Mono算法進行對比。2個算法在未開回環(huán)情況下的測試結(jié)果如圖7、圖8所示，圖中包含本文算法軌跡、VINS-Mono算法軌跡和真實軌跡。從生成結(jié)果來看：因本文方法加入了自適應(yīng)初始化模塊，提供了精確的初值估計；而VINS-Mono算法獲得的初值同真實值相差較大，波動幅度明顯，收斂速度慢，導致這一時間段內(nèi)的整個系統(tǒng)位姿優(yōu)化的精度不高，從而影響了整體的定位結(jié)果。所以我們的方法整體表現(xiàn)性能優(yōu)于VINS-Mono，誤差偏移量較小，尤其是在軌跡變化比較劇烈的地方，誤差較VINS-Mono明顯減少，定位精度更高。

圖7 V1_02_medium序列軌跡結(jié)果

圖8 V2_02_medium序列軌跡結(jié)果

取本文方法和VINS-Mono在9個EuRoC數(shù)據(jù)序列上多次測試結(jié)果的中位數(shù)，統(tǒng)計測試結(jié)果的誤差最大值和均方根誤差并匯總成表1。表1中：Max為最大誤差；RMSE（root mean square error）為均方根誤差。

表1 EuRoC數(shù)據(jù)集統(tǒng)計結(jié)果對比 m

從整體測試效果上來看所提方法總體定位精度高于VINS-Mono，有效減少了最大軌跡誤差值，說明了準確的初值估值可以提升整個系統(tǒng)的性能。但也可以看到在部分序列中二者結(jié)果相差并不大。通過實驗發(fā)現(xiàn)在這些序列中，無人機雖然在開始時處于靜止，但由于IMU元器件并沒有處于穩(wěn)定狀態(tài)，數(shù)據(jù)存在小幅波動，獲得的IMU偏置初值精度并不高；但由于這一階段速度和位置都置為零，等于真實值，所以效果也有小幅提升。

3.2 真實場景測試

我們基于實驗室真實環(huán)境，使用Intel Real Sense D435i相機進行數(shù)據(jù)采集并進行算法對比測試。在實驗前首先采用imu_tk工具包對相機中的IMU內(nèi)參值進行標定，以作為參照值。實驗過程中先將相機靜止放置，然后手持相機在實驗室以1.5 m/s的速度采集了一段方形軌跡數(shù)據(jù)，以2 m/s的速度采集了一段八字形軌跡數(shù)據(jù)。圖9畫出了方形軌跡的IMU偏置隨時間的估計結(jié)果，圖中包含本文算法和VINS-Mono算法的收斂變化情況以及標定值。從整體結(jié)果中可以看出：VINS-Mono算法的估計過程震蕩幅度較大，收斂速度較慢；而本文算法有著相對較好的性能。

為驗證所提方法的魯棒性，在沒有回環(huán)檢測的情況下分別運行所采集的方形軌跡和八字形軌跡數(shù)據(jù)，結(jié)果如圖10、圖11所示。從圖10二幅圖中對比可以看出，VINS-Mono算法在原點（0,0）處的軌跡發(fā)生了漂移，這可以從圖10中得知：開始時VINS-Mono通過視覺慣性松耦合獲得的IMU偏置初值較差，且在后端優(yōu)化中IMU偏置估計過程波動較大，導致開始時解算的位姿結(jié)果誤差較大，發(fā)生明顯的偏移，整個運行軌跡也發(fā)生了傾斜；而改進的方法因采用了靜止階段數(shù)據(jù)，在后端優(yōu)化前獲得了較好的初值參數(shù)，整個過程定位準確，沒有發(fā)生明顯偏移現(xiàn)象。從圖11中同樣可以看出：因VINS-Mono只能動態(tài)初始化，嚴重依賴于運動后的IMU和圖像數(shù)據(jù)，以致初始化較慢，在初始化成功之前無法進行定位，導致起始部分軌跡缺失，整個軌跡發(fā)生傾斜；而改進的方法在運動后極短的時間內(nèi)便完成初始化并實現(xiàn)了IMU偏置的收斂，因此較好地完成了整段軌跡的定位。

圖10 方形軌跡運行結(jié)果對比

圖11 八字形軌跡運行結(jié)果對比

4 結(jié)束語

為提高VINS的魯棒性和精度，本文基于VINS-Mono框架進行了改進，提出了一種自適應(yīng)的單目視覺慣性初始化方法，根據(jù)無人機初始運動狀態(tài)的不同，細分為3個初始化模塊，同時在VINS-Mono動態(tài)初始化的基礎(chǔ)上增加了加速度偏置的估計，可以獲得全要素初始參數(shù)值。通過在EuRoC公開數(shù)據(jù)集和真實場景中與VINS-Mono算法進行對比分析，本文的方法提供了更加準確的初始參數(shù)，能夠?qū)崿F(xiàn)更快的收斂，從而表現(xiàn)出更好的魯棒性和精度，但同時要求相應(yīng)狀態(tài)下具有穩(wěn)定的IMU數(shù)據(jù)輸出。后續(xù)我們將考慮勻速直線運動、勻速圓周運動等退化運動下初始化解決方案及狀態(tài)估計方法，由此進一步提高VIO算法的魯棒性和精度。

[1] MARTINELLI A. Vision and IMU data fusion: closed-form solutions for attitude, speed, absolute scale, and bias determination[J]. IEEE Transactions on Robotics, 2012, 28(1):44-60.

[2] MARTINELLI A. Closed-form solution of visual-inertial structure from motion[J]. International Journal of Computer Vision, 2014, 106(2):138-152.

[3] KAISER J, MARTINELLI A, FONTANA F, et al. Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation[J]. IEEE Robotics and Automation Letters, 2017, 2(1):18-25.

[4] CAMPOS C, MONTIEL J M M, JUAN D T. Fast and robust initialization for visual-inertial SLAM[J]. IEICE Transactions on Fundamentals of Electronics, Communications and Computer Sciences, 2019, 11(10):1288-1294.

[5] LUPTON T, SUKKARIEH S. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[J]. IEEE Transactions on Robotics, 2012, 28(1): 61-76.

[6] FORSTER C, CARLONE L, DELLAERT F, et al. On-manifold preintegration for real-time visual-inertial odometry[J]. IEEE Transactions on Robotics, 2017, 33(1):1-21.

[7] MUR A R, TARDOS J D. Visual-inertial monocular slam with map reuse[J]. IEEE Robotics and Automation Letters, 2017, 2(2): 796-803.

[8] MUR A R, MONTIEL J M M, JUAN D T. ORB-SLAM: a versatile and accurate monocular slam system[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.

[9] QIN Tong, LI Peiliang, SHEN Shaojie. VINS-Mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4):1004-1020.

[10] 陳潤澤，郝向陽，苗書鋒. 一種改進的多要素視覺/慣性初始化方法[J]. 導航定位學報, 2022, 10(2): 37-43.

[11] WEISS S, ACHTELIK M W, LYNEN S, et al. Real-time onboard visual-inertial state estimation and self-calibration of mavs in unknown environments[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of International Conference on Robotics & Automation. Saint Paul, USA: IEEE, 2013: 957-964[2022-04-26].

[12] EAGLE S J. Visual-inertial navigation, mapping and localization: a scalable real-time causal approach[J]. The International Journal of Robotics Research, 2011, 30(4):407-430.

[13] FAESSLER M, FONTANA F, FORSTER C, et al. Automatic re-initialization and failure recovery for aggressive flight with a monocular vision-based quadrotor[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of International Conference on Robotics & Automation. Seattle, USA: IEEE, 2015:1722-1729.

[14] WEISS S, BROCKERS R, ALBREKTSEN S, et al. Inertial optical flow for throw-and-go micro air vehicles[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of Winter Conference on Applications of Computer Vision. Waikoloa, USA: IEEE, 2015: 262-269[2022-04-26].

[15] BURRI M, NIKOLIC J, GOHL P, et al. The EuRoC micro aerial vehicle datasets[J]. International Journal of Robotics Research, 2016, 35(10):1157-1163.

A visual-inertial SLAM method with adaptive initialization

FU Peng, LI Shuaixin, YU Tianyou, YAO Wen, CHEN Xiaoqian

（Defense Innovation Institute, Chinese Academy of Military Science, Beijing 100071, China）

Aiming at the problems of poor robustness, limited application conditions and slow convergence speed of the traditional monocular visual-inertial system in the initialization stage, a monocular visual-inertial simultaneous localization and mapping (SLAM) initialization method based on uncertain motion state is proposed. Based on the nonlinear optimization theory of visual-inertial navigation system (VINS), this method establishes a set of adaptive initialization framework including initial value estimation of all elements, and provides an initial parameter estimation scheme for the initial state from static/hovering, linear motion to non-degenerate motion. The corresponding initial state and the corresponding initialization module are judged by the state mode selection algorithm, so as to make full use of the initial state information, achieve accurate estimation and improve the robustness, speed and accuracy of visual-inertial joint online initialization. Through the public datasets and the actual scene test, and compared with the current representative algorithms, it is shown that the proposed method has good robustness, greatly improves the convergence performance of parameter estimation, and significantly reduces the trajectory error.

simultaneous localization and mapping; uncertainty state; self-adaption; state mode selection; initialization module

P228

A

2095-4999(2022)06-0027-10

付鵬，李帥鑫，喻天佑，等. 一種自適應(yīng)初始化的視覺慣性SLAM方法[J]. 導航定位學報, 2022, 10(6): 27-36.（FU Peng, LI Shuaixin, YU Tianyou, et al. A visual-inertial SLAM method with adaptive initialization[J]. Journal of Navigation and Positioning, 2022, 10(6): 27-36.）

10.16547/j.cnki.10-1096.20220604.

2022-05-30

國家自然科學基金項目（11725211）。

付鵬（1995—），男，山東淄博人，碩士研究生，研究方向為視覺SLAM、組合導航。

李帥鑫（1992—），男，河南鄭州人，博士，助理研究員，研究方向為多傳感器融合SLAM、三維實景地圖構(gòu)建、組合導航。