林義忠，馬 凱

（廣西大學(xué) 機(jī)械工程學(xué)院，南寧530004）

移動機(jī)器人是機(jī)器人領(lǐng)域的重要分支，隨著計算機(jī)技術(shù)的進(jìn)步和硬件性能的提升，機(jī)器人正逐漸為人類分擔(dān)起更多的任務(wù)。在日常生活中，移動機(jī)器人可以自主完成地面清潔、商場導(dǎo)購及酒店迎賓等任務(wù)；在工業(yè)生產(chǎn)中，移動機(jī)器人可勝任無人倉儲貨運(yùn)和無人送貨等崗位，伴隨著無人汽車的出現(xiàn)，移動機(jī)器人的發(fā)展極大的豐富了人們的生活體驗(yàn)，給人們的生活帶來了巨大的變化。

要想實(shí)現(xiàn)智能移動機(jī)器人在環(huán)境中的自主導(dǎo)航，首要就是要帶領(lǐng)機(jī)器人認(rèn)識環(huán)境，即環(huán)境示教，并將環(huán)境信息存儲下來，這一過程稱為地圖構(gòu)建。常見的地圖構(gòu)建方法有通過貼磁條等方式形成拓?fù)涞貓D，基于幾何特征關(guān)聯(lián)的激光掃描匹配方法得到的幾何地圖，以及基于視覺的地圖構(gòu)建方法得到點(diǎn)云圖或柵格圖[1-4]。其中磁條等方式需經(jīng)常維護(hù)，激光價格昂貴且需要通過布置反射板以提高精度，而通過視覺帶領(lǐng)機(jī)器人感知環(huán)境不僅是最貼近人類認(rèn)識世界的方式，同時，視覺可攜帶的信息更為豐富，可以為移動機(jī)器人帶來更為廣闊的應(yīng)用空間。

本文主要從移動機(jī)器人的硬件、軟件架構(gòu)兩方面展開研究，在ROS（robot operating system）框架下設(shè)計并實(shí)現(xiàn)移動機(jī)器人在室內(nèi)復(fù)雜環(huán)境下的地圖構(gòu)建，并在此基礎(chǔ)上實(shí)現(xiàn)移動機(jī)器人的自主導(dǎo)航。

1 傳感器模型

1.1 相機(jī)幾何模型

視覺傳感器有多種類型，常用于移動機(jī)器人的視覺傳感器有單目攝像頭、雙目攝像頭、RGB-D 相機(jī)等類型[5]。單目相機(jī)結(jié)構(gòu)簡單，成本較低，但只能采集二維數(shù)據(jù)，無法直接確定物體的真實(shí)大小，具有尺度不確定性[6]，雙目攝像頭通過視差法計算出特征點(diǎn)在雙目相機(jī)左相機(jī)坐標(biāo)系下的坐標(biāo)，并且左右相機(jī)間距越大時可測得的距離越遠(yuǎn)，但是通過視差法計算非常占用計算資源[7]。RGB-D 相機(jī)可同時提供彩色和深度圖像信息，與其它相機(jī)相比，不僅避免了在獲取深度信息時不必要的計算，而且RGB-D 相機(jī)在拍攝時連續(xù)性好，環(huán)境信息更加豐富，適合用于移動機(jī)器人進(jìn)行環(huán)境感知[8]。

RGB-D 相機(jī)的幾何模型可用來描述將三維坐標(biāo)映射到二維平面的過程，通過相機(jī)標(biāo)定過程得到相機(jī)的內(nèi)外參數(shù)，進(jìn)一步解算出像素坐標(biāo)系下的點(diǎn)與世界坐標(biāo)系中點(diǎn)的坐標(biāo)轉(zhuǎn)化關(guān)系，如圖1所示。

圖1 相機(jī)成像模型Fig.1 Camera imaging model

式中：為相機(jī)內(nèi)參；M=［RT］為相機(jī)外參。

1.2 里程計模型

里程計是一種常用于輪式機(jī)器人相對定位的傳感器[9]，可以通過安裝在左右電機(jī)上的光電編碼器來獲取里程數(shù)據(jù)，然后根據(jù)機(jī)器人的輪子半徑和兩輪間距進(jìn)而計算出機(jī)器人的實(shí)時速度和位姿信息，其運(yùn)動模型如圖2所示。

