基于單目/IMU/里程計融合的SLAM算法

張福斌， 張炳爍， 楊玉帥

(1.西北工業大學 航海學院, 陜西 西安 710129; 2.天津航海儀器研究所, 天津 300130)

0 引言

輪式機器人在室內反恐防爆、探測、搜救等領域發揮著重要作用，而實現高精度的導航定位是輪式機器人執行各項任務的重要基礎。近年來，基于視覺和慣性測量元件(IMU)融合的同時定位與地圖構建(SLAM)[1]技術受到了廣泛關注。IMU和視覺傳感器之間具有良好的互補性，視覺SLAM定位精度高且漂移量小，但受工作環境影響較大，如在快速運動情況下估計效果較差；IMU穩定性高且能夠對短時間的快速運動提供良好估計，但存在明顯的累積誤差[2-3]。因此，視覺與IMU組合的視覺慣性里程計(VIO)，可以很好地解決系統的穩定性和精度問題[4]。

相比于雙目相機和深度相機，單目相機具有操作簡單、計算方便、使用范圍廣等優點[5]，更能滿足輪式機器人導航定位的實時性和多場景適用性要求。香港科技大學提出的單目視覺- 慣性導航系統(VINS-Mono)[6-7]是目前性能較為優秀的VIO系統，具有良好的魯棒性與較高的精度。該系統實現了單目視覺和慣性測量信息的緊耦合處理，并具有完整的回環檢測和重定位功能。但在輪式機器人上應用時，由于載體主要進行平面運動，且經常處于勻速或勻加速運動狀態，系統會產生額外的不可觀測度，從而導致定位精度下降[8-9]。

為進一步提高單目視覺- 慣性SLAM系統的定位精度，國內外研究人員進行了大量的研究工作，部分學者對前端光流跟蹤部分進行了改進[10]。其中，Hwangbo等[11-12]將陀螺儀信息用于視覺點特征跟蹤，有效提高了金字塔LK光流法對相機快速旋轉運動的魯棒性。Zhang等[13]基于VINS-Mono框架，提出了一種基于點- 線特征跟蹤的單目VIO算法，提高了在地下車庫等場景中的定位精度。本文通過引入陀螺儀和里程計導航信息，進一步優化了光流追蹤求解效率。

多傳感器融合能夠充分實現各傳感器間優勢互補，提高位姿估計的精度，同樣受到了廣泛關注[14-16]。部分研究人員提出基于EKF的單目視覺與輪式里程計融合位姿估計模型[17-18]，可以方便地融合多種傳感器信息，適用于輪式機器人應用場景。但基于松耦合的算法難以充分利用各傳感器信息，精度有待進一步提高。Quan等[19]提出了一種基于概率的緊耦合單目/IMU/輪式里程計SLAM算法，利用陀螺儀的角速度和里程計的線速度信息計算預積分，加入視覺和平面約束因子進行非線性優化，同樣提高了定位精度。但該算法拋棄了加速度量測，因此只能適用于水平面的運動。

結合輪式機器人特點，本文改進了單目視覺- 慣性SLAM系統，提出一種單目/IMU/里程計緊耦合SLAM算法。在視覺前端部分，對原始圖像金字塔光流法進行改進。通過MEMS IMU/里程計導航算法，將陀螺儀數據得到的幀間角增量信息和里程計數據得到的位移增量信息作為先驗，改進光流初值選擇，并優化像素偏導數的計算方法，提高特征匹配的速度和準確率。針對預積分環節，推導IMU/里程計預積分公式和誤差傳遞函數，將輪式機器人的里程計測量信息加入單目視覺/慣性SLAM系統。初始化階段中，利用預積分結果改進對速度、重力矢量、尺度因子的初始化過程。在后端非線性優化部分，推導了單目/IMU/里程計緊耦合SLAM系統的目標優化函數和優化狀態變量，構建了新的模型。最后，進行公開數據集仿真測試并設計實驗，驗證本文所提算法的有效性。

1 單目/IMU/里程計緊耦合SLAM算法設計

1.1 算法框架

本文針對輪式機器人導航定位問題，基于VINS-Mono算法框架，引入車輪里程計信息，提出了單目/IMU/里程計緊耦合的SLAM算法。算法的工作流程如圖1所示。

圖1 單目/IMU/里程計緊耦合算法框架

