視覺慣導SLAM 初始化算法研究

劉 剛，葛洪偉+

1.江蘇省模式識別與計算智能工程實驗室（江南大學），江蘇 無錫 214122

2.江南大學 人工智能與計算機學院，江蘇 無錫 214122

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

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

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

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

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

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

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

2 慣性測量單元模型

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

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

式中，p∈R3表示IMU的位移，四元數q∈? 表示IMU相對于世界坐標系的旋轉，表示IMU 的運動速度，bg∈R3表示IMU 的陀螺儀偏置，ba∈R3表示IMU 的加速度計偏置。

2.1 慣性傳感器測量模型

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

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

2.2 慣性傳感器運動學模型

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

2.3 預積分

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

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

2.3.1 標準狀態(tài)傳播

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

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

式（6）中Δt表示第i時刻點到第j時刻點之間的時間差，αij、βij、γij表示i時刻點到j時刻點的積分項（7）?？梢岳媒馕龇匠掏茖С霾煌鹊臄抵涤嬎惴椒?，例如歐拉法、中值法和龍格-庫塔法等方法進行積分。

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

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

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

其中：

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

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

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

系統(tǒng)的實際狀態(tài)是由標準狀態(tài)疊加上相應的誤差狀態(tài)，根據系統(tǒng)的標準狀態(tài)轉移方程（5）和誤差的定義（8）可推導出如下表達式：

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

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

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

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

3.1 慣性測量殘差模型

已知慣性測量單元的運動狀態(tài)Xj及在時間點i與j之間的測量集合為ui:j?？梢酝ㄟ^標準狀態(tài)轉移方程（5）、誤差的定義（8）和誤差狀態(tài)的傳播模型（9），在忽略IMU 軸偏差和尺度誤差的情形下，將標準狀態(tài)和實際狀態(tài)之間的誤差近似為高斯白噪聲，表示如下：

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

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

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

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

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

3.3 視覺慣性對齊

假設相鄰兩個關鍵幀的純視覺評估狀態(tài)分別為Xi、Xi+1，將視覺評估結果式（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)是線性方程可以直接求解相關狀態(tài)變量，然而式（18）沒有考慮到||g||=9.8 的約束，很容易得到是病態(tài)的收斂結果。因此必須加上重力加速度的約束，可以得到如下優(yōu)化方程。

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

其中，回縮因子如下：

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

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

4 效果評估

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

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

圖3～圖6 為本文所提視覺慣導初始化算法與Vins-Mono 初始化算法在Euroc Machine Hall 03 數據集上測試對比結果?？梢钥闯?，陀螺儀偏置在約2～4 s收斂，其x、y、z3 個方向上最終收斂于[-0.003,0.125,0.058]，單位rad/s，陀螺儀偏置比加速度偏置收斂快。這是由于陀螺儀偏置相較于加速度偏置不需要區(qū)分重力加速度，而加速度偏置因為有重力加速度的影響，所以相較于陀螺儀偏置收斂較慢。這也是引入最差情形評估函數時，將重力加速度和尺度相關費歇爾信息作為最差情形評估參數的原因。實驗中根據慣性傳感器特性，將加速度偏置初始值設為0，將其值域設置在[-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 最差情形評估參數

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

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

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

5 結束語

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