自主移動機器人（AMR）混合導航算法改進

孫明，陳日輝，陳燕，陳小冰

（1.澳門科技大學，澳門 999078；2.珠海科技學院，廣東 珠海 519041）

0 引 言

隨著信息技術的發展，機器人的智能性和自主性不斷增強，新一代的自主移動機器人（Autonomous Mobile Robot，AMR）逐步在工農業生產和日常生活中發揮更加重要的作用。特別是近年來，根據新冠疫情防控的需要，大規模的消毒機器人，配送機器人的使用，大大提高了工作效率，降低了人際病毒的風險。其中，路徑規劃和導航技術的使用，是自主移動機器人（AMR）實現各項功能，完成各種任務的關鍵性因素。

路徑規劃是指機器人通過一定的算法規劃出一條從當前節點到任務節點的路徑，屬于運籌學和機器人學的基本問題之一。根據機器人對當前地圖環境的掌握程度，路徑規劃分為兩種類型，分別是通過激光、機器視覺等傳感器掌握局部地圖信息的局部路徑規劃，掌握全局地圖信息的全局路徑規劃。國內外學者分別對兩種路徑規劃方法進行了研究，探索出適用于局部路徑規劃的滾動窗口法（陳明建，林偉等，2017）、人工勢場法（石為人等，2010）、模糊邏輯控制法（孫揚智，2016）等，以及適用于全局路徑規劃的A*算法（章淑君與曹建成，2005）、蟻群算法（Dorigo，Maniezzo& Colorni，1996）、遺傳算法以及粒子群算法（王雷與李明，2017）等。

目前，機器人路徑規劃中的主流算法是A*算法以及在其基礎上進行的各種改進算法。A*算法在經典路徑規劃的Dijkstra算法的基礎上發展起來的一種啟發式搜索算法，具有運算時間短，收斂速度快，求解路徑的時間更短的優點，從而達到快速尋路的要求（章淑君與曹建成，2005）。作為一種全局路徑規劃算法，A*算法在動態環境中出現障礙物的情況下并不適用，這時候需要引入人工勢場法等算法幫助機器人實時規劃出平滑安全的新路徑。

本文圍繞自主移動機器人（AMR）在復雜動態環境中的運行問題，分別從地圖構建、定位、路徑規劃三部分，綜合考慮全局路徑規劃和局部路徑規劃問題，提出基于雙向A*算法進行路徑規劃和基于激光SLAM加人工勢場法進行局部路徑規劃的混合導航算法，以提高機器人自主運行的穩定性和實時性。

1 地圖構建與機器人定位

自主移動機器人（AMR）可以利用自身裝備的激光傳感器、攝像頭等設備，通過SLAM算法，自主構建運行區域的環境地圖模型并進行自主定位。

1.1 環境建模

目前，機器人進行環境建模應用最廣泛的是google推出的基于圖優化的SLAM算法Cartographer，該算法使用姿態優化進行優化，具有閉環檢測，有良好的系統實時性。在計算機資源有限的情況下，能獲得更高精準度的2D地圖。Cartographer算法核心思想為利用全局優化和局部優化對機器人位姿=(ε，ε，ε)進行優化，其中ε和ε為機器人在和方向位移的位移量，ε為機器人繞平面旋轉的角度。激光雷達初始掃描幀記為原點（0，0），激光雷達掃描數據描述為={h}，∈1，2，3…，，掃描數據幀映射到子圖的位姿變換記作T，公式如下：

子圖由多個掃描幀構成，使用概率網格表示。每個網格定義兩種狀態hit和miss，每次掃描被插入概率網格中，將hit臨近的所有網格點加入hit集合，除已在hit集合中的點外，將hit與掃描點連接射線上的所有接觸點加入miss集合。沒有加入任何集合的點設置，已觀測到的點概率按照式（3）進行更新。

使用Ceres求解器，在一個掃描被插入子圖前，和當前的子圖進行局部優化。此操作是為了在子圖中找到使插入掃描中所有的掃描點概率和最大的掃描位姿，由此便可轉變為一個非線性最小二乘問題。

Cartographer算法通過稀疏位姿調整方法來全局優化，保存掃描幀插入子圖的位姿，當子圖構建完畢，使用所有對應的掃描幀和子地圖進行閉環檢測，Cartographer框架如圖1所示。

