劉 剛，葛洪偉+

1.江蘇省模式識別與計(jì)算智能工程實(shí)驗(yàn)室（江南大學(xué)），江蘇 無錫 214122

2.江南大學(xué) 人工智能與計(jì)算機(jī)學(xué)院，江蘇 無錫 214122

同時定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）研究工作最早可以追溯到1986年，至今已有30 多年歷史。SLAM 是機(jī)器人與計(jì)算機(jī)視覺領(lǐng)域中的基本問題，它主要研究裝置通過各種傳感器在未知環(huán)境中感知和定位自身方位并同時構(gòu)建環(huán)境三維地圖。使用單目相機(jī)來進(jìn)行運(yùn)動狀態(tài)的評估的方法由于其體積小、成本低和硬件設(shè)置簡單而獲得了社會的極大興趣[1-5]。然而，僅僅使用單目視覺系統(tǒng)無法恢復(fù)度量尺度，這樣也限制了它們在實(shí)際工程中的應(yīng)用。隨著智能手機(jī)等消費(fèi)電子的迅猛發(fā)展，慣性測量單元（inertial measurement unit，IMU）和相機(jī)模組的迅速普及，用低成本慣性測量單元輔助視覺系統(tǒng)進(jìn)行狀態(tài)評估已成為相關(guān)產(chǎn)業(yè)的發(fā)展趨勢。

近年來很多優(yōu)秀單目視覺慣導(dǎo)SLAM 系統(tǒng)被提出，比 如MSCKF[2]、視覺慣導(dǎo)ORB-SLAM[3]、Vins-Mono[4]以及蘇黎世大學(xué)Leutenegger 等人提出的OKVIS[5]（open keyframe based visual inertial SLAM）等。根據(jù)上述系統(tǒng)的求解方法，可以劃分為基于濾波[6]的緊耦合和基于優(yōu)化[7]的緊耦合。與濾波法[7-10]相比，基于優(yōu)化的緊耦合方法對一段時間內(nèi)所有時刻的所有狀態(tài)同時進(jìn)行優(yōu)化，有其較高的精度和計(jì)算效率[11]，從而引起了學(xué)術(shù)界的極大的興趣。但是這些基于優(yōu)化的單目視覺慣導(dǎo)融合的SLAM 算法有高度的非線性性，其性能嚴(yán)重依賴于初始狀態(tài)（視覺尺度、慣性傳感器偏置等）估計(jì)的準(zhǔn)確度[12-15]。若初始狀態(tài)不準(zhǔn)確，其誤差將隨著系統(tǒng)運(yùn)行時間的增加而不斷積累，大大降低系統(tǒng)的收斂速度甚至導(dǎo)致不正確的估計(jì)結(jié)果。另一方面，慣性測量單元需要加速度激勵，以便于進(jìn)行相關(guān)測量。這意味著IMU 不能從靜止?fàn)顟B(tài)啟動，必須從未知的移動狀態(tài)啟動。因此如何選擇這一未知的移動狀態(tài)在初始化階段也顯得尤為重要。