該系統中，對于新輸入的IMU、里程計測量值，計算IMU/里程計預積分得到幀間姿態、速度、位置增量；對于新獲得的圖像幀，進行Shi-Tomasi角點特征提取，并用改進金字塔LK光流法進行追蹤。如果系統還未初始化，則執行初始化操作，完成視覺、IMU、里程計對齊。內容包括恢復尺度，估計陀螺儀零偏和重力向量，確定世界坐標系，并且得到姿態、速度及位置的初值。最后進行基于滑動窗口和邊緣化的非線性優化，對滑窗中的各個變量進行實時優化。同時，進行回環檢測工作，如果檢測到正確回環，則執行重定位和四自由度位姿圖優化，保證全局軌跡一致性。

1.2 基于陀螺儀和里程計改進的圖像金字塔光流法

VINS-Mono系統前端采用圖像金字塔LK光流法追蹤幀間運動，該方法精度較高但效率有待提高。本文SLAM系統在視覺前端部分將陀螺儀和里程計的旋轉和平移增量作為先驗信息，對圖像金字塔光流法迭代過程進行改進，來提高光流追蹤速度。

1.2.1 低精度陀螺儀- 里程計導航算法

輪式機器人中通常使用的慣性器件為陀螺零偏及重復性精度低于0.1°/s量級的MEMS IMU，此時進行導航解算無需考慮與地球相關參數。因此，本節將推導忽略地球參數的簡潔慣性導航算法。

基于四元數的姿態更新算法算法如下

(1)

式中：

(2)

(3)

(4)

當載體速度不超過100 m/s時，可以不考慮地球自轉帶來的速度影響，此時位置更新微分方程將簡化為

(5)

離散化后，得到

(6)

以上旋轉和平移增量可用于改進本文SLAM系統前端所用圖像金字塔光流法的光流初值選擇。

1.2.2 改進的圖像金字塔光流法

由于特有的基于圖像金字塔分層的迭代操作，基于圖像金字塔的LK光流法相比于傳統的LK光流法在追蹤精度和魯棒性上性能有所提升，但追蹤速度卻略有不足。為盡量彌補追蹤速度上的缺陷及進一步提升追蹤精度，在原始的圖像金字塔光流法基礎上引入陀螺儀和里程計數據分別作為旋轉和平移先驗信息。

傳統的圖像金字塔LK光流法中，首先找到第I幀圖像中像素點u=(ux,uy)和第J幀圖像中的匹配像素點v=(vx,vy)，然后對第I幀圖像和第J幀圖像建立m層圖像金字塔：{IL,JL},L=0,…,Lm。隨后設定圖像金字塔光流初值：

(7)

在本文的SLAM系統中，由于存在陀螺儀和里程計，且幀間間隔較短，累積漂移小，可以獲得兩幀圖像間旋轉和平移的先驗值。設圖像I與圖像J中的匹配像素點對u和v的歸一化相機坐標分別為p1和p2，則根據對極幾何約束關系，有

p2=Rp1+t

(8)

式中：R表示陀螺儀的旋轉先驗信息；t表示里程計的平移先驗信息，根據(2)式、(6)式以及相機-IMU外部參數求得。由于相機內參矩陣K已知，根據p2可以求得像素點v的先驗像素坐標，從而優化基于圖像金字塔的LK光流法的光流初值選擇。并且在初值有先驗值的情況下，可以適當減少圖像金字塔層數，從而達到減小耗時且提高匹配精度的目的。

在計算圖像光流的過程中，首先找到第I幀圖像中像素點u=(ux,uy)和第J幀圖像中的匹配像素點v=(vx,vy)，然后對第I幀圖像和第J幀圖像建立m層圖像金字塔。則第J幀圖像中的匹配像素點v的預測值的計算過程為

(9)

改進后圖像金字塔的光流初值為

(10)

傳統的圖像金字塔LK光流法中，圖像IL對像素點u在x軸方向和Y軸方向的偏導數Ix和Iy的計算分別采用了該像素點左右和上下像素點的差分，而這種近似在實際的計算中效果并不如意。因此，在本文改進的光流法中，采用(11)式所示的兩個3×3的卷積核：

(11)

(12)

通過以上改進，可以有效提高SLAM系統前端光流追蹤的速度和精度。

1.3 IMU/里程計預積分

由于IMU通常具有比攝像機更高的量測速率，為避免在優化過程中重復積分，節省計算資源，視覺慣性SLAM系統中通常采用IMU預積分[20]方式來處理高速率IMU測量值。本文在IMU預積分理論的基礎上，引入里程計數據，推導IMU/里程計預積分的基本原理與公式。