圖1 Cartographer框架圖

1.2 自主定位

機器人通過Gmapping算法成功構建環境地圖后，需要準確判斷自己的位置，才能更好的進行路徑規劃，此時一般采用二維移動過程中自適應蒙特卡洛定位（Adaptive Monte Carlo Localization，AMCL），AMCL采用粒子濾波器來跟蹤已經知道的地圖中機器人位姿，對于大范圍的局部定位問題工作良好。該算法可在已知地圖的條件下，實時估計機器人的位姿信息。

AMCL算法思路如下：

位姿信息由機器人時刻的狀態x的后驗信度決定，使用S表示一組有限的粒子集，則后驗信度分布為：

AMCL算法在一段時間間隔內通過采集S和重采集S來繪制粒子，更新姿態并計算出后驗置信分布bel（x）在當前狀態下的離散估計：

式中為樣本估計概率。

重采樣的計算公式如下：

更新后的粒子集為：

使粒子均滿足：

2 路徑規劃

自主移動機器人（AMR）在地圖建模和自主定位完成后，根據收到的目標節點信息，既可利用A*算法進行路徑規劃。

2.1 A*算法

A*算法利用啟發函數對遍歷方向進行引導，使用評估函數計算每個格子的代價值()，是該算法中的核心，其啟發函數公式如下：

式中()為當前搜索節點到起點的真實距離代價，()為當前搜索節點到終點預測距離代價。為了實現最優路徑規劃，常用距離代價包括曼哈頓距離、對角線距離和歐幾里得距離，它們對應的距離如圖2所示，其中線1表示曼哈頓距離，適合4象限（東-西-南-北）運動，路徑規劃速度相對較快；線2表示對角線距離，適合8象限（東-東南-南-西南-西-西北-北-東北）運動；線3表示歐幾里得距離，適合任意角度的運動，路徑規劃的速度較慢。

哈曼頓距離不符合大部分機器人的實際運動場景，歐幾里距離需要遍歷大量節點，導致規劃速度較慢。在范圍較大且不規則地圖環境下，對角線距離比較符合真實機器人的移動模型并且路徑規劃的時間較快，因此本文采用對角線距離（Diagonal Distance）作為對()的計算。

對角線距離（Diagonal Distance）的計算公式如下：

式中x和y為當前節點的水平位置和垂直位置，和為終點的水平位置和垂直位置。

圖2 三種距離圖示

A*算法在運行中需要兩個列表來儲存遍歷的點，Open List用來存儲被遍歷的點，Close List用來存儲被選中的點。每個點被遍歷時會計算其()，Close List中的點被加入后不會再更新，每個點存儲著一個指向其父節點的指針和該點的()。

初始化兩個列表為空，然后將起點加入Open List。取出Open List中()最小的值加入Close List中，遍歷取出點的八個象限的所有點，如果該點為障礙物或者已在Close List中，則跳過該點。如果該點在Open List列表中，則重新計算該點的父節點，否則將該點加入Open List中，繼續遍歷直到終點或者Open List為空。如果Open List為空，說明沒有路徑可以到達終點。如果遍歷到終點，結束遍歷，隨著終點的父節點直到起點為該次尋路的最優路徑，A-star算法流程如圖3所示。

2.2 傳統A-star算法的不足及改進

伴隨著機器人運行區域的擴大，地圖規模也越來越大，傳統A-star算法隨著搜索空間增大，擴展節點增大，搜索速度下降。同時規劃出來的路徑存在折角過大，路徑不夠平滑的問題。

本文通過改進，讓A-star算法從起點和終點同時進行遍歷，直到兩邊遍歷過同一個柵格，將終點遍歷的路徑進行反向，獲取到一條完整的起點到終點的路徑，改進的A-star算法流程如圖4所示。

圖3 A-star算法流程圖

圖4 改進的A-star算法流程圖

2.3 解決路徑沖突的人工勢場法

在實際運行中，沿著規劃路徑前進的機器人會遇到各種動態的障礙物，形成路徑沖突，沿著既定路線前進的機器人需要重新規劃新的路徑，既進行局部路徑規劃。人工勢場法結構簡單，廣泛應用于機器人躲避障礙的局部路徑規劃。

