基于A*與DWA 算法的混合路徑規(guī)劃算法研究

李森杰,鄭洪瀛,楊 超,武 暢,王紅波

(吉林大學(xué) 電子科學(xué)與工程學(xué)院,長春130012)

0 引 言

隨著機器人已廣泛應(yīng)用于生產(chǎn)生活的各個方面,如智能物料運輸[1],無人駕駛汽車[2]、服務(wù)機器人等,特別是近年來的新冠肺炎導(dǎo)致醫(yī)護(hù)人員緊缺,傳統(tǒng)以人工進(jìn)行醫(yī)療資源配送方式已無法滿足當(dāng)今社會發(fā)展需要,而且病毒還可以通過人與人的接觸進(jìn)行傳播,因此醫(yī)療資源的配送成為目前人們十分關(guān)注的問題。而解決這一問題的關(guān)鍵技術(shù)之一就是針對移動機器人的路徑規(guī)劃[3]。

路徑規(guī)劃[4]是指在周圍環(huán)境存在障礙物的情況下,為控制對象規(guī)劃一條從起點到終點的安全的最優(yōu)路徑[5-6]。路徑規(guī)劃算法目前應(yīng)用較多的有遺傳算法、蟻群算法和A*算法等[7-8]。其中A*算法的核心思想是利用啟發(fā)函數(shù)對算法遍歷的方向進(jìn)行引導(dǎo),避免盲目搜索,提高算法的收斂效率,但該算法不適用于存在動態(tài)障礙物的環(huán)境。動態(tài)窗口算法(DWA:Dynamic Window Approach)可以預(yù)估一定范圍內(nèi)動態(tài)障礙物在一段時間內(nèi)的運動軌跡,規(guī)劃路徑以避開動態(tài)障礙物,但該算法對全局路徑規(guī)劃有維數(shù)爆炸增長特性[9]。

迄今為止,國內(nèi)外學(xué)者在無人駕駛以及移動機器人領(lǐng)域提出了許多關(guān)于路徑規(guī)劃的智能算法,每種算法都在路徑搜索上表現(xiàn)出不同的優(yōu)勢,但沒有任何一種算法適用于任何場景,因此筆者將在利用Gmapping算法進(jìn)行建圖,AMCL(Adaptive Monte Carlo Localization)算法對機器人在地圖中定位后,結(jié)合A*算法和DWA算法實現(xiàn)對路徑的靜態(tài)和動態(tài)規(guī)劃以達(dá)到尋路和躲避動態(tài)障礙物的目的,并分析研究它們在各種不同地形下的路徑規(guī)劃時間和平均尋路速度。

1 建圖與定位

1.1 Gmapping算法

其中dx,dy和dθ分別為對應(yīng)位姿信息的增量,rand n為高斯隨機數(shù);esrr和estr分別為平移誤差對平移誤差和旋轉(zhuǎn)誤差的影響;esrt和estt分別為旋轉(zhuǎn)誤差對平移誤差和旋轉(zhuǎn)誤差的影響;esxy為xy方向上的相互影響,取esxy=0.3 esrr。

Gmapping的結(jié)構(gòu)如圖1所示。

圖1 Gmapping框架圖Fig.1 Gmapping frame diagram

1.2 AMCL算法

AMCL算法即自適應(yīng)蒙特卡洛定位算法,用于實時估計機器人在已知地圖條件下的姿態(tài)信息[13]。當(dāng)前時刻的姿態(tài)信息由后驗信度決定,即

2 路徑規(guī)劃

2.1 A*算法

A*算法核心思想是利用啟發(fā)函數(shù)對算法遍歷的方向進(jìn)行引導(dǎo)[14]。利用評估函數(shù)對網(wǎng)絡(luò)的每個節(jié)點進(jìn)行評估[15],其評估函數(shù)如下

其中F(n)為從起始節(jié)點到目標(biāo)節(jié)點的評估代價,G(n)為特定起始節(jié)點到特定目標(biāo)節(jié)點的評估代價,H(n)為當(dāng)前節(jié)點到目標(biāo)節(jié)點預(yù)測評估代價。對H(n),筆者采用曼哈頓預(yù)估算法

其中D為曼哈頓距離,ab為柵格方塊面積,xn和yn分別為當(dāng)前方塊的水平和垂直位置,xgoal和ygoal分別為目標(biāo)方塊的水平和垂直位置。

A*算法工作原理如圖2所示。

圖2 A*算法流程圖Fig.2 A*algorithm flow chart