圖2 里程計測量模型Fig.2 Odometer measurement model

記移動機(jī)器人在某一時刻的移動速度與水平方向的夾角為α；速度為ν；角速度為ω，則任意時刻機(jī)器人的位姿可表示為

2 移動機(jī)器人自主導(dǎo)航系統(tǒng)

2.1 基于Gmapping 的二維柵格地圖構(gòu)建

Gmapping 是基于RBPF 粒子濾波器構(gòu)建算法（rao-blackwellized particle filter，RBPF）的改進(jìn)[10]。根據(jù)貝葉斯法則，RBPF 將定位和建圖分離，實(shí)現(xiàn)先定位再建圖，大大降低了SLAM 模型的復(fù)雜度，此過程如式（3）所示：

式中：P表示需要描述的分量；x代表移動機(jī)器人位姿；M代表環(huán)境地圖信息；Z代表觀測信息的集合；U代表運(yùn)動控制輸入的集合。

在針對RBPF 算法需要大量粒子擬合目標(biāo)分布且多次重采樣容易導(dǎo)致粒子耗散的問題上，Gmapping 在RBPF 的基礎(chǔ)上提出了改進(jìn)提議分布和選擇性重采樣，從而減少粒子個數(shù)和防止粒子退化[11]。在減少粒子個數(shù)方面，考慮到通常激光測得的數(shù)據(jù)比里程計精確，觀測模型的分布小于里程計估計分布，在僅以里程計運(yùn)動信息作為先驗(yàn)位姿估計的基礎(chǔ)上，將激光信息也融入到提議分布中以便更接近目標(biāo)分布，改進(jìn)后的提議分布函數(shù)為

在防止粒子退化方面，通過計算粒子權(quán)重，并設(shè)定閾值，將粒子權(quán)重與閾值比較，進(jìn)行選擇性重采樣，既保證了粒子權(quán)重，也減少了粒子重復(fù)采樣的次數(shù)，從而避免粒子退化。粒子權(quán)重計算方法為

Gmapping 算法以激光數(shù)據(jù)和里程計數(shù)據(jù)作為輸入，基于運(yùn)動模型和觀測模型實(shí)現(xiàn)同步定位與地圖構(gòu)建。其核心在于將機(jī)器人置于環(huán)境中的任意起始位姿后，通過驅(qū)動機(jī)器人移動獲取環(huán)境的先驗(yàn)信息，并通過觀測模型將獲取到的觀測信息與先驗(yàn)信息進(jìn)行匹配，實(shí)現(xiàn)對里程計由于打滑等原因造成數(shù)據(jù)誤差的實(shí)時修正，完成對機(jī)器人的定位，然后基于機(jī)器人的實(shí)時位姿構(gòu)建二維柵格地圖。

ROS 中通過Gmapping 構(gòu)建地圖的數(shù)據(jù)流程如圖3所示，里程計發(fā)布的數(shù)據(jù)傳入到STM32 控制開發(fā)板得到底盤移動的距離和轉(zhuǎn)動的角度，通過USB通信的方式將數(shù)據(jù)傳到上位機(jī)中，完成里程計信息的輸入。RGB-D 相機(jī)需要先將相機(jī)獲得的深度圖轉(zhuǎn)化為偽激光掃描信息，其核心原理為將深度圖中的像素經(jīng)過坐標(biāo)轉(zhuǎn)換得到世界坐標(biāo)系下的投影點(diǎn)到相機(jī)光心之間的距離[12]。以獲得的里程計信息和激光掃描信息作為Gmapping 的輸入?yún)?shù)，經(jīng)過復(fù)雜的運(yùn)算后得到地圖信息，最終將地圖信息顯示在上位機(jī)的Rviz 中。

圖3 地圖構(gòu)建系統(tǒng)主要的數(shù)據(jù)流程Fig.3 Main data flow chart of the map building system

2.2 路徑規(guī)劃