不少學(xué)者對視覺慣導(dǎo)的SLAM 的初始化方法進(jìn)行了深入研究；在文獻(xiàn)[16]中提出一種確定性的解析解來估計(jì)重力加速度和視覺尺度的初始化算法，但是該算法由于忽略了IMU 偏置導(dǎo)致評估精度并不高。文獻(xiàn)[17]中作者假定了微小型飛行器的初始狀態(tài)處于近似水平的環(huán)境下，進(jìn)行松耦合的系統(tǒng)初始化。類似的初始化方法在文獻(xiàn)[18]中也被提出，作者假設(shè)重力加速度向量與飛行器坐標(biāo)系進(jìn)行了對齊，在這條件下進(jìn)行初始化。然而以上這兩種方法，在沒有先驗(yàn)條件下，飛行器并不能真正實(shí)現(xiàn)在線初始化。此外，在文獻(xiàn)[19-20]中提出的視覺慣導(dǎo)初始化方法，由于陀螺儀偏置在系統(tǒng)初始化時被忽略，導(dǎo)致初始狀態(tài)不能被準(zhǔn)確估計(jì)。港科大Qin 等人提出了一種較有開創(chuàng)性的視覺慣導(dǎo)初始化算法[4,21]，并在其貢獻(xiàn)的開源算法Vins-Mono 中實(shí)現(xiàn)。該算法可以在運(yùn)行中對慣性測量單元的各初始狀態(tài)進(jìn)行標(biāo)定，但是該初始化方法忽略了慣性測量單元加速度偏置誤差和測量噪聲，從而影響了系統(tǒng)初始化的準(zhǔn)確度和運(yùn)行效率。Mur-Artal 等人提出了一個較為高效的視覺慣導(dǎo)初始化方法[22]，將初始化過程細(xì)化為多個步驟進(jìn)行，但是由于分步進(jìn)行求解各初始狀態(tài)，并未充分考慮誤差狀態(tài)的傳播對估計(jì)結(jié)果的影響，且沒有設(shè)定魯棒的終止條件來判斷初始化是否成功。Forster等人提出了慣性測量單元在流形面的預(yù)積分方法[23]，該方法是對IMU 觀測值進(jìn)行數(shù)值積分的理論基礎(chǔ)，但是其并未對誤差狀態(tài)的傳播機(jī)理進(jìn)行詳細(xì)分析。

本文在預(yù)積分理論基礎(chǔ)上利用最大后驗(yàn)估計(jì)（maximum a posteriori estimation，MAP）推導(dǎo)出了一個線性時變初始化系統(tǒng)來對尺度、速度、位移、姿態(tài)、加速度、慣性測量單元偏置等初始運(yùn)動狀態(tài)進(jìn)行聯(lián)合估計(jì)。這種統(tǒng)一求解的方法，綜合考慮了系統(tǒng)誤差狀態(tài)的傳播對初始化系統(tǒng)的影響，以及重力加速度的約束這兩個因素，因此可以對重力加速度方向和加速度偏置進(jìn)行精確求解。此外，本文提出了一種判別視覺慣導(dǎo)初始化算法終止條件的方法，通過提取觀測數(shù)據(jù)在估計(jì)方程上的費(fèi)歇爾信息矩陣（Fisher information matrix）[24]來評估估計(jì)的狀態(tài)準(zhǔn)確度，從而加快了算法初始化的收斂時間，提高了初始化的魯棒性。

1 Vins-Mono 初始化方法及其缺陷

Vins-Mono 初始化算法是一種松耦合傳感器融合的初始化方法。它首先使用視覺運(yùn)動中恢復(fù)結(jié)構(gòu)（structure from motion，SFM）算法獲得比較有價值的初始值，與IMU 的預(yù)積分結(jié)果構(gòu)建線性方程進(jìn)行分步求解，得出尺度、重力加速度、速度以及IMU 偏置的初始值，其初始化流程如圖1 所示。

Vins-Mono 初始化算法存在以下缺陷：首先，它忽略了IMU 的加速度偏置對系統(tǒng)的影響，此舉必然會影響算法求解的準(zhǔn)確度；其次，它采用逐步求解的方法，因而無法綜合考慮IMU 的觀測噪聲和偏置噪聲導(dǎo)致的誤差狀態(tài)對系統(tǒng)的影響；再次，由于其沒有考慮算法的終止條件，僅僅簡單地利用初始階段中某一段時間的數(shù)據(jù)進(jìn)行初始化，嚴(yán)重影響了算法的收斂速度和魯棒性。

2 慣性測量單元模型

IMU 測量的是離散的比力和角速度，其測量值含有隨機(jī)游走且緩慢變化的偏置，以及高斯分布的測量噪聲，因此想要得到每個時刻傳感器的位置、速度、姿態(tài)等狀態(tài)值，首先需要知道傳感器的初始時的位置、速度、姿態(tài)等信息，然后對傳感器的測量值進(jìn)行數(shù)值積分。一般情況下慣性測量單元的評估狀態(tài)可以表示為：

