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

張福斌， 張炳爍， 楊玉帥

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

0 引言

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

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

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

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

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

1 單目/IMU/里程計緊耦合SLAM算法設(shè)計

1.1 算法框架

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

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

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

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

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

1.2.1 低精度陀螺儀- 里程計導(dǎo)航算法

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

基于四元數(shù)的姿態(tài)更新算法算法如下

(1)

式中：

(2)

(3)

(4)

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

(5)

離散化后，得到

(6)

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

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

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

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

(7)

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

p2=Rp1+t

(8)

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

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

(9)

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

(10)

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

(11)

(12)

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

1.3 IMU/里程計預(yù)積分

由于IMU通常具有比攝像機更高的量測速率，為避免在優(yōu)化過程中重復(fù)積分，節(jié)省計算資源，視覺慣性SLAM系統(tǒng)中通常采用IMU預(yù)積分[20]方式來處理高速率IMU測量值。本文在IMU預(yù)積分理論的基礎(chǔ)上，引入里程計數(shù)據(jù)，推導(dǎo)IMU/里程計預(yù)積分的基本原理與公式。

(13)

(14)

在此基礎(chǔ)上，推導(dǎo)IMU/里程計預(yù)積分在連續(xù)形式下的誤差傳遞方程，t時刻誤差項的導(dǎo)數(shù)為

(15)

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

1.4 系統(tǒng)初始化

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

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

(16)

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

(17)

(18)

由于重力矢量的模值已知，在空間中只具有兩個自由度?？梢詫⒅亓κ噶恐匦逻M行分解，代入(17)式中進行計算，求得精細化分解的重力矢量，并與世界坐標系w坐標系的z軸對齊。據(jù)此可以將初始化的速度、位置及姿態(tài)等狀態(tài)量均旋轉(zhuǎn)至w坐標系中，完成系統(tǒng)初始化。

1.5 非線性優(yōu)化

(19)

單目/IMU/里程計緊耦合SLAM系統(tǒng)非線性優(yōu)化的目標函數(shù)為：

(20)

(21)

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

(22)

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

2 實驗結(jié)果與分析

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

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

表1 角點數(shù)量對比

表2 迭代次數(shù)對比

表3 匹配誤差對比

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

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

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

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

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

圖2 室內(nèi)實驗運行軌跡圖

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

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

表4 室內(nèi)開環(huán)實驗終點間距離誤差

表5 室內(nèi)開環(huán)實驗距離偏差占總里程的百分比

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

圖3 室外開環(huán)實驗軌跡對比

圖4 室外閉環(huán)實驗軌跡對比

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

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

圖5 室外閉環(huán)實驗距離誤差對比

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

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

3 結(jié)論

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