王世元，李季睿，陶寧果

（江漢大學(xué) 智能制造學(xué)院，湖北武漢，430056）

1 傳統(tǒng)的路徑規(guī)劃算法

1.1 路徑規(guī)劃算法的原理

路徑規(guī)劃主要包括局部規(guī)劃和全局規(guī)劃兩大類，全局路徑規(guī)劃模塊根據(jù)評價(jià)指標(biāo)搜索得到一條最優(yōu)路徑，局部路徑規(guī)劃負(fù)責(zé)完成路徑的跟蹤和局部自主避障等任務(wù)，保證機(jī)器人安全移動(dòng)至目標(biāo)點(diǎn)[1]。全局路徑規(guī)劃的評價(jià)指標(biāo)通常是與距離相關(guān)的估價(jià)函數(shù)，也就是最終搜索到的路徑要距離最優(yōu)，而在復(fù)雜的地圖環(huán)境中，我們無法直接判斷起始點(diǎn)與目標(biāo)點(diǎn)之間是否存在有效路徑，因此，從起始點(diǎn)開始，不斷選取周圍鄰域節(jié)點(diǎn)，根據(jù)估價(jià)函數(shù)選取距離最小的節(jié)點(diǎn)進(jìn)行擴(kuò)展，直到搜索到目標(biāo)點(diǎn)，這樣就得到一條距離代價(jià)小的路徑。

1.2 A*算法與Dijkstra 算法

A*算法是一種在靜態(tài)環(huán)境中尋找最優(yōu)路徑的高效算法，是將啟發(fā)式方法和常規(guī)方法相結(jié)合提出的一種算法[2]，其決定代價(jià)指標(biāo)的估價(jià)函數(shù)為：

g(s)是小車當(dāng)前位置與起始點(diǎn)的真實(shí)代價(jià)，用來反映小車當(dāng)前位置與起始點(diǎn)之間的距離，h(s)是小車當(dāng)前位置與目標(biāo)點(diǎn)之間的估計(jì)代價(jià)，用來反映小車當(dāng)前位置與目標(biāo)點(diǎn)之間的距離，因此，f(s)就能比較準(zhǔn)確的反映出起始點(diǎn)與目標(biāo)點(diǎn)之間的距離。

Dijkstra 算法則不考慮h(s)時(shí)，即h(s)為0，此時(shí)算法會(huì)從起始點(diǎn)向四周沒有方向性的擴(kuò)散，需花費(fèi)大量時(shí)間，進(jìn)行較大范圍的搜索。

正是由于h(s)的存在，讓起始點(diǎn)與目標(biāo)點(diǎn)之間有了聯(lián)系，A*算法的搜索才更具方向性，搜索的時(shí)間和范圍大大減小。

2 A*算法的改進(jìn)與MATLAB 驗(yàn)證

2.1 A*算法的改進(jìn)

傳統(tǒng)A*算法進(jìn)行路徑規(guī)劃時(shí)，每次只搜索中心節(jié)點(diǎn)臨近的8 個(gè)節(jié)點(diǎn)，也就是由中心節(jié)點(diǎn)向外拓展一層，運(yùn)動(dòng)方向只能是45°的整數(shù)倍。將傳統(tǒng)A*算法的每個(gè)中心節(jié)點(diǎn)擴(kuò)展鄰域由8 個(gè)增加至24 個(gè)，也就是由中心節(jié)點(diǎn)向外擴(kuò)展兩層，改進(jìn)成24 鄰域的A*算法[3]。

2.2 改進(jìn)算法在MATLAB 中的驗(yàn)證

在MATLAB2016b 的實(shí)驗(yàn)環(huán)境條件下，應(yīng)用A*算法對移動(dòng)機(jī)器人進(jìn)行3 組路徑規(guī)劃仿真實(shí)驗(yàn)。為了精確環(huán)境范圍，仿真環(huán)境設(shè)定為20×20 的柵格地圖，每個(gè)柵格的邊長設(shè)定為1 米，障礙物為模擬環(huán)境。分別采用傳統(tǒng)A*算法與24 鄰域的A*算法在環(huán)境空間已知的柵格地圖上進(jìn)行3 組改變起始點(diǎn)的不同距離長度的路徑規(guī)劃實(shí)驗(yàn)。圖1～3 為3 組不同狀態(tài)下移動(dòng)機(jī)器人分別應(yīng)用A*算法與24 鄰域A*算法路徑規(guī)劃仿真結(jié)果。