(13)

(14)

在此基礎上，推導IMU/里程計預積分在連續形式下的誤差傳遞方程，t時刻誤差項的導數為

(15)

式中：na、nω和nv分別為加速度計、陀螺儀和里程計的加性噪聲；nba和nbω為加速度計和陀螺儀偏置的導數，均為白噪聲。根據以上公式進一步得到誤差協方差傳遞矩陣，用于后端非線性優化。

1.4 系統初始化

視覺/慣性SLAM系統初始化包括純視覺SFM和視覺/慣性對準，能夠為系統提供初始化估計。經過視覺/慣性對準，可以實現對陀螺儀偏差的估計，并恢復速度、重力矢量和尺度因子信息。VINS-Mono系統在車輛勻速運動或機動較小的情況下，容易出現初始化誤差過大的問題，通過加入輪式里程計信息，可以有效改善初始化過程穩定性，提高SLAM系統在勻速運動場景下的適用性。

首先，通過視覺獲得的幀間旋轉值和IMU預積分的旋轉值，計算得到陀螺儀誤差。該過程通常只需要幾秒鐘時間，在此期間陀螺儀零偏變化極小，可認為是一個常值。陀螺儀零偏估計的目標函數為

(16)

隨后，根據(14)式中IMU/里程計預積分結果，對速度、重力矢量、尺度因子和相關外參進行估計：

(17)

(18)

由于重力矢量的模值已知，在空間中只具有兩個自由度。可以將重力矢量重新進行分解，代入(17)式中進行計算，求得精細化分解的重力矢量，并與世界坐標系w坐標系的z軸對齊。據此可以將初始化的速度、位置及姿態等狀態量均旋轉至w坐標系中，完成系統初始化。

1.5 非線性優化

(19)

單目/IMU/里程計緊耦合SLAM系統非線性優化的目標函數為：

(20)

(21)

IMU和里程計測量殘差部分所影響的優化變量為bk幀和bk+1幀對應的位置、速度、姿態、加速度計零偏、陀螺儀零偏和IMU-里程計對應的外參，這些優化量可表示為

(22)

采用擾動的方式對以上變量計算雅可比矩陣，并利用Ceres庫進行求解，實現對滑窗中各個變量的實時非線性優化，得出位姿估計的最優結果。如果檢測到回環，則執行重定位和四自由度位姿圖優化，對位姿進行進一步優化，減小累積誤差影響。

2 實驗結果與分析

2.1 基于陀螺儀和里程計改進的圖像金字塔光流法實驗驗證

為驗證本文提出的基于陀螺儀和里程計改進的圖像金字塔光流法的可靠性，以Euroc開源數據集作為實驗數據，進行仿真實驗。Euroc數據集中包含圖像數據和陀螺儀數據，根據數據集中標準軌跡的幀間位姿增量，加入方差(0.013 m/s)2的高斯白噪聲，得到模擬的里程計數據。然后從數據集中選出11幀連續關鍵幀對應的圖片，用改進的圖像金字塔光流法和原始的圖像金字塔光流法對這些圖片進行追蹤，分別記錄下這兩種算法在追蹤過程中的角點數量、迭代次數和像素匹配誤差。表1為兩算法在剔除誤匹配后追蹤到的角點數量對比，表2為兩算法在計算光流過程中的迭代次數對比，表3為兩算法最終的像素匹配誤差對比。

表1 角點數量對比

表2 迭代次數對比

表3 匹配誤差對比

從表1、表2和表3中可以看出，在兩種算法追蹤匹配角點數量相當的情況下，基于陀螺儀和里程計改進的圖像金字塔光流法的迭代次數均值要明顯少于原始的圖像金字塔光流法，減少了32.5%，證明了改進光流法的有效性。而像素匹配誤差均值只是略小于原始的圖像光流法，這是因為改進的圖像金字塔光流法中先驗信息起到了補償作用，使得算法的光流匹配速度有所提升，匹配誤差有輕微下降。

2.2 單目/IMU/里程計緊耦合SLAM算法實驗驗證

為驗證本文提出的驗證本文所提的單目/IMU/里程計緊耦合的SLAM算法有效性，搭建實驗平臺，進行室內和室外實驗。