A*算法實現(xiàn)需要兩個列表即OpenList和CloseList用于存儲路徑上節(jié)點。OpenList用于存儲待檢查的節(jié)點,其初始化時只包含一個起始節(jié)點,而CloseList用于存儲已檢查過的節(jié)點,CloseList中的節(jié)點不會再被第2次檢查,其初始值為空。A*算法從起始節(jié)點s開始進(jìn)行路徑規(guī)劃,規(guī)劃中產(chǎn)生的節(jié)點存儲于上面的兩個表中。路徑上的每個節(jié)點n都有一個指針指向其父節(jié)點,同時每個節(jié)點n都有與它相關(guān)聯(lián)的估價函數(shù)F(n)的值。

A*算法的每次循環(huán),都會從OpenList中選擇F(n)值最小的節(jié)點n,如果n是目標(biāo)節(jié)點,則循環(huán)結(jié)束,找到了最佳路徑。如果節(jié)點n不是目標(biāo)節(jié)點,則將其從OpenList里取出,存到CloseList中。同時,檢查所有與他相鄰的節(jié)點,除在CloseList中的節(jié)點外,如果節(jié)點不在OpenList中,則將他們存到OpenList中,并將剛存入到CloseList的節(jié)點作為這些新存入節(jié)點的父節(jié)點。通過這種方式進(jìn)行節(jié)點的擴展,最終OpenList上將有一個目標(biāo)節(jié)點,算法終止,然后從終點開始,通過指針向父節(jié)點移動回到起始點,就是A*算法所規(guī)劃的最佳路徑。

2.2 DWA算法

動態(tài)窗口法的核心思想是,在控制空間中對移動機器人每一時刻的速度空間即線速度vt和角速度wt進(jìn)行多組采樣,同時模擬在每個采樣速度下,機器人在時間t內(nèi)可能的運動軌跡以及可能會遇到的障礙,對所有可能的運動軌跡,采用目標(biāo)函數(shù)選取最優(yōu)路徑,再將最優(yōu)路徑所對應(yīng)下的速度(vt,wt),作為該路徑下的速度指令,控制機器人在該路徑下的運動狀態(tài)。速度采樣需滿足以下約束條件[16]

2.3 混合路徑算法

混合路徑規(guī)劃算法實現(xiàn)流程為:先加載全局柵格地圖后,基于A*算法全局路徑規(guī)劃獲取子目標(biāo)點序列,然后確定局部子目標(biāo)點判斷子目標(biāo)點是否被障礙物阻擋,若沒被障礙物阻擋就基于DWA算法進(jìn)行局部路徑規(guī)劃,否則更新全局路徑。DWA局部路徑規(guī)劃后,判斷子目標(biāo)點是否為全局目標(biāo)點,如是則完成路徑規(guī)劃,否則更新局部子目標(biāo)點重復(fù)操作,如圖3所示。

圖3 混合算法流程圖Fig.3 Hybrid algorithm flow chart

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

由于目前仿真軟件功能逐漸完善,已經(jīng)能極大地還原真實情況,為研究人員的開發(fā)與研究節(jié)省了大量時間。因此筆者后續(xù)所有的實驗都將基于仿真環(huán)境完成。所使用的環(huán)境配置如表1所示。

表1 仿真環(huán)境配置Tab.1 Simulation environment configuration

3.1 移動機器人模型建立

筆者選擇Turtlebot2作為仿真實驗主體機器人,選擇Kinect作為地圖圖像獲取傳感器。Turtlebot2是一款二輪差分驅(qū)動式移動機器人,具有體積小、重量輕、易于搭建和配置的優(yōu)點,與ROS系統(tǒng)具有較好的適配性。Kinect是一款雙目RGB-D攝像頭,優(yōu)點是可以獲取信息豐富的三維點云數(shù)據(jù),不僅包含環(huán)境的RGB信息,還包含環(huán)境的深度信息,也可以進(jìn)行降維轉(zhuǎn)換成二維激光數(shù)據(jù)。Turtlebot2和Kinect的具體參數(shù)如表2,表3所示。

表2 Kobuki底盤參數(shù)Tab.2 Kobuki chassis parameters

表3 Kinect傳感器參數(shù)Tab.3 Kinect sensor parameters

根據(jù)上述參數(shù),使用URDF和Xacro構(gòu)建移動機器人模型步驟如下:

1)簡化機器人模型,只保留左右輪、前后承重輪、傳感器、支撐板等關(guān)鍵部件;

2)根據(jù)實際數(shù)據(jù)確定各部件的形狀,并為各個部件設(shè)置物理和碰撞屬性;