Fig.1 Vins-Mono initialization flow圖1 Vins-Mono 初始化流程

式中，p∈R3表示IMU的位移，四元數(shù)q∈? 表示IMU相對于世界坐標(biāo)系的旋轉(zhuǎn)，表示IMU 的運(yùn)動速度，bg∈R3表示IMU 的陀螺儀偏置，ba∈R3表示IMU 的加速度計(jì)偏置。

2.1 慣性傳感器測量模型

IMU 的測量值是傳感器視加速度和角速度的離散值，通常這些測量值會受到傳感器隨時間緩慢變化的偏置和測量白噪聲的影響，因此其測量值由實(shí)際狀態(tài)、隨機(jī)高斯噪聲和偏置三部分疊加而成，可以用式（2）建模。

式中，(·)m表示在t時刻點(diǎn)IMU 的原始測量值，(·)t表示在t時刻傳感器的實(shí)際狀態(tài)。R表示IMU 坐標(biāo)系相對于世界坐標(biāo)系的位姿（旋轉(zhuǎn)矩陣）假設(shè)測量的加速度和角速度的噪聲服從高斯分布有，。由于加速度偏置ba和角速度偏置bg擾動是隨機(jī)游走的，因而相應(yīng)的導(dǎo)數(shù)也服從高斯分布,可以表示為。

2.2 慣性傳感器運(yùn)動學(xué)模型

IMU 的測量數(shù)據(jù)視加速度am與角速度ωm均相對于IMU 坐標(biāo)系，需要將其轉(zhuǎn)換到世界坐標(biāo)系中，定義表示四元數(shù)q與旋轉(zhuǎn)矩陣R的對應(yīng)關(guān)系。根據(jù)IMU 的測量模型（2）和偏置隨時間游走的噪聲模型，利用IMU 測量數(shù)據(jù)和積分動力學(xué)模型[25]來描述IMU 的位姿與速度。在式（3）中定義u?(am,ωm) 是IMU 在某一時刻點(diǎn)測量值的集合，運(yùn)算符?表示四元數(shù)乘法。n?[na,ng,nbg,nba]T用來表示IMU 測量噪聲和偏置噪聲的集合，其協(xié)方差一般會在IMU 的數(shù)據(jù)手冊中給出或者通過相關(guān)實(shí)驗(yàn)進(jìn)行標(biāo)定。

2.3 預(yù)積分

預(yù)積分的思想在文獻(xiàn)[26]中首次被提出，F(xiàn)orster等人將其進(jìn)一步拓展到李群流形上，形成了一套優(yōu)雅的理論體系[23]。本文使用了基于流形的IMU 預(yù)積分思想對IMU 數(shù)據(jù)進(jìn)行處理。通常情況下，如圖2 所示IMU 測量數(shù)據(jù)的輸出頻率遠(yuǎn)高于相機(jī)等視覺傳感器。當(dāng)視覺與慣導(dǎo)信息進(jìn)行融合時，首先需要考慮兩者測量頻率不同步的問題。IMU 的預(yù)積分是指預(yù)先對兩個圖像間的IMU 觀測數(shù)據(jù)進(jìn)行積分，此方法能極大地降低IMU 的輸出頻率，且有效地解決視覺和IMU 慣導(dǎo)兩種傳感器頻率不一致的問題，同時也能獲得相鄰視覺幀之間的相對位置。

Fig.2 IMU and visual observation model圖2 IMU 與視覺觀測模型

2.3.1 標(biāo)準(zhǔn)狀態(tài)傳播

假設(shè)在上述運(yùn)動解析方程（3）中忽略噪聲對系統(tǒng)的影響得到標(biāo)準(zhǔn)狀態(tài)的IMU 運(yùn)動方程。

將以上運(yùn)動解析方程（4）進(jìn)行離散化，i時刻點(diǎn)到j(luò)時刻點(diǎn)的狀態(tài)轉(zhuǎn)移方程F(?,ui:j)一般由狀態(tài)變量運(yùn)算和積分項(xiàng)構(gòu)成[25]。

