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

（1.澳門科技大學(xué)，澳門 999078；2.珠?？萍紝W(xué)院，廣東 珠海 519041）

0 引 言

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

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

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

本文圍繞自主移動機器人（AMR）在復(fù)雜動態(tài)環(huán)境中的運行問題，分別從地圖構(gòu)建、定位、路徑規(guī)劃三部分，綜合考慮全局路徑規(guī)劃和局部路徑規(guī)劃問題，提出基于雙向A*算法進行路徑規(guī)劃和基于激光SLAM加人工勢場法進行局部路徑規(guī)劃的混合導(dǎo)航算法，以提高機器人自主運行的穩(wěn)定性和實時性。

1 地圖構(gòu)建與機器人定位

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

1.1 環(huán)境建模

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

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

使用Ceres求解器，在一個掃描被插入子圖前，和當(dāng)前的子圖進行局部優(yōu)化。此操作是為了在子圖中找到使插入掃描中所有的掃描點概率和最大的掃描位姿，由此便可轉(zhuǎn)變?yōu)橐粋€非線性最小二乘問題。

Cartographer算法通過稀疏位姿調(diào)整方法來全局優(yōu)化，保存掃描幀插入子圖的位姿，當(dāng)子圖構(gòu)建完畢，使用所有對應(yīng)的掃描幀和子地圖進行閉環(huán)檢測，Cartographer框架如圖1所示。

圖1 Cartographer框架圖

1.2 自主定位

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

AMCL算法思路如下：

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

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

式中為樣本估計概率。

重采樣的計算公式如下：

更新后的粒子集為：

使粒子均滿足：

2 路徑規(guī)劃

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

2.1 A*算法

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

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

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

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

式中x和y為當(dāng)前節(jié)點的水平位置和垂直位置，和為終點的水平位置和垂直位置。

圖2 三種距離圖示

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

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

2.2 傳統(tǒng)A-star算法的不足及改進

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

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

圖3 A-star算法流程圖

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

2.3 解決路徑?jīng)_突的人工勢場法

在實際運行中，沿著規(guī)劃路徑前進的機器人會遇到各種動態(tài)的障礙物，形成路徑?jīng)_突，沿著既定路線前進的機器人需要重新規(guī)劃新的路徑，既進行局部路徑規(guī)劃。人工勢場法結(jié)構(gòu)簡單，廣泛應(yīng)用于機器人躲避障礙的局部路徑規(guī)劃。

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

目標(biāo)位置與移動機器人之間引力場為：

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

圖5 人工勢場法示意圖

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

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

機器人所受合力大小為：

3 混合算法的設(shè)計及實現(xiàn)

在復(fù)雜動態(tài)動態(tài)環(huán)境路徑規(guī)劃中，采取雙向A*加人工勢場法的混合算法的機器人更具有優(yōu)勢，如圖6所示，混合算法的實現(xiàn)流程是：

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

（2）機器人在向目標(biāo)節(jié)點運行過程中使用激光雷達等傳感器對行進路徑進行搜索，當(dāng)發(fā)現(xiàn)障礙物存在時，啟動人工勢場算法進行局部路徑規(guī)劃。

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

圖6 混合算法流程圖

3.1 仿真環(huán)境搭建

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

表1 仿真環(huán)境配置

本文利用Gazebo系統(tǒng)仿真軟件，選用Kobuki底盤作為仿真實驗主體，該底盤使用二輪差分驅(qū)動式，具有可擴展性強、易于搭建和配置的優(yōu)點，在ROS系統(tǒng)上具有良好的適配性，Kobuki底盤參數(shù)如表2所示。傳感器使用思嵐A2激光雷達進行機器人周圍環(huán)境搜索和建模，RPLIDAR A2參數(shù)如表3所示?？梢源_保機器人快速度移動時的地圖構(gòu)建質(zhì)量。

表2 Kobuki底盤參數(shù)

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

表3 RPLIDAR A2參數(shù)

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

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

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

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

（4）設(shè)置關(guān)節(jié)類型和發(fā)布TF樹。

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

建好的機器人拓撲關(guān)系如圖7所示。

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

圖7 機器人拓撲關(guān)系圖

圖8 TF變換關(guān)系正視圖

圖9 TF變換關(guān)系仰視圖

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

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

（2）確定環(huán)境中的障礙物的類型、尺寸和數(shù)量，繪制障礙物模型；

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

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

（5）調(diào)整好合適的觀察視角后保存環(huán)境為.world文件，方便后續(xù)調(diào)用。

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

圖10 O形地圖模型

圖11 S形地圖模型

圖12 U形地圖模型

圖13 L形地圖模型

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

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

（2）運行Cartographer算法構(gòu)建地圖的Launch文件；

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

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

（5）使用2D Nav Goal功能發(fā)布目標(biāo)點，使機器人運動完成地圖的構(gòu)建。

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

圖14 O形地圖構(gòu)建

圖15 S形地圖構(gòu)建

圖16 U形地圖構(gòu)建

圖17 L形地圖構(gòu)建

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

（2）運行cartographer_ros包里的cartographer_pbstream_to_ros_map將pbstream轉(zhuǎn)換為pgm和yaml以便后續(xù)使用。

3.2 算法驗證與分析

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

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

圖18 查看ROS路徑規(guī)劃可用插件

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

（3）在地圖模型中加入障礙物，模擬現(xiàn)實環(huán)境中的障礙。

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

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

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

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

圖19 O形地圖路徑規(guī)劃

圖20 S形地圖路徑規(guī)劃

圖21 U形地圖路徑規(guī)劃

圖22 L形地圖路徑規(guī)劃

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

表4 A-star算法實驗數(shù)據(jù)

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

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

4 結(jié) 論

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