室內移動機器人自主導航系統設計

林義忠，馬 凱

（廣西大學 機械工程學院，南寧530004）

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

要想實現智能移動機器人在環境中的自主導航，首要就是要帶領機器人認識環境，即環境示教，并將環境信息存儲下來，這一過程稱為地圖構建。常見的地圖構建方法有通過貼磁條等方式形成拓撲地圖，基于幾何特征關聯的激光掃描匹配方法得到的幾何地圖，以及基于視覺的地圖構建方法得到點云圖或柵格圖[1-4]。其中磁條等方式需經常維護，激光價格昂貴且需要通過布置反射板以提高精度，而通過視覺帶領機器人感知環境不僅是最貼近人類認識世界的方式，同時，視覺可攜帶的信息更為豐富，可以為移動機器人帶來更為廣闊的應用空間。

本文主要從移動機器人的硬件、軟件架構兩方面展開研究，在ROS（robot operating system）框架下設計并實現移動機器人在室內復雜環境下的地圖構建，并在此基礎上實現移動機器人的自主導航。

1 傳感器模型

1.1 相機幾何模型

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

RGB-D 相機的幾何模型可用來描述將三維坐標映射到二維平面的過程，通過相機標定過程得到相機的內外參數，進一步解算出像素坐標系下的點與世界坐標系中點的坐標轉化關系，如圖1所示。

圖1 相機成像模型Fig.1 Camera imaging model

式中：為相機內參；M=［RT］為相機外參。

1.2 里程計模型

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

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

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

2 移動機器人自主導航系統

2.1 基于Gmapping 的二維柵格地圖構建

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

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

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

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

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

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

圖3 地圖構建系統主要的數據流程Fig.3 Main data flow chart of the map building system

2.2 路徑規劃

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

式中：f（n）指從起始點經過中間節點n到達最終目標點所經過的這條最優路徑的估值函數；g（n）表示從初始點到某一時刻的節點n的實際代價函數；h（n）是從節點n到目標點的代價估值函數，又稱啟發函數。

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

然后通過預估一定時間內相應的軌跡，結合軌跡評價函數G（ν，ω）確定最終的規劃路線。

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

本文結合A* 算法和DWA 來進行移動機器人的路徑規劃，通過A* 算法搜索出從起點到目標點的路線信息，結合DWA 實現動態避障，使移動機器人具備自主導航的能力。

3 系統構建與實驗結果

3.1 硬件平臺和系統架構

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

圖4 移動機器人實驗平臺Fig.4 Mobile robot experimental platform

圖5 移動機器人自主導航系統框架Fig.5 Framework diagram of mobile robot autonomous navigation system

3.2 移動機器人地圖構建

首先在ROS 系統中驅動Kobuki 機器人底盤節點和鍵盤控制終端，建立PC 與底盤之間的控制關系，然后加載Gmapping 算法并通過鍵盤終端發布機器人移動的速度話題，底盤節點訂閱速度話題并按照指令在環境中移動，伴隨著底盤的移動，在移動機器人經過的地方，可在Rviz 中觀察增量式地圖構建的過程，其中白色區域為無障礙區，黑色區域表示有障礙物，其它區域表示未知區域。當移動機器人遍歷整個環境后，便可得到整個環境的二維柵格地圖信息。實際環境與柵格地圖如圖6所示，最終所建立的柵格地圖如圖6（b）圖所示。

圖6 實際環境與柵格地圖Fig.6 Actual environment and grid map

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

表1 地圖點坐標記錄Tab.1 Map point coordinate record

3.3 移動機器人自主導航

完成環境地圖的構建后，在上位機中加載構建的二維柵格地圖，通過基于粒子濾波的自適應蒙特卡洛算法對機器人進行定位，然后給定目標點坐標信息，便可規劃出一條從起點到目標點的路線，如圖7所示。

圖7 全局路徑規劃Fig.7 Global path planning

當環境發生小的變化或有障礙物突然出現在所規劃的路線前方時，通過RGB-D 相機識別障礙物的距離信息，及時地更新障礙物圖層，移動機器人將根據新的障礙物圖層重新規劃局部路線以繞開障礙物并繼續向著目標點運動，避障對比過程如圖8所示。

圖8 局部路徑規劃Fig.8 Local path planning

4 結語

本文首先對SLAM 技術進行了研究，然后完成了移動機器人軟硬件平臺的搭建，并驅動移動機器人在真實環境中完成地圖構建，最后結合A* 算法和動態窗口法對移動機器人進行路徑規劃。實驗結果表明，該自主導航系統在真實場景下建立的二維柵格地圖基本能夠滿足實際需求，并且基于該柵格地圖可以實現移動機器人的自主導航，提高了移動機器人在未知環境中的自主性。