人工勢場法（Artificial Potential Field Method）由Khatib在1994年提出，它通過將機器人在場景中的移動抽象到人工引力場之中，通過將機器人與目標產生的“斥力”和障礙物與機器人產生的“引力”之間產生的“合力”控制機器人的運動，圖5為人工勢場法的示意圖。通過建立對應的勢能場函數解決避障的問題。該方法能夠充分考慮機器人與每個障礙物之間產生的斥力，在A*算法找到最短路徑的同時避免碰撞。

目標位置與移動機器人之間引力場為：

機器人所受引力方向為引力勢場負梯度方向：

圖5 人工勢場法示意圖

設以障礙物為中心的斥力場影響半徑為ρ，機器人與障礙物之間的距離為，當機器人離開斥力場影響范圍，所受到的斥力為0，則斥力場大小為：

機器人所受斥力方向為斥力勢場負梯度方向：

機器人所受合力大小為：

3 混合算法的設計及實現

在復雜動態動態環境路徑規劃中，采取雙向A*加人工勢場法的混合算法的機器人更具有優勢，如圖6所示，混合算法的實現流程是：

（1）機器人采取雙向A*算法進行路徑規劃

（2）機器人在向目標節點運行過程中使用激光雷達等傳感器對行進路徑進行搜索，當發現障礙物存在時，啟動人工勢場算法進行局部路徑規劃。

（3）完成局部規劃后，判斷子目標點是否為終點，若是則完成路徑規劃，否則更新局部目標點。

圖6 混合算法流程圖

3.1 仿真環境搭建

本文仿真模擬采用的計算機配置如表1所示。

表1 仿真環境配置

本文利用Gazebo系統仿真軟件，選用Kobuki底盤作為仿真實驗主體，該底盤使用二輪差分驅動式，具有可擴展性強、易于搭建和配置的優點，在ROS系統上具有良好的適配性，Kobuki底盤參數如表2所示。傳感器使用思嵐A2激光雷達進行機器人周圍環境搜索和建模，RPLIDAR A2參數如表3所示。可以確保機器人快速度移動時的地圖構建質量。

表2 Kobuki底盤參數

高（mm） 124.8重量（kg） 2.35最大平移速度（cm·s-1） 70最大旋轉速度（°/s） 180里程計52 ticks/enc rev 2 578.33 ticks/wheel rev 11.7 ticks/mm陀螺儀 1軸（110度/秒）

表3 RPLIDAR A2參數

為便于研究，將移動機器人模型進行改造：

（1）對kobuki底盤模型進行簡化，刪掉無用傳感器，只留下左右輪、底盤、前后滾輪等必要的部件。

（2）在底盤上搭建支架，加入激光雷達傳感器。

（3）為機器人配置各個部件的碰撞屬性。

（4）設置關節類型和發布TF樹。

（5）修改Gazebo仿真中機器人的傳感器的參數和話題。

建好的機器人拓撲關系如圖7所示。

在Rviz中查看機器人模型的各項參數并進行調試。圖8、圖9為TF變換關系正視圖與仰視圖，使用joint_state_publisher_gui節點檢查動力輪的旋轉方向是否正確，觀察機器人模型各個link的TF變換關系，是否都與base_footprint有映射。

圖7 機器人拓撲關系圖

圖8 TF變換關系正視圖

圖9 TF變換關系仰視圖

完成機器人模型的構建后，使用Gazebo建立仿真環境，模擬機器人在真實的公共場合中運作。Gazebo可以使用Building Editor快速的繪制外圍墻壁，同時里面擁有大量的立方體、垃圾桶、桌子等模型，可以極大還原的還原真實世界。環境構建的步驟如下：

（1）確定環境的外圍墻壁的形狀、位置和尺寸；

（2）確定環境中的障礙物的類型、尺寸和數量，繪制障礙物模型；

（3）確定各個類型的障礙物的位置，根據實際需要擺放在合適的位置；

（4）使用Building Editor繪制圍墻，并將障礙物模型加入到環境中；

（5）調整好合適的觀察視角后保存環境為.world文件，方便后續調用。

