視覺慣性融合定位的室內SLAM研究

劉大鵬

（浙江農林大學光機電工程學院，浙江 杭州 311300）

0 引言

隨著人工智能技術飛躍式的發展，室內移動機器人開始廣泛進入人們的日常生活中，智能家居、工業輔助生產、物流便捷式倉儲與服務型機器人等熱點內容引起人們的關注與研究。而VI-SLAM（Visual And Inertial Simultaneous Localization And Mapping，視覺慣性即時定位與地圖構建）[1-4]中定位技術在智能移動機器人執行任務過程中發揮著舉重若輕的作用，使得移動機器人在未知場景中，通過識別障礙物的相對位置和方向，防止本體與物體接觸碰撞；通過感知且估計本體所在空間位姿信息，保證移動機器人構建地圖與場景環境信息全局的一致性。因此，研究VI-SLAM定位技術有助于增強移動機器人在室內環境中的空間信息感知與定位的功能，并為移動機器人自主導航提供先驗信息。

目前，VI-SLAM根據視覺傳感器與IMU數據融合方式分為松耦合和緊耦合。其中，松耦合[5]是指相機和IMU分別進行各自的位姿估計，然后對它們各自的結果共同構建運動方程和觀測方程進行位姿估計。Lynen等[6]提出MSF-EKF框架，建立按時間順序的視覺測量位姿狀態緩存區，當IMU狀態數據獲取時插入到相應的時間戳的狀態緩存區，更新視覺慣性融合后的狀態，以提高系統定位的精確性與一致性。

與松耦合相比，視覺慣性系統采用IMU的狀態數據與相機的觀測數據聯合后進行位姿估計優化的緊耦合[7]關聯方式，系統更加健壯，且定位更精準。Leutenegger等[8]提出采用緊耦合的方式將視覺與IMU測量數據關聯，構建基于關鍵幀的全概率的視覺與慣性誤差項，并以關鍵幀的相對不確定性來替代全局不確定性，最后通過滑動窗口的形式對其誤差項進行位姿優化。

1 雙目視覺慣性系統框架

本文構建的VI-SLAM框架系統如圖1所示：在系統前端的環境探索中，采用雙目相機以特征法提取場景特征，并以改進光流法匹配跟蹤，IMU使用流形預積分感知移動機器人運動位姿；在初始化階段，移動機器人提供視覺慣性初始運動參數；采用基于關鍵幀的緊耦合方式對視覺慣性數據進行定位融合，并在后端采取滑動窗口對運動位姿進行非線性優化得到移動機器人的軌跡姿態。

圖1 視覺慣性系統框架圖

2 雙目視覺里程計

在前端特征檢測器選擇的過程中，為了平衡系統的實時性與定位精度。對此，提出雙目特征點法和改進光流結合的視覺前端里程計用于對場景特征的檢測與跟蹤。Fast角點是根據圖片像素點亮度差異提取環境局部特征，且在圖像特征點提取速度和數量上表現優異，故選擇其作為特征提取器[9]。為提升系統實時性和提高系統適應動態場景的魯棒性，在特征點匹配跟蹤時采取改進光流算法。

在圖像特征點提取與正向跟蹤匹配后，特征點跟蹤由于噪聲存在誤匹配，錯誤匹配將影響下一幀中目標位置的判斷。針對特征存在誤匹配，對于圖像幀采用逆向光流[10]跟蹤，如圖2所示，通過當前幀追蹤上一幀的角點，篩除圖像像素距離過大的外點，提高特征點匹配質量。

圖2 正向和逆向光流跟蹤圖

稀疏LK光流法在實際場景下，相機獲取的圖像特征點相對速度過快，導致像素運動偏移量過大，造成跟蹤失敗，因此，選取圖像金字塔來追蹤特征點偏移量d。

如圖3所示，圖像金字塔指對同一圖像進行尺寸縮放得到不同分辨率下的圖像，光流跟蹤時自頂層到原始圖像由粗到細依次優化得到最佳光流，在最優解處，此時前后相鄰幀圖像對應特征點相似度最高。為進一步提升結果的精確性，金字塔光流跟蹤時在金字塔的每一層采取多次迭代來求得精確的最優解，最終獲得領域內匹配誤差最小的光流。

圖3 金字塔光流法示意圖

相機位姿的運動估計，以左目圖像為基準，在經過左右立體特征提取、光流跟蹤和前后幀間特征正逆向光流追蹤后，采用三角化獲得特征點深度信息，進而解算出上一幀與當前幀的特征點的三維坐標。并且已知相應的圖像特征點二維坐標，采用PNP（Perspective-n-Point，透視N點投影）構建BA最小二乘法優化相機幀間位姿（R，T）。

3 視覺慣性數據初始化

在移動機器人視覺慣性數據融合位姿估計之前，需要對視覺傳感器與IMU數據進行對齊校準，為狀態變量的精準求解與系統快速收斂提供必要的初始化參數。且良好的初始化在系統丟失時能夠快速重啟進入工作狀態。