式（6）中Δt表示第i時刻點(diǎn)到第j時刻點(diǎn)之間的時間差，αij、βij、γij表示i時刻點(diǎn)到j(luò)時刻點(diǎn)的積分項(xiàng)（7）?？梢岳媒馕龇匠掏茖?dǎo)出不同精度的數(shù)值計(jì)算方法，例如歐拉法、中值法和龍格-庫塔法等方法進(jìn)行積分。

2.3.2 誤差狀態(tài)的運(yùn)動模型

雖然誤差狀態(tài)相對于標(biāo)準(zhǔn)狀態(tài)是比較細(xì)微的，但是隨著標(biāo)準(zhǔn)狀態(tài)模型的運(yùn)行，誤差會隨時間積累，變得越來越大。雖然慣性測量單元的觀測噪聲和擾動是隨機(jī)發(fā)生的，但是它們的統(tǒng)計(jì)學(xué)特征是可估計(jì)的。因此為了計(jì)算出精確的實(shí)際狀態(tài)，本文定義標(biāo)準(zhǔn)狀態(tài)與實(shí)際狀態(tài)之差為誤差狀態(tài)。一方面，在不考慮系統(tǒng)噪聲和擾動的情況下利用標(biāo)準(zhǔn)狀態(tài)傳播模型計(jì)算系統(tǒng)的標(biāo)準(zhǔn)狀態(tài)；另一方面，通過對誤差狀態(tài)隨時間的傳播進(jìn)行分析，推導(dǎo)出誤差狀態(tài)的統(tǒng)計(jì)學(xué)特征，為進(jìn)行最大后驗(yàn)估算做準(zhǔn)備，將誤差狀態(tài)δx定義為：

將系統(tǒng)的誤差狀態(tài)視為高斯分布，根據(jù)運(yùn)動方程和誤差狀態(tài)方程可以得到誤差狀態(tài)的傳播方程。不難發(fā)現(xiàn)慣性傳感器的狀態(tài)誤差傳播可以定義為時變線性系統(tǒng)。通過測量值、標(biāo)準(zhǔn)狀態(tài)和系統(tǒng)噪聲來估計(jì)誤差狀態(tài)傳播。由運(yùn)動方程（3）和誤差方程（8）得到如下誤差狀態(tài)運(yùn)動模型：

其中：

在不考慮噪聲影響下，根據(jù)誤差狀態(tài)傳播方程（9）得到誤差狀態(tài)轉(zhuǎn)移方程。

由誤差狀態(tài)的轉(zhuǎn)移方程（10）得到誤差狀態(tài)δx的遞歸形式的雅可比矩陣，表示為Ji+1=Φi,i+1Ji，其相應(yīng)的遞歸的初始狀態(tài)J0=I。另外，也可推導(dǎo)出在系統(tǒng)噪聲影響下，誤差狀態(tài)的協(xié)方差矩陣的傳播方程，表示為如下形式：

這里Qi表示噪聲的協(xié)方差矩陣，其為對角陣，定義為。

系統(tǒng)的實(shí)際狀態(tài)是由標(biāo)準(zhǔn)狀態(tài)疊加上相應(yīng)的誤差狀態(tài)，根據(jù)系統(tǒng)的標(biāo)準(zhǔn)狀態(tài)轉(zhuǎn)移方程（5）和誤差的定義（8）可推導(dǎo)出如下表達(dá)式：

式（12）中廣義加法⊕是?的逆運(yùn)算，定義如下：

由誤差狀態(tài)運(yùn)動模型（9）得到Φ=eAΔt。由于IMU 的采樣時間間隔較短，可對其進(jìn)行一階近似，得出Φ≈I+AΔt。在一些運(yùn)算資源比較充沛的系統(tǒng)中，可以根據(jù)解析解進(jìn)行更高階的泰勒近似，或者通過龍格-庫塔數(shù)值積分等方法得到更為精確的近似。

