基于單目稀疏法多傳感器融合移動機器人定位

王立玲，李森，馬東

(1.河北大學電子信息工程學院，河北保定 071002；2.河北省數字醫療工程重點實驗室，河北保定 071002)

0 前言

移動機器人在運動中自主導航完成設定任務，需要定位系統計算自身所在環境中的位姿。SLAM是同步定位與地圖構建(Simultaneous Localization and Mapping)，主要用于解決移動機器人在未知環境中運行時定位導航與地圖構建的問題。為獲取先驗信息確定移動機器人的位姿，在設計移動機器人時配備了各種傳感器，如編碼器、IMU(慣性測量單元)傳感器、相機、激光雷達等。移動機器人從搭載的傳感器上獲取到觀測數據，進行移動機器人相對于未知環境的位姿估計，完成定位。

移動機器人自主定位導航算法研究過程中，基于MEMS-IMU的捷聯式慣性導航系統以其成本低、體積小、功耗低等成為研究的熱點[1]。尤其，基于MEMS-IMU/GPS 的組合導航系統[2]被廣泛用于各種領域，在移動機器人定位領域產生深遠影響。針對GPS無法提供服務的地方，提出一種基于高精度慣導和里程計的車載 DR 誤差補償算法。但是，此系統定位誤差會隨時間迅速增長,無法滿足實際使用要求[3]。

為了提高移動機器人姿態估計的精確性，采用多個傳感器進行數據融合，完成移動機器人定位位姿計算。傳感器融合的優點是能夠提供比單個傳感器更好的測量精度和更精準的推斷。

文獻[4]中提出了一個基于事件的卡爾曼濾波器的傳感器融合框架，將全局傳感器和慣性測量單元的測量結合，在基于事件的時間表上，改進了計算資源有限的移動機器人的定位系統。文獻[5]中提出了一種融合GPS和IMU測量的自適應神經元模糊推理系統。

根據載體搭載的傳感器不同，可構建不同的SLAM系統。當機器人搭載的傳感器是相機，則視為視覺SLAM。其中，視覺SLAM分為單目視覺、RGB-D SLAM、雙目SLAM。由于單目相機質量輕、功耗小、成本低，能夠廣泛應用在各個領域。MUR-ARTAL 等[6]于2015年提出了ORB-SLAM算法，運用單目相機在計算機的CPU上實時建圖與定位。該算法的關鍵點提取和描述子的計算花費近20 ms；但是，會忽略除特征點以外的大部分信息，在特征缺失地方采集不到足夠的匹配點進行計算，從而可能丟棄有用的視覺信息。FORSTER等[7]于2014年提出了一種半直接法的視覺里程計SVO(Semi-direct Monocular Visual Odometry)，前期只跟蹤稀疏的關鍵點(不計算描述子)，再使用直接法估計相機的位姿，計算速度快于使用特征點法的ORB-SLAM2，此算法存在累計誤差，在特征點丟失后便不能進行重定位如果相機自動曝光，當調整曝光參數時，會使得整體變亮或變暗造成直接法無法計算。

為解決視覺SLAM系統在定位過程中遇到的問題，如單目SLAM系統無法恢復尺度信息等，香港科技大學QIN等[8]提出VINS-Mono(Monocular Visual-Inertial State Estimator)算法，通過單目視覺與IMU結合恢復單目相機的尺度因子，實現在四旋翼飛機載體中的定位與導航。但是，算法整體對單目相機和 IMU的性能以及CPU的數據處理要求較高[9-10]。隨后，提出支持多種傳感器組合的視覺慣性SLAM算法框架VINS-Fusion[11]。MUR-ARTAL、TARDS[12]提出結合ORB-SLAM 緊耦合的單目視覺慣性 SLAM，使單目相機的狀態變量與IMU 傳感器的狀態變量進行數據融合，采用緊耦合方式共同構建出系統的運動方程和觀測方程，再計算系統的狀態。其算法中IMU傳感器的狀態變量的初始化依賴單目相機狀態的初始化。

綜合上述研究，本文作者在無視覺信息輸入以及單目相機移動過快視覺跟蹤丟失時，通過擴展卡爾曼濾波融合里程計和IMU輸入觀測數據進行移動機器人定位，提供短時間位姿估計，保證在缺失相機位姿時移動機器人能精確定位；視覺前端相機位姿計算采用半直接法，在移動端側(嵌入式端)上實時運行；移動過程中完成單目相機尺度信息的計算，單目輸出位置信息通過編碼器輸出的速度信息和IMU信息進行擴展卡爾曼融合輸出位姿，改善了編碼器和慣性測量單元單獨定位精度低、累計誤差大的缺點；邊緣側混合式多傳感器融合免去當前移動機器人移動過程必須有線連接便攜式計算機或者將便攜式計算機放置在移動機器人頂部進行SLAM算法計算的方式，減小了移動機器人的體積，具有廣泛的擴展性。

