基于雙重初始化和分級優化的改進視覺慣性SLAM方法

凌有鑄，郭俊陽，陳孟元，陳何寶，袁學超

（1. 安徽工程大學 電氣工程學院，蕪湖 241000； 2. 高端裝備先進感知與智能控制教育部重點實驗室，蕪湖 241000； 3. 蕪湖市固高自動化技術有限公司，蕪湖 241000）

移動機器人基于視覺的同步定位與建圖（Visual simultaneous localization and mapping, VSLAM）主要研究機器人如何在復雜多變的環境中準確地確定位置并建立當前場景地圖的問題。然而傳統VSLAM 算法僅依靠單一傳感器已無法滿足科研的需求，也無法在復雜快速運動的環境中及時捕捉機器人的運動信息并反饋給主控機進行運動補償。科研工作者開始融合兩種不同傳感器的信息意圖互補兩者的不足，期望慣性測量單元（Inertial Measurement Unit, IMU）可以在快速運動時替代相機捕捉相機運動的物理信息提高VSLAM 定位精度，而相機也可以彌補IMU 在慢速運動下因使用時間過長使得累計誤差漂移的問題。

文獻[1]建立IMU 位置和速度積分模型，利用三個連續圖像幀和一個特征點匹配的視覺信息求解理想IMU 模型閉式解，但測量值未考慮噪聲對IMU 測量值的影響。文獻[2]提出利用歐拉角參數化旋轉誤差，避免重復積分。文獻[3]在[2]的基礎上對旋轉角進行流形參數化和數據處理，提取初值數據避免重復更新，減少積分帶來的誤差累計，但該方法未考慮IMU 隨機游走誤差的影響。2017 年，國內沈劭劼團隊[4]在IMU初始化階段將特征點深度信息、相機平移與重力、速度一起估計，該方法計算量大且精準度低。同年，MUR-ARTAL R 提出VI-ORB（Visual Inertial Oriented FAST and Rotated BRIEF）算法，利用松耦合的方法在原有基礎上估計零偏，但沒有考慮外補參數的影響[5]。

IMU 預積分的提出極大地促進了基于圖優化的視覺慣性聯合導航系統。Klein G 等人[6]提出的PTAM算法能夠同時對某個時間段內的相關狀態進行位姿和誤差優化，具有較高的計算精度，然而具有極高的局限性與依賴性。這類方法只有在初始狀態已知的情況下才能使用，計算結果的準確度也與初始狀態的數據準確度緊緊相關[7,8]。文獻[9]在VSLAM 階段以相同計算資源為條件對比實驗，得出優化算法比濾波算法更能得出準確的效果。文獻[10]提出的OKVIS 是一種緊耦合的雙目慣性里程計，將視覺和IMU 的誤差項和狀態量在一起優化，但這種算法不支持重定位，也不包含閉環檢測功能。

2015 年Leutenegger 等人[11]提出了一種基于關鍵幀的算法，在局部優化中優化相機位姿和IMU 誤差，跟蹤和重建圖像中興趣點（interest- point）的3D 位置。2016 年Usenko 等人[12]提出了一種類似于非線性優化的方法，通過在局部優化中使用直接圖像對齊而不是間接使用興趣點測量值來得到全局的連續地圖。RaulMur-Artal[13]在ORB-SLAM 的追蹤前段添加IMU模塊，并將視覺慣性聯合初始化，優化位姿信息，減少誤差多次使用導致的數據漂移，但其無法在相機劇烈運動下正常使用。2017 年Forster 等人[14]提出了一種基于預積分理論的視覺慣性里程計，將預積分理論應用于SO(3)模型中，將IMU 測量數據與圖像中稀疏子集直接跟蹤相結合，從而加速了優化計算。文獻[15]以OKVIS 為主要框架，添加基于視覺慣性融合的前端，實現視覺和慣性數據緊耦合，但未計算出尺度變化和旋轉角偏差，也沒有考慮到IMU 自身攜帶的誤差，定位精度和魯棒性較差。Forster[16]開發一種預積分理論，恰當地解決旋轉群的多種結構。同理，單目視覺慣性狀態估計器（Monocular Visual-inertial State Estimator, VINS-mono）通過增加IMU 偏置校正在連續時間擴展IMU 預積分[17]。