3)確定各部件連接的關(guān)節(jié)類型和坐標(biāo)變換關(guān)系;

4)使用URDF和Xacro編寫機器人模型描述文件。

按照上述步驟建立的機器人模型的拓?fù)潢P(guān)系如圖4所示。

圖4 移動機器人模型拓?fù)鋱DFig.4 Topology diagram of AGV model

在Rviz中可對建成的機器人模型的各項參數(shù)進(jìn)行可視化觀察和調(diào)試,并使用joint_state_publisher節(jié)點檢查模型輪子的旋轉(zhuǎn)方向是否正確,機器人模型的TF(Tranform)變換關(guān)系如圖5,圖6所示。

圖5 TF變換關(guān)系正視圖Fig.5 Front view of TF transform

圖6 TF變換關(guān)系俯視圖Fig.6 Vertical view of TF transform

3.2 環(huán)境模型建立

搭建完成機器人模型后,需要使用Gazebo對機器人運行的環(huán)境進(jìn)行搭建。Gazebo為用戶提供了豐富的模型庫,如立方體、圓柱體、桌子和垃圾箱等諸多障礙物模型,極大地提高了用戶搭建環(huán)境的速度。而且Gazebo與ROS系統(tǒng)具有良好的通信接口,可以方便地在ROS上發(fā)布或訂閱話題信息。使用Gazebo搭建環(huán)境模型步驟如下:

1)確定外圍墻壁的形狀和尺寸,繪制外圍墻壁;

2)確定內(nèi)部障礙物類型、形狀和尺寸,繪制內(nèi)部障礙物;

3)確定內(nèi)部障礙物的布局,根據(jù)需要將內(nèi)部障礙物布置在合適位置;

4)使用Gazebo的Building Editor功能繪制墻壁,并在主界面中創(chuàng)建和布置障礙物和機器人模型;

5)保存環(huán)境模型為.world文件,編寫launch文件以方便后續(xù)啟動。

經(jīng)過考察和研究,筆者選取了4種典型的環(huán)境地形進(jìn)行研究,分別是U型、S型、L型和狹長通道地形,建成的環(huán)境模型如圖7~圖10所示,圖中標(biāo)注了模型的尺寸參數(shù)。

圖7 U型地形環(huán)境模型Fig.7 U-shaped Terrain environment model

圖10 狹長通道地形環(huán)境模型Fig.10 Narrow passage terrain environment model

3.3 環(huán)境地圖建立

環(huán)境模型搭建完成后,需要使用SLAM技術(shù)對環(huán)境模型進(jìn)行建圖。筆者采用最為常用和成熟的gmapping算法獲取柵格地圖,并使用Rviz獲取和保存生成的柵格地圖。具體步驟如下:

1)確定gmapping算法的配置參數(shù),編寫launch啟動文件;

圖8 S型地形環(huán)境模型Fig.8 S-shaped terrain environment model

圖9 L型地形環(huán)境模型Fig.9 L-shaped terrain environment model

2)啟動環(huán)境模型的launch文件;

3)啟動gmapping算法的launch文件;

4)啟動turtlebot的keyboard_teleop的launch文件,實現(xiàn)對機器人模型的運動控制;

5)啟動Rviz,訂閱/map、/RobotModel、/LaserScan節(jié)點,對機器人的建圖情況進(jìn)行實時監(jiān)控;

6)使用map_saver節(jié)點保存建成的環(huán)境地圖,以備后續(xù)使用。

上述地形建成后的環(huán)境地圖如圖11~圖14所示。

圖11 U型地形環(huán)境地圖Fig.11 U-shaped terrain environment ma p

圖12 S型地形環(huán)境地圖Fig.12 S-shaped terrain environment map

圖13 L型地形環(huán)境地圖Fig.13 L-shaped terrain environment map

圖14 狹長通道地形環(huán)境地圖Fig.14 Narrow passage terrain environment map

由于Kinect作為RGB-D傳感器,精度不高,可以發(fā)現(xiàn)建成的地圖都出現(xiàn)噪點,但環(huán)境模型的基本要素都已具備,并不影響使用。

4 算法驗證與分析

基于上述準(zhǔn)備,使用A*和DWA的混合算法,驗證在不同地形條件下移動機器人在自主導(dǎo)航過程中的搜索能力。這將用到ROS中的兩個核心功能包amcl和move_base。前者用于實現(xiàn)機器人的位姿估計,后者用于規(guī)劃最優(yōu)路徑。具體的使用步驟如下。