圖1 第1 組實(shí)驗(yàn)路徑仿真對比

圖2 第2 組實(shí)驗(yàn)路徑仿真對比

圖3 第3 組實(shí)驗(yàn)路徑仿真對比

3 組實(shí)驗(yàn)的具體數(shù)據(jù)分別在表1 與表2 中體現(xiàn)，其中表1 列舉了傳統(tǒng)A*算法與24 鄰域A*算法3 組實(shí)驗(yàn)的路徑長度的數(shù)據(jù)對比；表2 列舉了傳統(tǒng)A*算法與24 鄰域A*算法在擴(kuò)展點(diǎn)數(shù)與搜索時(shí)間上的具體情況。

表1 傳統(tǒng)A*算法與24鄰域A*算法路徑長度對比

表2 傳統(tǒng)A*算法與24鄰域 A*算法擴(kuò)展節(jié)點(diǎn)與搜索時(shí)間對比

其中，表1 中起止點(diǎn)距離使用歐幾里算法計(jì)算起始點(diǎn)到目標(biāo)點(diǎn)的直線距離（不考慮障礙物），路徑長度是規(guī)劃結(jié)果的實(shí)際長度。從表1 中可以看出24 鄰域的A*算法相較傳統(tǒng)A*算法能規(guī)劃出距離更短的路徑并具有可操作性。

在表2 中，由于24 鄰域A*算法的原理，擴(kuò)大鄰域的同時(shí)必然會(huì)導(dǎo)致擴(kuò)展節(jié)點(diǎn)數(shù)增多，增加節(jié)點(diǎn)數(shù)雖然可以間接解決優(yōu)秀節(jié)點(diǎn)被過早刪除的問題，但是也不可避免的帶來運(yùn)行時(shí)間加長的弊端。

通過對比傳統(tǒng)A*算法與24 鄰域A*算法在同一環(huán)境下的實(shí)驗(yàn)結(jié)果可以看出，傳統(tǒng)A*算法在規(guī)劃路徑的過程中由于受到自身運(yùn)動(dòng)方向的限制導(dǎo)致規(guī)劃路徑轉(zhuǎn)折點(diǎn)過多、路徑不平滑，通過24 鄰域A*算法路徑仿真圖可以看出，改進(jìn)后的算法在路徑選擇上與傳統(tǒng)A*算法存在不同，并且在轉(zhuǎn)折點(diǎn)個(gè)數(shù)和路徑平滑上有明顯好轉(zhuǎn)。

3 移動(dòng)小車及仿真環(huán)境搭建

3.1 系統(tǒng)的軟件平臺(tái)

本研究所設(shè)計(jì)的移動(dòng)機(jī)器人及仿真環(huán)境是基于機(jī)器人操作系統(tǒng)（ROS）來實(shí)現(xiàn)的，ROS 是基于Linux 開發(fā)的一套機(jī)器人通用軟件框架，其集成了大量的工具、庫、協(xié)議，提供類似OS 所提供的功能，可以提升功能模塊的復(fù)用性，簡化對機(jī)器人的控制，使機(jī)器人的相關(guān)研究更加方便。ROS的架構(gòu)圖如圖4 所示，主要分為三個(gè)層次：基于Linux 系統(tǒng)的OS 層，用于實(shí)現(xiàn)不同程序功能包之間的數(shù)據(jù)交流且提供豐富的開發(fā)庫供開發(fā)者使用的中間層，在Master 管理下保證各節(jié)點(diǎn)正常運(yùn)行的應(yīng)用層[4]。

圖4 ROS 架構(gòu)圖