本文以VINS-mono 算法為基礎，提出一種基于雙重初始化和關鍵幀分級優化的改進視覺慣性SLAM方法（Double Initiatlation and Covisibility Graph-VINS，DICG-VINS）。針對IMU 預積分帶來的誤差累計的問題，在視覺慣導初始化階段結合費雪矩陣構建情形評估函數對初始化信息矩陣進行度量，使得重力信息和尺度信息具有可觀性，更加準確地表征局部地圖；在滑動窗口優化前，對關鍵幀按照共視關系進行分級處理，聯合重投影誤差和IMU 預積分誤差構建誤差代價函數，優化分級得到的強共視關鍵幀，提高運動精度的同時減少軌跡誤差。

1 系統框架

本文設計的系統框架如圖1 所示。

圖1 系統框架Fig.1 System framework diagram

2 視覺慣性初始化

2.1 IMU 誤差分析

IMU 通常測量載體的角速度wm和加速度am的離散值，測量值受到傳感器隨時間緩慢變化的偏置和測量白噪聲的影響，故測量值可以由真實值、隨機游走偏差和高斯白噪聲三個部分組成[18]，為：

其中，wm和am分別表示慣性測量單元IMU 中陀螺儀和加速度計的測量值；wB和wW分別表示陀螺儀和加速度計的真實值；bg和ba分別表示陀螺儀和加速度計的隨機游走偏差；表示世界坐標系（World）到IMU 坐標系（Body）的旋轉矩陣；ηg和ηa分別表示陀螺儀和加速度計的高斯白噪聲，gW表示世界坐標系下的重力加速度矢量，本文選擇東北天坐標系作為世界坐標，gW=[0,0,g]T。IMU 坐標系變換示意圖如圖2 所示。

圖2 IMU 坐標系轉換Fig.2 IMU coordinate system transformation

定義IMU 運動狀態如下：

其中ZF為IMU 狀態變量。F為圖像幀，W為世界坐標系，p、v和q分別表示IMU 位置、速度和姿態四元數。

2.2 IMU 初始化終止條件

單一的視覺里程計無法獲得絕對尺度，聯合慣性數據能夠使得整個視覺系統的尺度與重力方向可觀測[19]。VINS-mono 在視覺慣性系統運行的前15 s 進行聯合初始化，以確保所有參數可觀，但固定初始化時長的方法存在不確定性，導致初始化時間延長。而IMU 無法直接從靜止狀態開始運動，需要有足夠的運動激勵[20]。因此需要保證IMU 和相機的觀測值是機器人在充分運動時測量得到，觀測值的好壞充分影響著系統的魯棒性程度[21]。

則狀態變量費雪信息矩陣可以表示為

將視覺慣導觀測樣本情形評估函數定義如下：

其中，Ωk(F)是從關于評估狀態ZF的尺度和重力加速度等相關狀態獲得的信息矩陣。F為運行過程中某個圖像幀，Δτ是與圖像幀相關的圖像信息矩陣。

為了最小化置信度的局域半徑，需要最小化協方差的行列式，由于得到的最小化置信度局域半徑與最大化信息矩陣的對數行列式相同，從而得到觀測樣本情形評估函數的性能度量：

定義情形評估函數值小于閾值Sth來衡量初始化狀態具有較高的置信度，認為此時的ZF具有高確定性，終止初始化，將相應的預積分和初始化數據融合，進入下一步的視覺慣性實時優化。為避免因情形評估函數計算時間過長使得IMU 誤差累積，故在初始化過程中額外增加一個15 s 時間設定，若在15 s 內未達到較高的置信度，在第15 s 時直接終止初始化，將當前初始化結果送入下一階段實時優化。情形評估函數算法如算法1 所示。