3 視覺慣導(dǎo)聯(lián)合初始化

進(jìn)行視覺慣導(dǎo)聯(lián)合初始化，首先需要執(zhí)行視覺部分的初始化，以獲得良好的初始值，然后進(jìn)行慣性測量單元的初始化。這種延遲初始化的方法有兩個好處：（1）IMU 能夠進(jìn)行充分運(yùn)動讓所有的變量可觀測；（2）在純視覺SLAM 的姿態(tài)評估后，系統(tǒng)進(jìn)行局部圖優(yōu)化，能夠優(yōu)化關(guān)鍵幀的位姿狀態(tài)，這樣就可以利用優(yōu)化后的關(guān)鍵幀位姿來初始化IMU。一般情況下，純視覺的初始化在進(jìn)行局部圖優(yōu)化后，可以獲得性能良好的相機(jī)初始姿態(tài)，這也為下一步進(jìn)行慣性測量單元的初始化打下了堅(jiān)實(shí)的基礎(chǔ)。在完成純視覺的初始化后，得到相機(jī)各關(guān)鍵幀的姿態(tài)，通過相機(jī)與慣性測量單元之間的標(biāo)定可以進(jìn)行相機(jī)坐標(biāo)與慣性測量單元坐標(biāo)之間的相互轉(zhuǎn)換。聯(lián)合慣性測量單元的預(yù)積分，執(zhí)行視覺慣導(dǎo)的聯(lián)合初始化，以獲得精準(zhǔn)的系統(tǒng)初始參數(shù)。

3.1 慣性測量殘差模型

已知慣性測量單元的運(yùn)動狀態(tài)Xj及在時間點(diǎn)i與j之間的測量集合為ui:j。可以通過標(biāo)準(zhǔn)狀態(tài)轉(zhuǎn)移方程（5）、誤差的定義（8）和誤差狀態(tài)的傳播模型（9），在忽略IMU 軸偏差和尺度誤差的情形下，將標(biāo)準(zhǔn)狀態(tài)和實(shí)際狀態(tài)之間的誤差近似為高斯白噪聲，表示如下：

協(xié)方差矩陣Pj可以由傳播方程（11）求得。因此將預(yù)測狀態(tài)與標(biāo)準(zhǔn)狀態(tài)之間的殘差表示如下：

根據(jù)標(biāo)準(zhǔn)狀態(tài)轉(zhuǎn)移方程（6）求得j時刻點(diǎn)的狀態(tài)，另外利用誤差狀態(tài)轉(zhuǎn)移方程（10）求得其相應(yīng)的誤差狀態(tài)轉(zhuǎn)移的雅可比矩陣。由于慣性測量單元的加速度偏置和旋轉(zhuǎn)偏置是隨時間緩慢變化的，且第i到j(luò)時間點(diǎn)之間的采樣時間Δt比較短，因此可以對其進(jìn)行一階近似，以估算出因偏置所產(chǎn)生的誤差。得到如下慣性測量單元運(yùn)動的殘差方程：

3.2 視覺狀態(tài)評估

目前視覺部分初始化的方法較為成熟，一般流程是通過篩選關(guān)鍵幀后，使用對極幾何、PnP（perspective-n-point）、ICP（iterative closest point）等SFM 算法進(jìn)行視覺幀的位姿評估，然后利用局部圖優(yōu)化對關(guān)鍵幀的位姿進(jìn)行進(jìn)一步修正，得到更精確的位姿數(shù)據(jù)。該部分延用了Vins-Mono 的相關(guān)視覺初始化算法，在得到較為精確的關(guān)鍵幀位姿數(shù)據(jù)后，將其用于視覺慣導(dǎo)聯(lián)合初始化算法。假定視覺初始化后的輸出第i關(guān)鍵幀的位姿值記為，由于單目視覺尺度信息的不可觀測性，位姿值沒有包含相應(yīng)的尺度信息，因此需要將其轉(zhuǎn)化為IMU 估計(jì)模型下的位姿值(qi,pi)，則有式（17）：

