未知環境下履帶式移動機器人SLAM研究*

賀利樂, 王消為, 趙 濤

(西安建筑科技大學 機電工程學院，陜西 西安 710055)

0 引 言

同步定位與地圖構建(simultaneous localization and mapping，SLAM)是指在未知環境中，移動機器人從某一未知起點開始移動，在移動過程中通過自身攜帶的傳感器進行識別特征路標，在增量式建立環境地圖的同時利用已建立的地圖來更新自身的位置，進而實現移動機器人的自主定位與導航。Montemerlo M,Doucet A等學者[1～4]提出基于Rao-Blackwellized粒子濾波器(Rao-Blackwellized particle filters,RBPF)的SLAM算法，至今這種算法仍然得到廣泛應用。文獻[5～7]主要基于RBPF算法應用組合導航的方式進行移動機器人SLAM的研究，采用多傳感器融合有效提高了定位與地圖構建的準確性及魯棒性，但仍存在地圖構建不精確，實時性差等問題；文獻[8]基于圖優化的同步定位與地圖創建進行了闡述，但圖優化的方法復雜度高、計算量較大，對SLAM的實時性無法滿足；文獻[9]提出一種單目視覺ORB-SLAM(oriented-FAST and rotated BRIEF-SLAM)/慣性導航系統(inertial navigation system,INS)組合導航的方法并設計了試驗平臺進行驗證，組合導航系統具有較高的定位精度，但魯棒性不強。

本文基于改進的RBPF算法研究激光雷達與雙目視覺融合的移動機器人同步定位與地圖構建；采用改進的RBPF算法，在計算提議分布時將移動機器人的觀測數據與里程計信息融合；對于常規視覺信息處理速度較慢，采用基于快速魯棒性局部特征檢測器—ORB對視覺圖像進行處理，從不同視覺角度的圖像中提取特征點，匹配并計算得到旋轉角度;通過在安裝有開源機器人操作系統(robot operating system,ROS)的履帶式移動機器人上進行多組實驗，驗證其有效性及可行性。

1 改進RBPF實現SLAM

由于傳統RBPF-SLAM一般將里程計運動模型作為提議分布，但里程計運動模型噪聲比較大，會使得各粒子間的權值差別很大，導致粒子出現乏匱現象，因此提出采用改進的RBPF算法。

1.1 改進提議分布

將觀測傳感器(激光雷達與雙目視覺當前觀測信息)提取出來的融合信息zt融入到粒子提議分布中，能夠有效降低粒子匱乏問題的發生。采樣分布提議如下

(1)

式中xt為t時刻的實際位姿向量,ut為移動機器人的控制信息,m為地圖。則相應的計算重要性權重公式為

(2)

式中z1:t為觀測信息，u1:t為控制的歷史信息。

1.2 改進的RBPF的SLAM算法步驟

2 特征提取及數據關聯與融合

由于移動機器人移動過程中可能出現輪子打滑、漂移等問題，導致里程計數據出錯;二維激光雷達只能在固定某一平面上掃描;視覺易受環境背景、光照等條件影響。故提出上述應用改進RBPF的SLAM算法信息融合及同步定位與地圖構建建模的分析，采用雙目視覺與激光雷達結合里程計，實現移動機器人的自主導航。

2.1 基于ORB的圖像特征提取與匹配

ORB算法[10]主要步驟分為特征點檢測及特征點描述。機器人所在環境下雙目視覺傳感器獲得的圖像中的ORB特征點提取和匹配效果如圖1所示。

圖1 圖像特征點處理

將上述匹配的特征點作為可視化路標，隨機選取路標點代表如圖1(a)中A(0.93,1.32,-0.01),B(0.84,1.94,-0.01),C(-0.91,2.63,0.01),D(-0.76,5.68,1.98),E(0.67,6.63,2.53)，建立包含可視化路標點坐標信息的路標數據庫，從圖像中提取出的三維坐標轉化至水平面上，并構建出水平面上2D路標地圖。

2.2 數據關聯與融合

2.2.1 數據關聯

基于多傳感器融合及組合導航的方法將不同傳感器的觀測信息進行關聯時，存在局部數據之間及局部與全局數據的關聯問題，為了更好的獲得全局環境地圖和實現定位，本文采用近鄰算法[11]進行數據關聯，其過程為：

2.2.2 數據融合

首先對激光雷達測距數據進行處理，得到環境中障礙物的距離信息；然后用ORB算法從雙目視覺傳感器中獲得的兩幅不同視角的圖像中尋找特征點，匹配并得到旋轉角度，將得到的匹配角度與里程計角度融合，使其更加接近實際角度；再結合激光測距與雙目視覺傳感器，分別融合距離信息與旋轉角度信息，定位移動機器人的初始位置；根據激光雷達與雙目視覺傳感器獲得的路標信息，構建特征地圖；最后采用上述改進的RBPF算法進行移動機器人的自主導航。其流程如圖2。