算法1：Double initialization of case evaluation function Input：關鍵幀狀態變量 ZF 聯合初始化時間t最差情形評估閾值 Sth Output：聯合初始化后的狀態變量ZFbest 視覺慣性聯合初始化 for 聯合初始化時間 t＜ 15s 計算樣本情形評估函數 fλ(F )隨初始化時間t 的變化 iffλ( F)＜Sth聯合初始化終止 end if 聯合初始化 end for 聯合初始化終止 計算聯合初始化后的狀態變量ZFbest

3 基于共視約束的視覺慣性緊耦合優化

基于共視關鍵幀優化的視覺慣性緊耦合示意圖如圖2 所示，其中相機數據與IMU 預積分統一在一個非線性優化框架中進行優化。在關鍵幀進入滑動窗口優化之前，對關鍵幀進行分級處理，按照幀間公共觀測地圖點稀疏程度將關鍵幀分為強共視關鍵幀和弱共視關鍵幀兩類，后續滑動窗口優化只優化強共視關鍵幀。

圖2 基于共視關鍵幀優化的視覺慣性緊耦合示意圖Fig.2 Schematic diagram of tightly coupled visual inertia based on covisibility keyframe optimization

3.1 分級關鍵幀篩選

視覺SLAM后端一般都是利用滑動窗口最小化重投影誤差作為約束項去優化關鍵幀，但每次進入滑動窗口的關鍵幀數量固定，處理信息的實時性低，移動舊關鍵幀的時候也會對關鍵幀攜帶的信息有不利影響。文獻[22]在后端增加雙步邊緣化處理，對高維度矩陣分布拆解后進行優化求解以保證計算稀疏化，計算效率比傳統的邊緣化快，但仍存在計算速度慢、實時性低的缺點。Mur-Aetal R[23]在處理后端優化時利用關鍵幀之間所有的共視關系作為滑動窗口優化的約束項，充分發揮先驗信息的作用，但該方法容易造成滑動窗口關鍵幀信息冗余，處理效率低下。

在關鍵幀進入滑動窗口優化之前，本文借助Covisibility Graph 的概念構建基于共視約束的關鍵幀局部優化概率圖模型，按共視強弱對關鍵幀進行分級處理，保留強共視關鍵幀，忽略弱共視關鍵幀，將幀間觀測信息及關鍵幀上地圖點等信息作為約束項優化強共視關鍵幀。此時共視約束概率圖模型可以看成一個無向加權圖，其聯合概率分布即可分解為多個團Q勢能函數（因子）的乘積：

該模型僅利用強共視關鍵幀構建局部場景地圖，提高觀測信息的準確性，減少了滑動窗口的優化時間。共視關鍵幀局部優化圖模型如圖3 所示。左側圖中關鍵幀之間含有公共觀測地圖點，為了解關鍵幀間信息變化，將幀間關系簡單化，得到右側的位姿圖，虛線表示關鍵幀之間的共視關系。設定含150 個及以上共同觀測點的關鍵幀為強共視關鍵幀，少于150 個共同觀測點的關鍵幀為弱共視關鍵幀。

圖3 共視關鍵幀局部優化圖模型Fig.3 Covisibility keyframe local optimization graph model

在滑動窗口優化之前先進行共視關鍵幀篩選可以將進入滑動窗口關鍵幀數量固定在一定的范圍內，減少優化的計算量；忽略弱共視關鍵幀可以消除因關鍵幀數量過多導致位姿信息冗余；保留弱共視關鍵幀的地圖點及相關位姿信息并作為約束項優化強共視關鍵幀，提高局部地圖的精準性。

3.2 滑動窗口緊耦合優化

在滑動窗口優化時，僅優化強共視關鍵幀，同時保留弱共視關鍵幀的位姿信息作為約束項。視覺位姿估計誤差項、IMU 預積分誤差項分量構建一個目標函數中，最小化重投影誤差和IMU 誤差優化狀態量[24]：

Eproj( k , k+ 1)為第k 和k+1 個關鍵幀之間的重投影誤差；Eimu( i , j )為時間i 到j 之間的IMU 誤差項，由視覺位姿估計和IMU 預積分測量組成，如式(9)所示：

IMU 測量誤差包括傳統的位姿誤差 ep、 ev和 eq及偏差eb，各誤差分量如式(10)所示：

