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

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

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

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

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

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

0 引言

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

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

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

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

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

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

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

1.2 IMU測(cè)量模型

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

1.3 預(yù)積分處理

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

本文算法基于VINS-Mono重點(diǎn)對(duì)初始化部分進(jìn)行改進(jìn)，流程框架如圖2所示。

圖2 初始化流程

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

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

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

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

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

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

2.2 視覺慣性初始化方法

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

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

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

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

世界坐標(biāo)系軸方向矢量在IMU坐標(biāo)系下的投影為

因而可以求解出加速度計(jì)偏置為

陀螺儀偏置由角速度觀測(cè)值為

然后在無人機(jī)運(yùn)動(dòng)后，通過將圖像數(shù)據(jù)和加速度數(shù)據(jù)對(duì)齊以恢復(fù)尺度。

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

2.2.2 模式二（直線運(yùn)動(dòng)到非退化運(yùn)動(dòng)狀態(tài)）

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

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

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

式中表示單位矩陣。

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

2.2.3 模式三（非退化運(yùn)動(dòng)狀態(tài)）

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

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

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

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

3 實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

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

圖5 加速度計(jì)偏置估計(jì)過程

圖6 陀螺儀偏置估計(jì)過程

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

3.1.2 軌跡誤差分析

為進(jìn)一步驗(yàn)證本文所提出算法的整體性能，我們分別在V1_02_medium、V2_02_medium 2個(gè)序列上對(duì)軌跡誤差進(jìn)行了測(cè)試，并同VINS-Mono算法進(jìn)行對(duì)比。2個(gè)算法在未開回環(huán)情況下的測(cè)試結(jié)果如圖7、圖8所示，圖中包含本文算法軌跡、VINS-Mono算法軌跡和真實(shí)軌跡。從生成結(jié)果來看：因本文方法加入了自適應(yīng)初始化模塊，提供了精確的初值估計(jì)；而VINS-Mono算法獲得的初值同真實(shí)值相差較大，波動(dòng)幅度明顯，收斂速度慢，導(dǎo)致這一時(shí)間段內(nèi)的整個(gè)系統(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個(gè)EuRoC數(shù)據(jù)序列上多次測(cè)試結(jié)果的中位數(shù)，統(tǒng)計(jì)測(cè)試結(jié)果的誤差最大值和均方根誤差并匯總成表1。表1中：Max為最大誤差；RMSE（root mean square error）為均方根誤差。

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

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

3.2 真實(shí)場(chǎng)景測(cè)試

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

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

圖10 方形軌跡運(yùn)行結(jié)果對(duì)比

圖11 八字形軌跡運(yùn)行結(jié)果對(duì)比

4 結(jié)束語

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

國家自然科學(xué)基金項(xiàng)目（11725211）。

付鵬（1995—），男，山東淄博人，碩士研究生，研究方向?yàn)橐曈XSLAM、組合導(dǎo)航。

李帥鑫（1992—），男，河南鄭州人，博士，助理研究員，研究方向?yàn)槎鄠鞲衅魅诤蟂LAM、三維實(shí)景地圖構(gòu)建、組合導(dǎo)航。