移動機(jī)器人的路徑規(guī)劃指的是基于對環(huán)境信息的掌握情況，結(jié)合移動機(jī)器人自身的定位信息，搜索出一條從起點(diǎn)到目標(biāo)點(diǎn)的無碰撞并盡量最優(yōu)的路徑。路徑規(guī)劃通常有全局路徑規(guī)劃和局部路徑規(guī)劃[13]。常用的路徑規(guī)劃算法包括人工勢場法、蟻群算法、A*算法及DWA 算法等[14]。其中全局路徑規(guī)劃是基于靜態(tài)環(huán)境下的路徑規(guī)劃，即在先驗(yàn)地圖模型的基礎(chǔ)上，給機(jī)器人規(guī)劃出一條到達(dá)最終目標(biāo)點(diǎn)且滿足一定要求的全局路徑。A*是一種典型的全局路徑規(guī)劃算法，通過引入啟發(fā)函數(shù)使整體性能相比于Dijkstra 有了較大改善，其數(shù)學(xué)描述可表示為

式中：f（n）指從起始點(diǎn)經(jīng)過中間節(jié)點(diǎn)n到達(dá)最終目標(biāo)點(diǎn)所經(jīng)過的這條最優(yōu)路徑的估值函數(shù)；g（n）表示從初始點(diǎn)到某一時刻的節(jié)點(diǎn)n的實(shí)際代價函數(shù)；h（n）是從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的代價估值函數(shù)，又稱啟發(fā)函數(shù)。

局部路徑規(guī)劃側(cè)重于實(shí)時避障，對于環(huán)境中出現(xiàn)的動態(tài)障礙物或者當(dāng)環(huán)境特征出現(xiàn)較小的改動時，通過機(jī)器人上搭載的視覺傳感器對環(huán)境的實(shí)時感知，在基于靜態(tài)先驗(yàn)地圖模型的基礎(chǔ)上，添加障礙物信息，形成障礙物層以實(shí)現(xiàn)環(huán)境信息的動態(tài)更新，局部路徑規(guī)劃基于障礙物圖層，規(guī)劃出實(shí)時路徑避開障礙物。DWA 算法是通過添加約束條件并取其交集，將移動速度限定在一個小范圍內(nèi)，如式（7）所示：

然后通過預(yù)估一定時間內(nèi)相應(yīng)的軌跡，結(jié)合軌跡評價函數(shù)G（ν，ω）確定最終的規(guī)劃路線。

式中：h（ν，ω）為方位角評價函數(shù)，用來評估若移動機(jī)器人按照當(dāng)前采樣的速度移動，在到達(dá)預(yù)測軌跡末端時的朝向與目標(biāo)方向的一致性；d（ν，ω）為間隙評價函數(shù)，評估移動機(jī)器人按照預(yù)測的軌跡運(yùn)動到目標(biāo)點(diǎn)時與障礙物間的最小距離；v（ν，ω）為速度評價函數(shù)，評估當(dāng)前軌跡的速度大小。

本文結(jié)合A* 算法和DWA 來進(jìn)行移動機(jī)器人的路徑規(guī)劃，通過A* 算法搜索出從起點(diǎn)到目標(biāo)點(diǎn)的路線信息，結(jié)合DWA 實(shí)現(xiàn)動態(tài)避障，使移動機(jī)器人具備自主導(dǎo)航的能力。

3 系統(tǒng)構(gòu)建與實(shí)驗(yàn)結(jié)果

3.1 硬件平臺和系統(tǒng)架構(gòu)

本文設(shè)計的自主導(dǎo)航系統(tǒng)主要由機(jī)器人底盤、RGB-D 相機(jī)和上位機(jī)組成，其中機(jī)器人底盤采用Yujin 公司開發(fā)的Kobuki 移動機(jī)器人，該款機(jī)器人底盤采用兩輪差速驅(qū)動，內(nèi)部配備了STM32 控制開發(fā)板，并配有外接擴(kuò)展口，可通過USB 通信的方式與上位機(jī)進(jìn)行通信；RGB-D 相機(jī)選用華碩的Xtion Pro Live，其有效視距為0.8~3.5 m；上位機(jī)為一臺Intel Core i5-3210M，8 GB 內(nèi)存的筆記本電腦，并預(yù)裝有ROS 操作系統(tǒng)，主要承擔(dān)著算法運(yùn)算和指令發(fā)布等工作，搭建的移動機(jī)器人實(shí)驗(yàn)平臺如圖4所示，自主導(dǎo)航系統(tǒng)框架如圖5所示。