其中∑η為測量噪聲的信息矩陣，∑i為預積分信息矩陣，是在IMU 預積分階段計算殘差得出的雅克比矩陣，ρ（?）為魯棒核函數。

利用g2o（general graph optimization）方法構建圖優化模型處理后端優化的問題，借助列文伯格-馬夸爾特法（Levenberg-Marquardt method，L-M）求解誤差 狀態增量的最小值使得目標函數最小化。在滑動窗口算法中實現狀態更新，便于后續SLAM 系統的運動跟蹤和位姿估計。

4 實驗結果與分析

4.1 視覺慣性聯合初始化參數收斂性分析

本文算法（DICG-VINS）在VINS 初始化階段基礎上借助費雪矩陣構建觀測樣本情形評估函數，評估視覺慣性聯合初始化誤差變化，使用Ceres Solver 軟件來實現牛頓高斯迭代法在黎曼流形面上的迭代搜索，將加速度計偏差和陀螺儀偏差快速穩定在基準值附近，達到快速收斂的目的。對比DICG-VINS 與VINS算法的初始化算法在MH_04 數據集的偏差變化，多次實驗取平均值測得的陀螺儀和加速度計偏置數據繪制散點圖進行分析，如圖4 和圖5 所示。

圖4 陀螺儀偏置收斂過程示意圖Fig.4 Schematic diagram of the convergence process by the gyroscope bias

陀螺儀偏置收斂過程示意圖如圖4 所示，VINS和本文算法（DICG-VINS）初始化算法都能實現偏差的快速收斂，但VINS 算法未對偏差進行實時優化處理，而DICG-VINS 算法構建了情形評估函數實時評估當前偏差的狀態，因此精度和收斂性都在一定程度高于VINS-mono 算法。加速度偏置收斂過程示意圖如圖5 所示，DICG-VINS 加速度收斂過程整體上與陀螺儀偏置收斂過程類似，最終都趨向定值，僅個別估計值存在誤差，但收斂速度較VINS-mono 更快更準確。

圖5 加速度偏置收斂過程Fig.5 Schematic diagram of the convergence process by the accelerometer bias

本文設定情形評估函數可以滿足待估計參數收斂的要求，能夠使得IMU 達到快速收斂的目的，在一定程度上有效的抑制了IMU 偏差的增大。與VINS-mono在初始化階段以時間為終止條件的情況相比，有效地節約了初始化時間，減少了IMU 測量值誤差，提高了后續階段的優化精度。

4.2 滑動窗口優化方法性能分析

本文在后端使用了共視約束的滑動窗口優化方法對局部軌跡進行校正。為驗證方法改進后的效果，在MH_01_easy、MH_03_medium、MH_05_difficult 三個數據集上對DICG-VINS 和VINS-mono 的滑動窗口優化方法進行測試。為排除其他因素的干擾，在軌跡評估時使用擬合的尺度，每組實驗測試10 次。統計其軌跡誤差，對比結果見圖6。其中不難看出，對于MH_01數據集和MH_03 數據集來說，經過滑動窗口優化以后，位置漂移最大值在± 100 mm 左右，比傳統的VINS算法的位置誤差減少了34%。對于復雜數據集（如MH_05_difficult）位置誤差在± 200 mm 以內。

圖6 DICG-VINS 算法位置漂移誤差Fig.6 Position drift error of DICG-VINS algorithm

在圖 6(a)中 y 軸的漂移誤差過大是因為MH_01_easy 數據集是在工廠環境下拍攝，工廠環境情況復雜且障礙物多，在一定程度上阻礙了無人機的飛行；在圖6(b)中y 軸在0-40 m 的時候波動性較強，此時IMU 還處于初始化階段，加速度計由于自身噪聲過大，無法完成初始化任務；在圖6(c)中z 軸和y 軸位置漂移在50 m 的時候達到谷底，是因為此時無人機前方有障礙物，無人機急速下降導致DICG-VINS 算法無法及時跟隨，出現短暫的誤差過大情形。