Gazebo 與Rviz 是ROS 中集成的兩個(gè)強(qiáng)大工具，大大提高了機(jī)器人的開發(fā)效率。Gazebo 是一款3D 動(dòng)態(tài)模擬器，提供高保真度的物理模擬，并提供了一整套傳感器模型，用于顯示機(jī)器人模型并創(chuàng)建仿真環(huán)境，能夠在復(fù)雜的室內(nèi)和室外環(huán)境中準(zhǔn)確有效地模擬機(jī)器人。Rviz 是三維可視化工具（ROS Visualization Tool），它的主要目的是以三維方式顯示ROS 消息，可以將數(shù)據(jù)進(jìn)行可視化表達(dá)。

3.2 移動(dòng)小車模型搭建

URDF（united robotics description format）是ROS提供的一個(gè)統(tǒng)一的機(jī)器人描述格式，建立在XML 語法框架下，為機(jī)器人建模仿真[5]。仿真所用到的移動(dòng)小車就是通過URDF 搭建的兩輪差速移動(dòng)小車，在Gazebo 中顯示的小車模型如圖5 所示。

圖5 Gazebo 中顯示的小車模型

該移動(dòng)小車的整體結(jié)構(gòu)如圖6 所示，圖中的矩形框是小車的連桿（Link）部分，橢圓形框是小車中連桿與連桿之間的關(guān)節(jié)（Joint），小車主要包含：小車主體（base_link），起驅(qū)動(dòng)作用的左右驅(qū)動(dòng)輪（left_wheel、right_wheel），起支撐作用的前后萬向輪（front_wheel、back_wheel），激光雷達(dá)（laser_link），單目相機(jī)（camera_link），深度相機(jī)（kinect_link）。

圖6 移動(dòng)小車的結(jié)構(gòu)圖

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

在Gazebo 中搭建邊長為20 米的正方形地圖，并在地圖中的不同位置添加一些墻體作為障礙物，得到兩個(gè)不同的地圖環(huán)境進(jìn)行仿真實(shí)驗(yàn)，Gazebo 中的地圖環(huán)境一如圖7 所示，地圖環(huán)境二如圖8 所示。

圖7 地圖環(huán)境一

圖8 地圖環(huán)境二

3.4 柵格地圖構(gòu)建

控制小車在搭建的物理環(huán)境中移動(dòng)，通過Carto grapher SLAM 算法[6]，利用激光雷達(dá)進(jìn)行珊格地圖的構(gòu)建。得到的柵格地圖分辨率是0.05，即每個(gè)柵格的邊長是5厘米。在ROS 的柵格地圖中，每個(gè)柵格主要攜帶兩個(gè)重要信息：該柵格在地圖中的位置坐標(biāo)，該柵格的狀態(tài)（空閑、被占據(jù)、未知）。兩個(gè)不同的物理地圖環(huán)境構(gòu)建的柵格地圖一和柵格地圖二在Rviz 中顯示，分別如圖9、10 所示。圖中白色部分表示柵格狀態(tài)是空閑的，小車可以安全通過；黑色部分表示柵格狀態(tài)是被占據(jù)的，即有障礙物；灰色部分表示柵格狀態(tài)未知，也就是在建圖過程中激光雷達(dá)無法掃描到的部分。

圖9 柵格地圖一

圖10 柵格地圖二

4 改進(jìn)A*算法在仿真環(huán)境中的應(yīng)用

4.1 改進(jìn)A*算法作為移動(dòng)小車的全局路徑規(guī)劃器

ROS 中集成的move_base 功能包是用來進(jìn)行路徑規(guī)劃并控制機(jī)器人運(yùn)動(dòng)的，將改進(jìn)后的24 鄰域A*算法作為插件的形式插入move_base 中，啟動(dòng)move_base 即可調(diào)用改進(jìn)后的A*算法作為全局路徑規(guī)劃器進(jìn)行路徑規(guī)劃。在ROS 中的24 鄰域A*算法流程圖如圖11 所示。

圖11 24 鄰域Astar 算法流程圖

4.2 傳統(tǒng)A*算法與改進(jìn)A*算法的對比

