賀利樂, 王消為, 趙 濤

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

0 引 言

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

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

1 改進(jìn)RBPF實(shí)現(xiàn)SLAM

由于傳統(tǒng)RBPF-SLAM一般將里程計(jì)運(yùn)動模型作為提議分布，但里程計(jì)運(yùn)動模型噪聲比較大，會使得各粒子間的權(quán)值差別很大，導(dǎo)致粒子出現(xiàn)乏匱現(xiàn)象，因此提出采用改進(jìn)的RBPF算法。

1.1 改進(jìn)提議分布

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

(1)

式中xt為t時(shí)刻的實(shí)際位姿向量,ut為移動機(jī)器人的控制信息,m為地圖。則相應(yīng)的計(jì)算重要性權(quán)重公式為

(2)

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

1.2 改進(jìn)的RBPF的SLAM算法步驟

2 特征提取及數(shù)據(jù)關(guān)聯(lián)與融合

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

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

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

圖1 圖像特征點(diǎn)處理

將上述匹配的特征點(diǎn)作為可視化路標(biāo)，隨機(jī)選取路標(biāo)點(diǎn)代表如圖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)，建立包含可視化路標(biāo)點(diǎn)坐標(biāo)信息的路標(biāo)數(shù)據(jù)庫，從圖像中提取出的三維坐標(biāo)轉(zhuǎn)化至水平面上，并構(gòu)建出水平面上2D路標(biāo)地圖。

2.2 數(shù)據(jù)關(guān)聯(lián)與融合

2.2.1 數(shù)據(jù)關(guān)聯(lián)

基于多傳感器融合及組合導(dǎo)航的方法將不同傳感器的觀測信息進(jìn)行關(guān)聯(lián)時(shí)，存在局部數(shù)據(jù)之間及局部與全局?jǐn)?shù)據(jù)的關(guān)聯(lián)問題，為了更好的獲得全局環(huán)境地圖和實(shí)現(xiàn)定位，本文采用近鄰算法[11]進(jìn)行數(shù)據(jù)關(guān)聯(lián)，其過程為：

2.2.2 數(shù)據(jù)融合

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

圖2 數(shù)據(jù)融合流程

3 實(shí) 驗(yàn)

3.1 實(shí)驗(yàn)平臺及環(huán)境

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

圖3 履帶式移動機(jī)器人

實(shí)驗(yàn)環(huán)境選取光線較明亮的學(xué)校工科大樓十九層走廊作為實(shí)驗(yàn)環(huán)境。

3.2 自主導(dǎo)航實(shí)現(xiàn)

1)使遠(yuǎn)程控制計(jì)算機(jī)和移動機(jī)器人控制計(jì)算機(jī)連接到同一個(gè)WiFi網(wǎng)絡(luò)下;2)在遠(yuǎn)程控制PC上建立節(jié)點(diǎn)管理器和創(chuàng)建鍵盤調(diào)試、SLAM與路徑規(guī)劃等節(jié)點(diǎn)，同時(shí)在移動機(jī)器人控制計(jì)算機(jī)上創(chuàng)建雙目視覺、激光雷達(dá)測距、里程和基礎(chǔ)控制器等節(jié)點(diǎn);3)在節(jié)點(diǎn)管理器中進(jìn)行所有的節(jié)點(diǎn)注冊并由節(jié)點(diǎn)管理器統(tǒng)一管理，通過端對端的拓?fù)浣Y(jié)構(gòu)在同一網(wǎng)絡(luò)中進(jìn)行TCP/IP通信；4)利用3D可視化工具RVIZ實(shí)時(shí)顯示移動機(jī)器人自主導(dǎo)航的狀態(tài)信息。

3.3 實(shí)驗(yàn)結(jié)果與分析

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

圖4 為RVIZ顯示的實(shí)時(shí)地圖

圖5 構(gòu)建的柵格地圖

實(shí)驗(yàn)中傳統(tǒng)RBPF算法與改進(jìn)RBPF算法創(chuàng)建程度一致性地圖時(shí)所平均消耗時(shí)間分別為280 s和130 s，可以看出:改進(jìn)RBPF算法需要的粒子數(shù)目遠(yuǎn)小于傳統(tǒng)RBPF算法，同時(shí)在構(gòu)建地圖的過程中所用的時(shí)間也很大程度地縮短，有效提高了算法的運(yùn)算效率。

分別僅使用激光雷達(dá)傳感器與僅使用雙目視覺傳感器進(jìn)行移動機(jī)器人的自主導(dǎo)航實(shí)驗(yàn)，效果如圖6所示。

圖6 單一傳感器與改進(jìn)算法構(gòu)建的地圖

對比傳統(tǒng)算法與改進(jìn)算法，可以看出傳統(tǒng)算法隨著時(shí)間的推移，構(gòu)建出的地圖出現(xiàn)扭曲、不一致現(xiàn)象，但采用改進(jìn)RBPF算法時(shí),構(gòu)建的地圖相對穩(wěn)定、精確。

在滿足移動機(jī)器人SLAM實(shí)時(shí)性的情況下，傳統(tǒng)算法與改進(jìn)算法各選取6，10，15，20，28，35，40，46，50，60共10組粒子數(shù)在實(shí)驗(yàn)環(huán)境下進(jìn)行實(shí)驗(yàn)，每個(gè)條件下進(jìn)行a=10次，創(chuàng)建地圖成功的次數(shù)為b，得出相應(yīng)粒子數(shù)下地圖構(gòu)建的成功率h=(b/a)×100 %，將所創(chuàng)建地圖的連續(xù)來作為判斷創(chuàng)建地圖成功的標(biāo)準(zhǔn)，地圖連續(xù)性可通過將所建地圖放大30倍像素觀察判斷。構(gòu)建地圖粒子數(shù)與成功率的關(guān)系如圖7所示。

圖7 構(gòu)建地圖成功率與粒子數(shù)的關(guān)系

可以看出，當(dāng)粒子數(shù)達(dá)到10個(gè)時(shí)改進(jìn)算法就可以構(gòu)建出一個(gè)高精度的柵格地圖，而傳統(tǒng)算法則需要30個(gè)粒子才能構(gòu)建出同樣完整的地圖；當(dāng)使用相同粒子數(shù)創(chuàng)建地圖時(shí)，改進(jìn)算法的成功率明顯高于傳統(tǒng)算法，有效提高了移動機(jī)器人SLAM的魯棒性。

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

圖8 狀態(tài)估計(jì)誤差

4 結(jié) 論

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