EuRoC 測試中運行時間的比較如表1 所示，在測試中，DICG-VINS 相較于VINS-Mono 在運行速度上平均提升11.19%。其中，在MH_01_easy、MH_02_easy和V2_01_easy 數據集上，DICG-VINS 比VINS-mono的運行時間加快了25 s 左右。在V1_02_medium 和V2_02_medium 數據集上，DICG-VINS 僅比VINS- mono 運行時間快了8 s。分級關鍵幀篩選后進入滑動窗口優化，節省滑動窗口優化的關鍵幀的時間，但也提高了算法的軌跡精度。

表1 測試中各算法運行時間比較Tab.1 Comparison of the running time in each algorithm

4.3 算法精度對比實驗分析

為了進一步驗證算法整體性效果，在EuRoC 數據集不同環境下共4 個序列（MH_01_easy、MH_04 _difficult、V1_01_easy、和V2_03_difficult）驗證DICG- VINS 的整體性能，并與當前主流的VIO 方法對比。DICG-VINS 在上述數據集上的精度與VI-ORB 接近，優于無閉環情況下的 VI-ORB 算法、無閉環的VINS-Mono 算法。本文算法軌跡與VINS-Mono（無閉環）、VI-ORB（無閉環）軌跡、ORB-SLAM2（無閉環）軌跡及真實軌跡在數據集上的比較結果如圖7所示。根據圖7(a)可看出系統在運行MH_01_easy 數據集時，軌跡結果誤差較小，且累計誤差得到了很好的消除，這是由于MH_01_easy 序列，數據采集時無人機速度緩慢，系統在處理關鍵幀的同時進行了全局優化操作。圖7(c)可看出系統運行V1_03_difficult 數據集時，軌跡結果誤差較大，這是因為該數據集在房間采集，環境與工廠環境不同，能夠采集的特征信息十分稀疏，不利于系統運行。

圖7 各算法運行軌跡比較（含真值）Fig.7 Comparison of the running trajectory of each algorithm (including true value)

EuRoC 數據集下VIO 算法軌跡精度（RMSE）對比如表2 所示。與VI-ORB、ORB-SLAM2 和VINS- mono 等算法相比，DICG-VINS 算法在EuRoC 大多數據集上的軌跡精度都與主流算法保持在同一水準或者有所提高，定位精度普遍有所提高。對比VINS-mono算法，DICG-VINS 在算法軌跡精度上平均提高29.47%，其中在MH_01_easy、MH_03_medium 和V1_03_difficult 上，DICG-VINS 取得了最佳性能，具有良好的表現。對比ORB-SLAM2 算法，本文算法還是略有不足，主要是因為前段光流法雖然提高了圖像提取速率，提取地圖點的質量與特征法相比明顯較差，但后端分級優化對關鍵幀信息進行約束優化，彌補了部分不足，甚至效果比ORB-SLAM2 更佳。

表2 VIO 算法軌跡精度（RMSE）對比（單位：m）Tab.2 VIO algorithm set Trajectory accuracy (RMSE) comparison (unit: m)

5 結 論

為提高VINS-mono 在初始化階段和局部優化階段的定位精度和系統魯棒性，本文提出了一種改進初始化和共視約束緊耦合優化的視覺慣性SLAM 方法。利用費雪矩陣構建最差情形誤差函數反推狀態矩陣的信息量期望值，加快初始化偏差收斂速度。在關鍵幀緊耦合優化階段，通過共視關系分級關鍵幀，優化強共視關鍵幀的同時保留弱共視關鍵幀的幀間信息作為滑動窗口的約束項，降低了運算難度，并提高了運算精度。實驗分析環節，采用EuRoC 數據集對DICG- VINS 與其他主流算法進行對比實驗分析，結果表明，DICG-VINS 與VINS-mono 相比在平均運行速度方面提高了11.19%，精度方面提高了29.47%；但對比ORB-SLAM2 算法，DICG-VINS 圖像提取精度低，后續關鍵幀優化效率提升不明顯。DICG-VINS 后續會進一步完善視覺慣性初始化終止條件的設定，在情形評估時能夠更快更準確地評估當前狀態的期望值，完成初始化的同時減少誤差的增加，構建更加完備的視覺慣性SLAM 系統。