1)確定各項配置參數(shù),編寫配置文件。在本地規(guī)劃器配置文件中聲明使用DWA算法;在全局規(guī)劃器配置文件中聲明使用A*算法。

2)編寫launch啟動文件。內(nèi)容包括啟動各項配置文件、地圖服務(wù)器加載環(huán)境地圖、move_base節(jié)點和amcl節(jié)點以及發(fā)布/odom與/map之間的靜態(tài)坐標(biāo)變換。

3)以環(huán)境模型文件作為輸入,啟動環(huán)境模型的launch啟動文件。

4)以環(huán)境地圖文件作為輸入,啟動自主導(dǎo)航的launch啟動文件。

5)啟動Rviz,訂閱map、/RobotModel、/Path等節(jié)點。先使用2D Pose Estimate功能使機器人對自身位姿進(jìn)行大致估計,再使用2D Nav Goal功能進(jìn)行簡單的導(dǎo)航,使機器人逐漸對自身位姿有準(zhǔn)確的估計。

6)編寫并運行Python腳本,使移動機器人在選定的起始點和目標(biāo)點之間反復(fù)運動,并輸出其路程長度和尋路時間等數(shù)據(jù)。

上述地形規(guī)劃出的全局路徑如圖15~圖18所示。

圖15 U型地形路徑規(guī)劃Fig.15 U-shaped terrain path planning

圖16 S型地形路徑規(guī)劃Fig.16 S-shaped terrain path planning

圖17 L型地形路徑規(guī)劃Fig.17 L-shaped terrain path planning

圖18 狹長通道地形路徑規(guī)劃Fig.18 Narrow passage terrain path planning

通過以上實驗結(jié)果圖可發(fā)現(xiàn),移動機器人成功地在這4種地形下規(guī)劃出了最優(yōu)路徑,但這并不一定是機器人最終實際的運行軌跡,由于配置了DWA算法作為局部路徑規(guī)劃器,在機器人運動的過程中,傳感器仍然會不斷采集數(shù)據(jù),生成局部代價地圖修正原先的路徑以避免碰撞。

因此,筆者將A*和DWA混合算法與單獨的A*算法在不同地形中進(jìn)行比較,以研究該混合算法在不同場景下的普適性,并驗證混合算法是否較單一算法有更好的性能。筆者進(jìn)行兩組實驗,并多次測量求取平均值,最終將所獲得的實驗數(shù)據(jù)匯總成表格如表4,表5所示。

表4 A*算法實驗數(shù)據(jù)Tab.4 Experimental data of A*algorithm

表5 A*算法和DWA混合算法實驗數(shù)據(jù)Tab.5 Experimental data of A*algorithm and DWA hybrid algorithm

通過以上的路徑規(guī)劃結(jié)果和實驗數(shù)據(jù)可以發(fā)現(xiàn):1)移動機器人能避開障礙得到平滑路徑,行進(jìn)過程中再出現(xiàn)障礙依舊可以避開,即使遇到無法避免的障礙也能及時停下避免碰撞,算法具有有效性,在完成任務(wù)前提下保障安全;2)A*和DWA混合算法相較于單獨的A*算法具有更短的路徑長度,使平均速度增快;3)A*和DWA混合算法的機器人在L型地形下尋路情況最佳,平均尋路速度很快,在U型地形和S型地形下則速度一般。

5 結(jié) 語

筆者基于ROS仿真環(huán)境,搭建了機器人模型、傳感器模型、環(huán)境模型、環(huán)境地圖進(jìn)行A*與DWA算法的混合路徑規(guī)劃研究。實驗選取了U型、S型、L型、狹長通道地形4種常見的地形,統(tǒng)計分析了路徑規(guī)劃時間、尋路時間、路徑長度、平均速度4個方面的數(shù)據(jù)。最終,基于實驗數(shù)據(jù)可以發(fā)現(xiàn),A*和DWA混合算法相較于單獨的A*算法具有更加優(yōu)良的性能,且4種地形在最初的路徑規(guī)劃時間上相差不大,但在尋路過程中,L型地形的尋路速度最快,U型地形和S型地形則速度較慢,原因是在尖銳拐角處A*與DWA的混合算法迭代次數(shù)多計算量大,未來可以通過優(yōu)化DWA算法的評價函數(shù)改進(jìn)這一問題。通過該實驗可以為實際情況下移動機器人使用A*與DWA算法進(jìn)行混合路徑規(guī)劃的選擇提供一個可靠的思路和方向。