通過研究和觀察，本文將對選取幾種典型的公共場所環境地形進行研究，建完的地圖模型如圖10～圖13所示。

圖10 O形地圖模型

圖11 S形地圖模型

圖12 U形地圖模型

圖13 L形地圖模型

環境模型建立好后，由機器人自行探索環境進行SLAM建圖。本文使用Cartographer算法進行地圖建立，利用Rviz可視化建立的地圖模型數據。建圖步驟如下：

（1）啟動Launch打開啟動Gazebo并加載地圖和機器人模型到環境中；

（2）運行Cartographer算法構建地圖的Launch文件；

（3）運行Rviz，訂閱/submap_list和/scan話題；

（4）運行move_base功能包，接收控制機器人的話題；

（5）使用2D Nav Goal功能發布目標點，使機器人運動完成地圖的構建。

待地圖構建完成，使用命令“rosservice call /finish_trajectory 0”停止接收數據，構建好的地形圖如圖14～圖17所示。

圖14 O形地圖構建

圖15 S形地圖構建

圖16 U形地圖構建

圖17 L形地圖構建

（1）運行rosservice call /write_state "{filename：'路徑/文件名.pbstream'}"序列化保存當前的狀態。

（2）運行cartographer_ros包里的cartographer_pbstream_to_ros_map將pbstream轉換為pgm和yaml以便后續使用。

3.2 算法驗證與分析

基于完成的準備工作，使用改進的A-star算法和人工勢場法的混合算法，驗證不同環境地形下機器人自主運作的避障能力。具體步驟如下：

（1）編寫路徑規劃算法的C++文件，使用插件注冊到ROS中來，查看可用插件，如圖18所示。

圖18 查看ROS路徑規劃可用插件

（2）將move_base的全局規劃替換成A-star算法，本地規劃替換成人工勢場法。

（3）在地圖模型中加入障礙物，模擬現實環境中的障礙。

（4）使用上述建立好的地圖模型，加入地圖啟動的配置中。

（5）編寫Launch文件一鍵啟動環境，包括仿真環境、機器人模型、Rviz等的啟動。

（6）啟動move_base和amcl功能包。

（7）編寫Python文件，使反復機器人到達選定終點并回到選定起點，輸出路徑規劃時間和路徑長度等信息（圖19～圖22）。

圖19 O形地圖路徑規劃

圖20 S形地圖路徑規劃

圖21 U形地圖路徑規劃

圖22 L形地圖路徑規劃

本文將A-star算法和人工勢場法混合算法與傳統的A-star算法在不同地形中進行比較，以研究該混合算法在不同場景下的普適性，并驗證改進的A-star算法相對于A-star算法是否有更好的性能。本文進行兩組實驗，將所獲得的實驗數據匯總成表格如表4，表5所示。

表4 A-star算法實驗數據

表5 改進A-star算法和人工勢場法混合算法實驗數據

由路徑規劃結果和實驗數據可以得出：（1）移動機器人能避開障礙得到平滑路徑，行進過程中再出現障礙依舊可以避開，即使遇到無法避免的障礙也能及時停下避免碰撞，算法具有有效性，在完成任務前提下保障安全；（2）改進的A-star算法規劃的路徑相對于傳統的A-star算法較長；（3）改進的A-star算法相較于傳統的 A-star算法具有更平滑的路線，使平均速度增快；（4）改進的A-star算法在U型地形中規劃路徑需要的時間更多。

4 結 論

本文通過系統仿真，搭建了地圖模型、機器人模型、環境地圖，驗證了改進的A*算法和人工勢場法混合算法的路徑規劃的可行性和有效性。通過選取幾種常見的環境地形，并將路徑規劃時間、平均速度、路徑長度、重新規劃次數等實驗數據進行了分析對比，最后得出結論，驗證了改進的A-star算法相對于傳統的A-star算法規劃的路徑更加平滑，能夠適應不同的環境地形，具有良好的魯棒性。改進的A-star算法規劃的平均規劃時間和平均運行時間分別減少了26.67%和23.4%，除此之外，平滑度提升明顯。可以為智能機器人使用改進A*算法和人工勢場法相結合的混合導航算法進行路徑規劃提供一種可靠的技術方案。