1）建立IMU預計分離散模型，根據視覺與慣性旋轉量一致性建立最小二乘法方程迭代優化陀螺儀偏置bg；

2）由位姿的平移轉換關系線性推算系統尺度S和重力g的近似值；

3）IMU坐標系與重力旋轉構建線性方程，獲取加速度計偏置ba，并對尺度和重力進一步計算精確數值；

4）利用IMU速度預計分模型求得速度v參數。

4 滑動窗口緊耦合的非線性優化

隨著移動機器人在各種場景的不斷探索，視覺慣性系統獲取的環境路標、特征點的觀測量和狀態估計量的規模劇增，系統的實時性與魯棒性受到約束，系統在衡量精度與計算效率后，采取基于關鍵幀的滑動窗口來控制系統狀態量的參數優化。

滑動窗口：在窗口內固定關鍵幀數量N=10，在加入一個新的關鍵幀以及觀測到的路標點時，對一個舊的關鍵幀進行“剔除”。基于滑動窗口狀態量緊耦合的非線性優化中，通過邊緣化把舊關鍵幀的狀態量從當前固定窗口的狀態估計中移除，但移除的狀態量作為先驗信息保留在當前滑動窗口中，避免窗口中其他幀和路標點信息的丟失。

為了達到視覺慣性系統的定位準確性和全局一致性，在滑動窗口中構造邊緣化殘差、IMU殘差和視覺誤差項來非線性優化移動機器人的狀態變量，如下所示：

邊緣化殘差為滑動窗口移除舊關鍵幀的位姿與特征時，將與其關聯的約束關系轉化為先驗（prior）參與系統優化；IMU殘差為相鄰幀間預積分觀測值與估計值的殘差；視覺誤差是場景特征點在相機下的重投影誤差，即特征點在當前幀中光流跟蹤的歸一化坐標與特征點在當前幀的重投影歸一化坐標的殘差[11-14]。

5 實驗測試與結果

對構建的視覺慣性SLAM算法（簡稱為Ours）采用公共EuRoc數據集進行測試評價，并與當前開源的OKVIS、VINS-Fusion對比，獲取實驗測試現象與結果，如表1所示。EuRoc數據集共五個序列（采用簡寫表示），MH_01至MH_05數據集背景為大型機器廠房。數據集含有豐富的信息，并設置紋理、光照和速度組合的對比場景。

1）定位精度的檢驗：由表1可知，在公開的EuRoc的五個數據集中，Ours與VINS-Fusion、OKVIS實驗測試中，性能最好的算法，并以加粗字體突出顯示。在MH_01良好的紋理、場景明亮和低速運動的狀況下，系統保持著優異的位姿估計精準性。與MH_01相比MH_03在快速運動時，移動機器人系統穩健性良好。在MH_04與MH_05中，移動機器人快速運動中，降低環境光線強度，數據表明VINS-Fusion定位性能最優，Ours其次，OKVIS表現最差。然而，Ours在五個場景數據的均方根誤差的平均值最小，可知Ours在各種變化場景中保持良好的定位準確性。

表1 雙目視覺慣性SLAM算法的均方根誤差

2）累計誤差的檢驗：移動機器人對場景的空間信息進行感知，在噪聲的干擾和位姿的錯誤恢復下，系統的軌跡定位在長時間的運行中產生累計誤差，對三種算法進行軌跡定量剖析。

隨著運動距離的增加，移動機器人視覺慣性的誤差逐漸累積，采用折線圖來比較絕對位姿中的平移偏量。如圖4所示，通過MH_01、MH_03和MH_05三種不同條件場景下，觀察RMSE的中位數，對比三種算法在長時間運行下系統累積平移誤差的大小，并觀測三種算法箱體波動程度的走勢圖。結果表明，Ours算法隨著距離的增加系統累計誤差在箱體上下波動幅值最小，VINS-Fusion中等，OKVIS波動最大，體現出Ours算法系統的穩定性。

圖4 OKVIS、VINS-Fusion和Ours絕對位姿的平移誤差對比圖

3）系統的穩健性測試：在教學樓實際環境下，手持與裝載Ubuntu18.04電腦連接的Realsense T265，行走路線從一樓至四樓，中途經走廊和帶有落地窗的樓梯平臺，測試期間在光照不足和弱紋理的走廊快速移動，上樓時設置相機抖動，轉彎時快速旋轉變換前進方向。

構建的視覺慣性SLAM系統數據解耦后生成的軌跡路線如圖5所示，結果表明，算法能夠在弱紋理、光線不足、快速旋轉和移動的挑戰場景中實現連貫的位姿追蹤，驗證了系統良好的穩健性。

圖5 教學樓軌跡路線圖

6 結束語

通過IMU數據與視覺圖像幀信息融合，在兼顧系統精度的前提下，通過構建改進LK光流法跟蹤Fast角點的輕量型雙目視覺里程計，并與IMU流形預計分數據緊耦合實現數據關聯，經基于關鍵幀的滑動窗口內非線性優化移動機器人位姿，實現移動機器人在室內定位的穩健性與魯棒性。