圖4 移動機(jī)器人實(shí)驗(yàn)平臺Fig.4 Mobile robot experimental platform

圖5 移動機(jī)器人自主導(dǎo)航系統(tǒng)框架Fig.5 Framework diagram of mobile robot autonomous navigation system

3.2 移動機(jī)器人地圖構(gòu)建

首先在ROS 系統(tǒng)中驅(qū)動Kobuki 機(jī)器人底盤節(jié)點(diǎn)和鍵盤控制終端，建立PC 與底盤之間的控制關(guān)系，然后加載Gmapping 算法并通過鍵盤終端發(fā)布機(jī)器人移動的速度話題，底盤節(jié)點(diǎn)訂閱速度話題并按照指令在環(huán)境中移動，伴隨著底盤的移動，在移動機(jī)器人經(jīng)過的地方，可在Rviz 中觀察增量式地圖構(gòu)建的過程，其中白色區(qū)域?yàn)闊o障礙區(qū)，黑色區(qū)域表示有障礙物，其它區(qū)域表示未知區(qū)域。當(dāng)移動機(jī)器人遍歷整個環(huán)境后，便可得到整個環(huán)境的二維柵格地圖信息。實(shí)際環(huán)境與柵格地圖如圖6所示，最終所建立的柵格地圖如圖6（b）圖所示。

圖6 實(shí)際環(huán)境與柵格地圖Fig.6 Actual environment and grid map

為了定量的描述柵格地圖的精度，在柵格地圖和真實(shí)環(huán)境的橫向和縱向2 個方向上分別取3 個不同的點(diǎn)對進(jìn)行測量，每個點(diǎn)對在柵格地圖中和真實(shí)環(huán)境中都測量5 次，取其平均值并記錄于表1中，最后將柵格地圖中測量平均值與真實(shí)環(huán)境中測量結(jié)果的平均值進(jìn)行對比，可以從橫向和縱向兩個維度衡量出柵格地圖的整體精度。

表1 地圖點(diǎn)坐標(biāo)記錄Tab.1 Map point coordinate record

3.3 移動機(jī)器人自主導(dǎo)航

完成環(huán)境地圖的構(gòu)建后，在上位機(jī)中加載構(gòu)建的二維柵格地圖，通過基于粒子濾波的自適應(yīng)蒙特卡洛算法對機(jī)器人進(jìn)行定位，然后給定目標(biāo)點(diǎn)坐標(biāo)信息，便可規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的路線，如圖7所示。

圖7 全局路徑規(guī)劃Fig.7 Global path planning

當(dāng)環(huán)境發(fā)生小的變化或有障礙物突然出現(xiàn)在所規(guī)劃的路線前方時，通過RGB-D 相機(jī)識別障礙物的距離信息，及時地更新障礙物圖層，移動機(jī)器人將根據(jù)新的障礙物圖層重新規(guī)劃局部路線以繞開障礙物并繼續(xù)向著目標(biāo)點(diǎn)運(yùn)動，避障對比過程如圖8所示。

圖8 局部路徑規(guī)劃Fig.8 Local path planning

4 結(jié)語

本文首先對SLAM 技術(shù)進(jìn)行了研究，然后完成了移動機(jī)器人軟硬件平臺的搭建，并驅(qū)動移動機(jī)器人在真實(shí)環(huán)境中完成地圖構(gòu)建，最后結(jié)合A* 算法和動態(tài)窗口法對移動機(jī)器人進(jìn)行路徑規(guī)劃。實(shí)驗(yàn)結(jié)果表明，該自主導(dǎo)航系統(tǒng)在真實(shí)場景下建立的二維柵格地圖基本能夠滿足實(shí)際需求，并且基于該柵格地圖可以實(shí)現(xiàn)移動機(jī)器人的自主導(dǎo)航，提高了移動機(jī)器人在未知環(huán)境中的自主性。