1 單目位姿輸出

MUR-ARTAL等提出的ORB-SLAM系統與其他SLAM系統相比較具有較高的精度和運行效率。但是，嵌入式領域不具備實時運行ORB-SLAM系統的能力。半直接法(SVO)視覺SLAM算法能夠在移動側運行完成相機位姿估計，但是半直接法(SVO)視覺里程計只能估計當前時間的相機位姿，不具備后端優化和閉環檢測的能力。

作者采用特征點與直接法相結合的方法進行優化相機位姿，將半直接法(SVO)前端位姿計算方法融合到ORB-SLAM2中，充分利用直接法對光照變化、稀疏紋理的魯棒性，在使用單目相機之前首先進行相機標定，獲取相機的內參以及畸變參數。融合后的半直接法的初始化位姿流程如圖1所示。

圖1 Tracking線程流程

最小化殘差損失函數：

(1)

通過式(1)計算幀間匹配得到的當前幀相機位姿具有累計誤差，從而導致相機位姿漂移，因此需要通過特征對齊找到地圖中特征塊在新一幀中真正的位置。基于光度不變性的假設，重新構造殘差對特征點的預測位置進行再次的優化：

(2)

式(2)優化后比式(1)預測的相機位姿位置更精準，但是式(2)優化后和式(1)預測的特征點的位置也會存在誤差，因此需要構造新的目標函數進行優化。由式(3)進行優化計算，得出更準確的相機位姿：

(3)

2 多傳感器融合算法流程

張福斌、劉宗偉[13]提出改進ORB-SLAM2算法，將單目相機、三軸陀螺儀、里程計3種傳感器進行緊組合，通過非線性優化實現精度較高的定位。而文獻[14]中創新性是提出穩定自治系統的光照魯棒直接視覺里程計，空中機器人在不可預知的光照條件下飛行。因此，為提高移動機器人的定位精度采用多傳感器融合方法。

根據傳感器融合方法的不同，多傳感器融合方式分為松耦合和緊耦合兩種方式。其中，松耦合是各個傳感器以各自的傳感器數據進行運算后再以融合算法將其數據融合后計算出位姿。緊耦合是考慮各個傳感器之間的耦合性，通過視覺和慣性傳感器進行數據耦合計算，但是對計算載體的計算算力具備一定要求。多傳感器緊耦合框架面對某個傳感器失效時，會出現迅速漂移等問題。松耦合面對單個傳感器故障而導致失效時，也能夠精確定位。因此，本文作者采用基于擴展卡爾曼濾波的松耦合融合算法進行定位研究。

2.1 算法思路

本文作者通過采集單目攝像頭數據流、IMU和里程計數據，通過擴展卡爾曼濾波融合算法進行位姿定位。基本思路如圖2所示。

圖2 算法思路

2.2 單目視覺位姿估計模型：

針對單目相機，選用針孔相機模型。

Puv=KPw

(4)

(5)

移動機器人移動的距離xb通過編碼器和IMU的擴展卡爾曼融合數據獲取，解決單獨使用編碼器計算移動距離導致的誤差累計這一問題。與此同時獲取固定時間間隔內的單目視覺里程計估算的移動距離xc。在實驗過程中，實時估計計算出單目視覺的尺度因子，采用如式(6)進行計算，完成單目視覺的尺度恢復。

λ≈xb/xc

(6)

2.3 里程計模型

2.4 IMU運動模型

IMU的觀測模型描述如式(7)—式(8)所示，上標I表示IMU坐標系，上標G表示參考坐標系。

(7)

(8)

3 基于EKF的多傳感器融合算法實現

以擴展卡爾曼濾波法作為多傳感器融合框架，是以松耦合方式融合編碼器里程計速度、單目視覺位姿、慣性傳感器信息進行定位。設計的SLAM系統算法框架如圖3所示。

圖3 多信息融合算法

由于移動機器人工作在平面中，它是非完整約束機器人，需要將3D變量歸零，包括Z軸、俯仰、橫滾以及各自的速度和Z軸加速度。但是需要將Y軸速度融合進去，因為只有這樣才能夠證明Y軸沒有移動。即表示為7維列向量：

系統方程及測量方程：

xk=f(xk-1)+ωk

(9)

zk=h(xk)+vk

(10)

其中：變量xk、zk分別表示為在時間k時刻的狀態向量和測量向量。協方差為Q的過程噪聲由變量ωk-1表示，協方差為R的觀測噪聲由vk表示。過程噪聲和觀測噪聲為互不相關的零均值高斯白噪聲。

(11)

(12)

(1)系統狀態初始化

多傳感器融合系統在接收到傳感器發出的第一個測量數據時，對系統狀態變量和狀態協方差進行初始化，得出初始狀態量和初始狀態協方差。