其中，i=0,1,…,N,s∈R 表示視覺尺度。

3.3 視覺慣性對齊

假設(shè)相鄰兩個關(guān)鍵幀的純視覺評估狀態(tài)分別為Xi、Xi+1，將視覺評估結(jié)果式（17）代入到殘差方程（16），發(fā)現(xiàn)慣性測量單元的待初始化狀態(tài)變量s、g、v、bg、ba在方程（18）中是線性的，這樣就變?yōu)橐阎d體的姿態(tài)狀態(tài)[qi,pi]來求解r(Z)的最小值的MAP問題，優(yōu)化方程表示如下：

其中，Z=[s,g,v,bg,ba]T，由于r(Z)是線性方程可以直接求解相關(guān)狀態(tài)變量，然而式（18）沒有考慮到||g||=9.8 的約束，很容易得到是病態(tài)的收斂結(jié)果。因此必須加上重力加速度的約束，可以得到如下優(yōu)化方程。

不難看出新的優(yōu)化方程是典型的二次約束二次規(guī)劃問題，這樣就變成一個凸函數(shù)最優(yōu)化問題。因?yàn)橥购瘮?shù)最優(yōu)化問題的局部解即是其全局解，理論上可以通過優(yōu)化方程（19）求解出視覺慣導(dǎo)在當(dāng)前觀測數(shù)據(jù)下初始狀態(tài)的最優(yōu)的解。對于方程（19）可以將其轉(zhuǎn)化為在黎曼流形上非約束的優(yōu)化問題進(jìn)行求解。

其中，回縮因子如下：

其中，ζ=[ζs,ζθ,ζv,ζbg,ζba]T，以上回縮因子可以確保其流形面上重力加速度g的強(qiáng)度保持不變。

IMU 是典型的內(nèi)感受型傳感器，需要運(yùn)動進(jìn)行激勵以便進(jìn)行觀測。這意味著不能從靜止?fàn)顟B(tài)來進(jìn)行初始化，需要慣性測量單元有充分的運(yùn)動，以便充分獲取慣性測量單元的測量數(shù)據(jù)的統(tǒng)計(jì)信息。因此確保觀測數(shù)據(jù)是由載體充分運(yùn)動所產(chǎn)生的，是算法魯棒性的重要前提條件。在統(tǒng)計(jì)學(xué)里，克拉美羅下界（Cramer-Rao lower bound，CRLB）設(shè)定了一個基本界限，它明確定義出利用已觀測值的估計(jì)效果來衡量估計(jì)方式好壞的標(biāo)準(zhǔn)，對于無偏估計(jì)其定義的邊界可以由費(fèi)歇爾信息矩陣來確定[24]。因此可以用費(fèi)歇爾信息矩陣來度量觀測數(shù)據(jù)估計(jì)的未知狀態(tài)的置信度，一般在正態(tài)分布的負(fù)對數(shù)似然估計(jì)中，費(fèi)歇爾信息矩陣即是其觀測值處的海森矩陣。這種關(guān)于樣本選擇的最差情形誤差評估的方法在文獻(xiàn)[27]和文獻(xiàn)[28]中也有相關(guān)論述和實(shí)驗(yàn)。在視覺慣導(dǎo)初始化系統(tǒng)中，通過計(jì)算r(Z)的海森矩陣來進(jìn)行最差情形的誤差評估，從而加快初始化的收斂速度，確保初始化的狀態(tài)變量的魯棒性。將觀測樣本最差情形評估函數(shù)定義如下：

4 效果評估

使用Euroc 數(shù)據(jù)集[29]來測試本文算法，其包含兩個場景，分別是工廠車間和臥室環(huán)境，利用無人機(jī)采集了11 個數(shù)據(jù)集，并按照光照、紋理、快速/慢速運(yùn)動、運(yùn)動模糊等分為容易、中等、困難3 個難度等級。Euroc 數(shù)據(jù)集包含同步的752×480 雙目圖像和IMU數(shù)據(jù)，頻率分別為20 Hz、200 Hz，并提供相機(jī)運(yùn)動ground-truth 軌跡。實(shí)驗(yàn)硬件平臺Core i7-6700HQ 2.6 GHz、8 GB RAM。