路徑規(guī)劃算法的優(yōu)劣主要體現(xiàn)在規(guī)劃出的路徑長短、路徑規(guī)劃所耗費(fèi)的時(shí)間、搜索的節(jié)點(diǎn)數(shù)量這3 個(gè)方面，本文將在兩個(gè)不同的地圖環(huán)境中，用傳統(tǒng)A*算法與改進(jìn)A*算法，選取相同的起始點(diǎn)和目標(biāo)點(diǎn)進(jìn)行路徑規(guī)劃，通過這3 個(gè)方面的數(shù)據(jù)進(jìn)行對比。

在柵格地圖一中，傳統(tǒng)A*算法作為移動(dòng)小車的全局規(guī)劃器，規(guī)劃的路徑如圖12 所示，24 鄰域A*算法作為全局規(guī)劃器，規(guī)劃的路徑如圖13 所示。在柵格地圖二中，傳統(tǒng)A*算法作為移動(dòng)小車的全局規(guī)劃器，規(guī)劃的路徑如圖14 所示，24 鄰域A*算法作為全局規(guī)劃器，規(guī)劃的路徑如圖15所示，圖中的綠色路徑即為規(guī)劃的全局路徑。兩種算法的數(shù)據(jù)對比如表3、4 所示。

表3 柵格地圖一中兩種算法對比

圖12 柵格地圖一中傳統(tǒng)Astar算法規(guī)劃路徑

圖13 柵格地圖一中24鄰域Astar算法規(guī)劃路徑

圖14 柵格地圖二中傳統(tǒng)Astar算法規(guī)劃路徑

圖15 柵格地圖二中24 鄰域Astar算法規(guī)劃路徑

根據(jù)圖中的路徑數(shù)據(jù)可得：改進(jìn)后的A*算法相較于傳統(tǒng)的A*算法，減少了轉(zhuǎn)彎的次數(shù)，路徑更加平滑，同時(shí)能更快的搜索到障礙物而提前改變方向，有效的減短了路徑長度。

由圖12、13 結(jié)合表3 中的數(shù)據(jù)可得：在不是很復(fù)雜的環(huán)境中，24 鄰域A*算法規(guī)劃的路徑長度明顯減短，但由于單個(gè)節(jié)點(diǎn)所搜索的鄰域從8 個(gè)擴(kuò)大到了24 個(gè)，搜索的節(jié)點(diǎn)數(shù)也隨之增多，使規(guī)劃時(shí)間變長，減短了2 米左右的距離，大約增加了30 微秒的時(shí)間，這個(gè)代價(jià)完全是可以接受的。

由圖14、15 結(jié)合表4 中數(shù)據(jù)可得：在稍加復(fù)雜的環(huán)境中，改進(jìn)后的算法就有非常明顯的提升，由于24 鄰域A*算法每個(gè)節(jié)點(diǎn)搜索的鄰域擴(kuò)大，便能提前搜索到障礙物，從而提前改變搜索方向，因此在地圖二中的路徑長度減少了6米左右，規(guī)劃時(shí)間并沒有增加，反而有2 微秒左右的減少。

表4 柵格地圖2中兩種算法對比

5 結(jié)束語

本文主要對A*算法的搜素鄰域的各方向上擴(kuò)大一個(gè)柵格，將原來的8 鄰域變?yōu)?4 鄰域，通過MATLAB 進(jìn)行思路驗(yàn)證，并將算法用C++代碼重寫，通過ROS 應(yīng)用到移動(dòng)小車上，規(guī)劃出移動(dòng)小車在仿真的物理地圖環(huán)境中的路徑，讓小車運(yùn)動(dòng)，得到算法在不同環(huán)境運(yùn)行的實(shí)際效果。本文使用了一個(gè)稍簡單的地圖環(huán)境和一個(gè)稍復(fù)雜的地圖環(huán)境對算法進(jìn)行測試，改進(jìn)后的算法相較于傳統(tǒng)的A*算法都有優(yōu)勢，隨著環(huán)境的復(fù)雜度提高，改進(jìn)后的算法優(yōu)勢也將越來越明顯。