(2)預測過程

(13)

(14)

(3)更新過程

(15)

(16)

(17)

上述傳感器的輸入數據Rk受到誤差Q和方差Pk-1的影響。

4 算法的驗證及分析

4.1 實驗平臺

為完成上述實驗算法的驗證，采用自主設計實驗平臺，驗證平臺采用Ackerman模型的四輪智能小車移動機器人，按圖4所示的硬件框圖自主搭建實驗平臺。設計的移動機器人采用Teensy單片機讀取直流減速電機和MPU9250數據，視覺里程計傳感器采用樹莓派自帶單目相機。多傳感器融合的決策部分采用樹莓派完成移動機器人位姿估計計算，PC端實時顯示當前位姿，通過路由進行無線鏈接進行數據傳輸。搭建完成的實物平臺如圖5所示。

圖4 硬件系統框圖

圖5 實物平臺

硬件平臺參數：

(1)相機為Pi Camera V2：分辨率2 592像素×1 944 像素(500萬像素)，感光芯片OV5647,角度65°；

(2)慣性傳感器采用MPU9250，其中陀螺儀采用±250(°)/s，陀螺儀的RMS噪聲0.1(°)/s(RMS),噪聲密度0.01(°)/(s·Hz1/2)；加速度為±2g；

(3)采用減速比30、編碼器精度為390的直流減速電機；

(4)采用四核64位處理器的3B+樹莓派；

(5)計算機：RedmiBook16,CPU為AMD-R7-4700U；

軟件平臺：

開發平臺使用安裝Linux系統虛擬機的RedmiBook16便攜式計算機。Linux系統版本是Ubuntu16.04，并在此系統下安裝ROS機器人操作系統。樹莓派使用Ubuntu16.04系統并搭建ROS系統。運用ROS分布式的系統優勢，進行數據傳輸完成算法的軟件開發。

4.2 實驗設計

采用Nature-Point公司的高速動作捕捉系統Motive，完整記錄移動機器人頂部mark點的移動軌跡，設定動作捕捉系統記錄的移動軌跡為真值。設定小車以約0.01 m/s的速度恒速運行，前輪舵機以約0.012 rad/s速度進行轉彎。其中，由于移動機器人運動在平面上，只有繞Z軸轉動，因此繪制XY軸上的軌跡。圖形以EKF融合后的數據以及動作捕捉獲取的移動小車的真實位置數據繪制。

為驗證光照變化對單目視覺以及融合算法的影響，根據不同時間光照射入室內，在室內形成的光影變化不同，設定光照較為明亮的14：00-16：30時間段為時間段1和光照變化明顯的17：00-19：30時間段為時間段2，在兩個時間段進行驗證實驗數據采集。

設定兩種運行軌跡運行方式：

(1)圓弧運行：(1)以設定轉向角度前進運動軌跡成半圓弧；(2)先以設定的舵機轉向角度前進，然后再以原點為前進方向運行，后形成橢圓形圓弧。

(2)直線加圓弧運行。進行直線運行，完成兩次直角轉彎，最后采用圓弧轉彎回歸到起始位置。

上述兩種運行軌跡能夠完成室內移動機器人室內移動、避障、返回充電等全部運動的原始基本操作。

4.3 實驗結果

圖6是在時間段1移動小車以設定轉彎角度在軸方向運行約1 m左右后，再以原點作為運行終點，記運行的為軌跡1。

圖6 軌跡1實驗結果對比

圖7是在時間段2先沿直線運行，在第二個直角轉彎后，以設定轉動角度使移動小車返回到起點，記為軌跡2。

圖7 軌跡2實驗結果對比

表1 不同傳感器及融合方案均方根誤差比較 單位：m

在光照強度變化的場景下，單目視覺里程計誤差明顯減小，單目稀疏法的視覺里程計能夠準確估計相機位姿。其中，擴展卡爾曼濾波融合各個傳感器數據后得到的均方根誤差相較于單個傳感器均方根誤差明顯減小。

以上實驗證明在光照強度變化的環境中采用稀疏直接法(半直接法)的視覺里程計計算出的相機位姿具備有效性和準確性，并且根據視覺得出的位姿，EKF融合算法抑制了編碼器的誤差累計，有效提升整體移動機器人的定位精度。

5 結論

以稀疏直接法作為單目視覺的前端估計相機位姿，結合慣性傳感器和編碼器進行擴展卡爾曼濾波多傳感器融合定位。通過在光照明亮不變和光照變化的時間段內進行實驗,驗證此算法的準確性，證明該算法能夠改善在光照變化的場景中的定位精度，提高單個傳感器定位的定位精度，降低編碼器的累計誤差，擴展單目視覺的應用場景，減小了當前自主定位移動機器人的體積。