圖2 數據融合流程

3 實 驗

3.1 實驗平臺及環境

本文利用一臺雙輪差分驅動的履帶式移動機器人進行實驗，如圖3。機器人本體上配有HOKUYO二維激光雷達URG-04LX、雙目視覺系統Bumblebee2、里程計等傳感器及一個串口連接的控制計算機。且控制計算機上安裝有Ubuntu14.04操作系統及Indigo版本的ROS，通過與另外一臺安裝ROS的DELL筆記本電腦建立通信，進行遠程連接并控制機器人移動。

圖3 履帶式移動機器人

實驗環境選取光線較明亮的學校工科大樓十九層走廊作為實驗環境。

3.2 自主導航實現

1)使遠程控制計算機和移動機器人控制計算機連接到同一個WiFi網絡下;2)在遠程控制PC上建立節點管理器和創建鍵盤調試、SLAM與路徑規劃等節點，同時在移動機器人控制計算機上創建雙目視覺、激光雷達測距、里程和基礎控制器等節點;3)在節點管理器中進行所有的節點注冊并由節點管理器統一管理，通過端對端的拓撲結構在同一網絡中進行TCP/IP通信；4)利用3D可視化工具RVIZ實時顯示移動機器人自主導航的狀態信息。

3.3 實驗結果與分析

在實驗環境下進行，分別用上述改進的RBPF算法與傳統RBPF算法進行對比實驗。移動機器人運動速度為0.5 m/s，通過ROS中的可視化工具RVIZ可以實時顯示地圖創建的過程,如圖4、圖5(a)為采用多傳感器融合的傳統RBPF算法30個粒子構建的2D柵格環境地圖，由于其算法僅使用里程計作為提議分布，因此隨著時間的推移里程計累積誤差越來越大，所構建的地圖出現了不一致現象。圖5(b)為使用上述多傳感器融合改進的RBPF算法10個粒子構建精確的2D柵格環境地圖。

圖4 為RVIZ顯示的實時地圖

圖5 構建的柵格地圖

實驗中傳統RBPF算法與改進RBPF算法創建程度一致性地圖時所平均消耗時間分別為280 s和130 s，可以看出:改進RBPF算法需要的粒子數目遠小于傳統RBPF算法，同時在構建地圖的過程中所用的時間也很大程度地縮短，有效提高了算法的運算效率。

分別僅使用激光雷達傳感器與僅使用雙目視覺傳感器進行移動機器人的自主導航實驗，效果如圖6所示。

圖6 單一傳感器與改進算法構建的地圖

對比傳統算法與改進算法，可以看出傳統算法隨著時間的推移，構建出的地圖出現扭曲、不一致現象，但采用改進RBPF算法時,構建的地圖相對穩定、精確。

在滿足移動機器人SLAM實時性的情況下，傳統算法與改進算法各選取6，10，15，20，28，35，40，46，50，60共10組粒子數在實驗環境下進行實驗，每個條件下進行a=10次，創建地圖成功的次數為b，得出相應粒子數下地圖構建的成功率h=(b/a)×100 %，將所創建地圖的連續來作為判斷創建地圖成功的標準，地圖連續性可通過將所建地圖放大30倍像素觀察判斷。構建地圖粒子數與成功率的關系如圖7所示。

圖7 構建地圖成功率與粒子數的關系

可以看出，當粒子數達到10個時改進算法就可以構建出一個高精度的柵格地圖，而傳統算法則需要30個粒子才能構建出同樣完整的地圖；當使用相同粒子數創建地圖時，改進算法的成功率明顯高于傳統算法，有效提高了移動機器人SLAM的魯棒性。

為了更加直觀地對比、分析基于改進RBPF算法的多傳感器融合組合導航優勢，本文進行改進算法與傳統算法在MATLAB中仿真位姿狀態估計精確性對比。分析結果如圖8，可以看出改進算法對位姿估計的均方根誤差遠小于傳統算法，曲線趨勢相對平滑。由于提議分布中加入了激光雷達與雙目視覺的融合信息，使粒子分布在真實狀態附近。因此，改進算法的狀態估計精度優于傳統算法。

圖8 狀態估計誤差

4 結 論

本文提出一種雙目視覺與激光雷達信息融合實現移動機器人的自主導航。實驗表明通過基于改進RBPF算法的履帶式移動機器人組合導航方法，進行多傳感器融合提供的冗余信息能夠構建可靠性更高、更精確的2D柵格環境地圖，有效提高了移動機器人SLAM的魯棒性。