為了評估本文提出的視覺慣導(dǎo)初始化算法，在Vins-Mono 基礎(chǔ)上添加了該初始化算法，并在其視覺圖優(yōu)化窗口對視覺關(guān)鍵幀評估然后執(zhí)行優(yōu)化算法。使用Ceres[30]來實(shí)現(xiàn)牛頓高斯法在黎曼流形面上的迭代搜索，使算法快速收斂。經(jīng)多次實(shí)驗(yàn)初始化系統(tǒng)對估計(jì)的狀態(tài)變量實(shí)現(xiàn)收斂的時間為8～11 s。

圖3～圖6 為本文所提視覺慣導(dǎo)初始化算法與Vins-Mono 初始化算法在Euroc Machine Hall 03 數(shù)據(jù)集上測試對比結(jié)果。可以看出，陀螺儀偏置在約2～4 s收斂，其x、y、z3 個方向上最終收斂于[-0.003,0.125,0.058]，單位rad/s，陀螺儀偏置比加速度偏置收斂快。這是由于陀螺儀偏置相較于加速度偏置不需要區(qū)分重力加速度，而加速度偏置因?yàn)橛兄亓铀俣鹊挠绊?，所以相較于陀螺儀偏置收斂較慢。這也是引入最差情形評估函數(shù)時，將重力加速度和尺度相關(guān)費(fèi)歇爾信息作為最差情形評估參數(shù)的原因。實(shí)驗(yàn)中根據(jù)慣性傳感器特性，將加速度偏置初始值設(shè)為0，將其值域設(shè)置在[-0.5,0.5]，單位m/s2，這樣大大縮小了加速度偏置的迭代范圍，縮短了收斂時間。

Fig.3 Convergence process of gyroscope bias圖3 陀螺儀偏置的收斂過程

Fig.4 Convergence process of acceleration bias圖4 加速度偏置的收斂過程

Fig.5 Convergence process of visual scale圖5 視覺尺度的收斂過程

Fig.6 Worst case evaluation parameters圖6 最差情形評估參數(shù)

正如圖3 所示，Vins-Mono 初始化算法同樣都能實(shí)現(xiàn)快速收斂，但是由于其沒有對陀螺儀偏置進(jìn)行聯(lián)合優(yōu)化求解，而僅僅使用線性方程進(jìn)行分步求解，其精度略遜于本文算法。

如圖4 和圖5 所示，相較于陀螺儀偏置，加速度偏置和視覺尺度收斂較慢，Vins-Mono 初始化算法比本文初始化算法求解精度差，這是由于本文算法充分考慮了重力加速度的約束，并在凸函數(shù)上進(jìn)行數(shù)值優(yōu)化求解，從理論上保證局部最優(yōu)解即是全局最優(yōu)解。

如圖6 所示，利用最差情形評估函數(shù)Ω(Z)來評估初始化所獲得參數(shù)的置信度。大約在8 s 時，最差情形評估函數(shù)值小于實(shí)驗(yàn)所設(shè)算法終止條件的截止值（0.003），表示要估計(jì)的變量都已經(jīng)收斂。

5 結(jié)束語

針對傳統(tǒng)視覺慣導(dǎo)初始化算法收斂速度慢，精確魯棒性相對較差的問題，本文提出了一種對系統(tǒng)各初始狀態(tài)進(jìn)行聯(lián)合估計(jì)的初始化算法。通過分析估計(jì)函數(shù)的費(fèi)歇爾信息矩陣來評估估計(jì)狀態(tài)的置信度，從而確定初始化算法的終止條件。因此和Vins-Mono 相比，在收斂速度、魯棒性和精確度方面都具有更優(yōu)的結(jié)果。