采用改裝的Autolabor Pro輪式遙控車作為實驗平臺，搭載的相機為OV2710單目相機，相機圖像采集頻率為20 Hz，圖像分辨率為640×480。同時，在車上安裝MEMS IMU傳感器，型號為法國SBG公司的Ellipse2-A，采樣頻率100 Hz。輪式編碼器為增量型歐姆龍E6B2-CWZ3E，采樣頻率為100 Hz。主控為一臺mini工控機，處理器為Intel i7 4560U 4核，運行內存為8 GB，運行系統為Ubuntu16.04。室外實驗使用差分GPS設備來提供基準軌跡，其定位精度可達厘米級。

室內開環實驗在實驗室走廊內進行，遙控小車沿地板上預定的軌跡行駛，從起點運動到終點。運動軌跡包括2段直行和1個轉彎的過程，大致為“L”形狀，總長度大概為25 m。本文算法實時運行后在RVIZ工具下得到的軌跡圖2所示。

圖2 室內實驗運行軌跡圖

該實驗中，將本文單目/IMU/里程計緊耦合SLAM算法和VINS-Mono算法進行對比，通過判斷實驗軌跡和預定軌跡的終點誤差來評判各定位算法的性能優劣。

從表4和表5中可以觀察到，在室內進行的10次開環實驗中，本文算法(單目/IMU/里程計緊耦合SLAM算法)的終點誤差為0.168～0.290 m，占總里程的0.67%～1.16%。VINS-Mono算法的終點誤差為0.212～0.308 m，占總里程的0.85%～1.23%。對比得出，本文算法相比VINS-Mono算法的終點定位精度略有提高。室內開環實驗在一定程度上驗證了本文算法的有效性，但實時基準軌跡的缺失使得室內實驗不能全面地體現出算法的性能，在此基礎上，進一步應用差分GPS設備和Autolabor Pro小車進行了室外場景實驗。

表4 室內開環實驗終點間距離誤差

表5 室內開環實驗距離偏差占總里程的百分比

室外環境相比于室內更為復雜，存在各種動態或未知的干擾。為進一步驗證本文單目/IMU/里程計緊耦合算法的有效性，進行了室外場景下的開環和閉環實驗驗證。控制Autolabor Pro輪式小車在校園內道路上運動，將速度控制在1 m/s左右。開環實驗軌跡總長度為100 m，包含兩個90°轉彎。閉環實驗運動軌跡包括4段直行和4個轉彎的過程，大致為一個閉合的矩形形狀，總長度大概為600 m。路線分別如圖3和圖4中差分GPS軌跡所示。

圖3 室外開環實驗軌跡對比

圖4 室外閉環實驗軌跡對比

將實驗軌跡(包括本文的單目/IMU/里程計緊耦合算法和VINS-Mono算法)與差分GPS提供的軌跡通過Evo工具進行對齊，然后將實驗軌跡與差分GPS軌跡進行對比得到誤差均值、誤差最大值和均方根誤差(RMSE)，并對其誤差進行分析。

實驗結果如圖3、圖4和圖5所示，圖3和圖4為實驗軌跡(本文的單目/IMU/里程計緊耦合算法和VINS-Mono算法)與差分GPS軌跡的對比圖，圖5為閉環實驗中，實驗運行軌跡與差分GPS軌跡之間距離誤差的對比圖。詳細的定位誤差對比如表6所示。

圖5 室外閉環實驗距離誤差對比

表6 室外實驗定位誤差對比

對比以上實驗結果可以得出，由于本文SLAM算法加入了里程計信息作為緊耦合約束，在室外長距離運動下，定位誤差均值相比VINS-Mono算法減少近40%，導航定位精度相比VINS-Mono有明顯提高。

3 結論

本文針對室內輪式機器人高精度導航定位的需求，在VINS-Mono算法框架的基礎上，引入里程計數據，提出了一種基于改進光流法的單目/IMU/里程計緊耦合SLAM算法。首先，在前端圖像金字塔LK光流法基礎上，加入陀螺儀和里程計信息對算法進行改進。通過將陀螺儀和里程計得到的幀間位置和姿態增量作為先驗信息，改進圖像金字塔光流法的光流初值，并且減少了一層圖像分層，從而減少光流追蹤計算量，減小泰勒展開的誤差以提高匹配精度。在Euroc數據集下進行了仿真測試，驗證了改進光流法對于加快光流匹配速度的有效性。推導了IMU/里程計預積分公式，并對初始化、非線性優化等過程進行改進，加入了里程計信息約束，進一步提高位姿估計精度。進行小車實驗，驗證了本文SLAM算法的有效性，該算法定位精度相比VINS-Mono具有明顯提高。