基于可搜索24鄰域的A*算法路徑規(guī)劃*

崔寶俠， 王淼弛， 段 勇

(沈陽工業(yè)大學(xué) 信息科學(xué)與工程學(xué)院， 沈陽 110870)

路徑規(guī)劃是機(jī)器人學(xué)的基本問題，其目的是在構(gòu)型空間中求解一個(gè)特定序列，使得機(jī)器人自起點(diǎn)開始，避開不可達(dá)和存在碰撞風(fēng)險(xiǎn)的構(gòu)型，最終到達(dá)目標(biāo)構(gòu)型[1].國內(nèi)外大量學(xué)者均積極對(duì)其展開研究，同時(shí)提出了很多的規(guī)劃策略與相關(guān)計(jì)算方法，例如遺傳算法、蟻群算法、神經(jīng)網(wǎng)絡(luò)算法等都是常用的計(jì)算方法，這些方法對(duì)具有一定代價(jià)要求的路徑規(guī)劃都能夠在某種程度上完成.

柵格法[2]是機(jī)器人對(duì)環(huán)境建模的主要方法之一，以柵格路徑規(guī)劃為基礎(chǔ)的算法在目前有很多，例如A*、D*[3]、D*Lite[4]、FocussedD*[5]及其他方法.其中，D*、D*Lite、FocussedD*側(cè)重于對(duì)環(huán)境動(dòng)態(tài)變化引起的行駛代價(jià)變化問題的處理；而在已知的靜態(tài)環(huán)境下，A*算法可以對(duì)兩點(diǎn)間最短路徑[6]快速計(jì)算.

但是基于柵格的A*路徑規(guī)劃算法存在一個(gè)問題，其立足于柵格的中心點(diǎn)，在狀態(tài)空間中作為節(jié)點(diǎn)是能夠被搜索的，也就是說它能夠?qū)?個(gè)鄰域搜索，但路徑搜索方向就會(huì)產(chǎn)生限定，即均屬于π/4的整數(shù)倍.這樣求解最終產(chǎn)生的路徑實(shí)際上不是最優(yōu)的，就路徑長(zhǎng)度而言也并非屬于最短的，存在大量的轉(zhuǎn)彎及大量的折點(diǎn)[7-8].

結(jié)合A*路徑規(guī)劃算法的缺點(diǎn)，本文對(duì)傳統(tǒng)A*算法的搜索鄰域進(jìn)行了擴(kuò)展，即從原來離散的8個(gè)增加為現(xiàn)在的24個(gè)，搜索方向也有了一定的改變，由限定方向轉(zhuǎn)變?yōu)檫B續(xù)任意方向.改進(jìn)的A*算法成功縮短了路徑長(zhǎng)度，減小了折點(diǎn)個(gè)數(shù)，實(shí)驗(yàn)證明該方法能夠在柵格地圖上求解出更優(yōu)的路徑，移動(dòng)機(jī)器人路徑規(guī)劃具有了更高效率.

1 A*算法移動(dòng)機(jī)器人路徑規(guī)劃

1.1 A*算法

A*算法[9]從本質(zhì)而言，屬于人工智能范疇，具有鮮明啟發(fā)式搜索特征，算法因?yàn)槠鋸?qiáng)大的靈活性以及對(duì)不同路況的適應(yīng)能力，在路徑規(guī)劃搜索中受青睞.A*算法最為成功的特點(diǎn)體現(xiàn)為：它將Dijkstra算法(靠近初始點(diǎn)的節(jié)點(diǎn))和BFS算法(靠近目標(biāo)點(diǎn)的節(jié)點(diǎn))的信息塊結(jié)合起來，將從初始節(jié)點(diǎn)到任意節(jié)點(diǎn)n的代價(jià)g(n)與從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的啟發(fā)式評(píng)估代價(jià)h(n)結(jié)合起來，評(píng)價(jià)當(dāng)前節(jié)點(diǎn)，其評(píng)價(jià)函數(shù)為

f(n)=g(n)+h(n)

式中：f(n)為從初始點(diǎn)經(jīng)由節(jié)點(diǎn)n到目標(biāo)點(diǎn)的估價(jià)函數(shù)；g(n)為狀態(tài)空間中從初始節(jié)點(diǎn)到n節(jié)點(diǎn)的實(shí)際代價(jià)；h(n)為當(dāng)前節(jié)點(diǎn)n至目標(biāo)節(jié)點(diǎn)路徑這一過程中的預(yù)算代價(jià)，它在評(píng)價(jià)函數(shù)中起關(guān)鍵性作用，決定了A*算法效率的高低.

A*算法結(jié)合具體路徑規(guī)劃要達(dá)到的目的，設(shè)計(jì)與之相適應(yīng)的啟發(fā)函數(shù)，從而使搜索方向越來越接近目標(biāo)狀態(tài).

A*算法原理圖如圖1所示，包含開始節(jié)點(diǎn)(start)、障礙物、結(jié)束節(jié)點(diǎn)(end)，需要通過A*算法規(guī)劃從start節(jié)點(diǎn)到end節(jié)點(diǎn)的路線.探尋過程中，會(huì)以當(dāng)前節(jié)點(diǎn)為基點(diǎn)，掃描其相鄰的八個(gè)節(jié)點(diǎn)，計(jì)算當(dāng)前節(jié)點(diǎn)與start節(jié)點(diǎn)及到end的距離，從而計(jì)算出最短路徑.

以start節(jié)點(diǎn)為例，探尋相鄰的八個(gè)節(jié)點(diǎn)，且只能上下左右移動(dòng)，每移動(dòng)到鄰近的單元格，就認(rèn)為行走了一個(gè)距離.如從start移動(dòng)到A節(jié)點(diǎn)，移動(dòng)了兩個(gè)距離，當(dāng)從A節(jié)點(diǎn)移動(dòng)到end節(jié)點(diǎn)(此時(shí)忽略障礙，僅計(jì)算距離)時(shí)，g(n)即可以理解為g(A)=2，h(n)可理解為h(A)=6，f(n)即為start節(jié)點(diǎn)到end節(jié)點(diǎn)的實(shí)際距離，公式描述為f(A)=g(A)+h(A)=8.于是便有了A節(jié)點(diǎn)的數(shù)字描述：f(n)位于左上方，g(n)位于左下方，h(n)位于右下方.同理可計(jì)算出f(B)=g(B)+h(B)=1+5=6.將start相鄰的其它節(jié)點(diǎn)距離都計(jì)算出來，便可以通過對(duì)相鄰節(jié)點(diǎn)的遍歷及父節(jié)點(diǎn)的不斷嵌套來形成一條路徑.如把start相鄰的所有節(jié)點(diǎn)進(jìn)行遍歷比較，選出f(n)最小的一個(gè)節(jié)點(diǎn)(稱為X節(jié)點(diǎn))，并設(shè)置X節(jié)點(diǎn)的父節(jié)點(diǎn)為start節(jié)點(diǎn)，以X節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)，再判斷X節(jié)點(diǎn)周圍的有效節(jié)點(diǎn)(非障礙物點(diǎn))，選出f(n)最小的節(jié)點(diǎn)后設(shè)置X節(jié)點(diǎn)為其父節(jié)點(diǎn)，依次類推，就形成了一條節(jié)點(diǎn)線.

圖1 A*算法原理示意圖Fig.1 Schematic A* algorithm principle

1.2 路徑規(guī)劃算法

A*算法在執(zhí)行路徑規(guī)劃時(shí)主要用兩個(gè)表來實(shí)現(xiàn)節(jié)點(diǎn)的擴(kuò)展和最優(yōu)節(jié)點(diǎn)的選?。篛PEN表和CLOSE表.OPEN表作用是保存搜索過程中遇到的擴(kuò)展節(jié)點(diǎn)，同時(shí)將這些節(jié)點(diǎn)按代價(jià)值進(jìn)行排序；CLOSE表作用是保存OPEN表中代價(jià)值最小的可擴(kuò)展節(jié)點(diǎn).

本文將A*算法應(yīng)用在柵格地圖上進(jìn)行路徑規(guī)劃，啟發(fā)式信息h(n)為當(dāng)前節(jié)點(diǎn)(xn，yn)和目標(biāo)節(jié)點(diǎn)(xT，yT)之間的直線距離，即

A*算法移動(dòng)機(jī)器人路徑規(guī)劃過程如下：

1) 在OPEN表納入起點(diǎn)，CLOSE表納入障礙點(diǎn).

2) 選取OPEN表中具有最小f值的節(jié)點(diǎn)n，將其納入CLOSE表.

3) 判斷n是否為目標(biāo)點(diǎn)，若n是目標(biāo)點(diǎn)，則根據(jù)它的前向指針生成最優(yōu)路徑；若n不是目標(biāo)點(diǎn)，則擴(kuò)展節(jié)點(diǎn)n，生成后繼節(jié)點(diǎn)m.

4) 在OPEN表中建立從后繼節(jié)點(diǎn)m返回到n的指針，計(jì)算f(m)=g(m)+h(m).

5) 增加判斷語句來判斷OPEN表中是否已有節(jié)點(diǎn)m，如果判斷失敗，就應(yīng)在OPEN表納入m；若判斷成功，則比較擁有不同前向指針的f(m)值的大小，選取較小的f(m)值.

6) 更新g(m)，f(m)以及后繼節(jié)點(diǎn)m的前向指針.

7) 按照數(shù)值大小的正序排列，把f值在OPEN表中進(jìn)行重新排序，并返回步驟2).

路徑規(guī)劃的流程圖如圖2所示.

圖2 移動(dòng)機(jī)器人路徑規(guī)劃流程Fig.2 Flow chart of path planning of mobile robot

2 改進(jìn)A*算法

2.1 24鄰域擴(kuò)展

傳統(tǒng)A*算法的8鄰域示例圖如圖3所示.在柵格地圖上使用A*算法進(jìn)行路徑規(guī)劃時(shí)，每個(gè)柵格的中心都被假設(shè)為節(jié)點(diǎn)[10]，節(jié)點(diǎn)臨近的8個(gè)區(qū)域都是這個(gè)節(jié)點(diǎn)的擴(kuò)展個(gè)數(shù)，因此運(yùn)動(dòng)方向的角度也被限定為π/4的整數(shù)倍.傳統(tǒng)A*算法規(guī)劃路徑如圖4所示，圖4中虛線為根據(jù)傳統(tǒng)A*算法所得到的自動(dòng)最佳路徑，路徑有轉(zhuǎn)折點(diǎn)，比較復(fù)雜，并非起終點(diǎn)之間的最佳期望路徑.

為獲得更好的規(guī)劃路徑，本文對(duì)原有的A*算法進(jìn)行了改進(jìn).將傳統(tǒng)A*算法的每個(gè)節(jié)點(diǎn)擴(kuò)展鄰域由8個(gè)增至24個(gè).擴(kuò)展算法的過程為：對(duì)于每個(gè)輸入點(diǎn)的待擴(kuò)展點(diǎn)，檢查它四周的24個(gè)鄰接點(diǎn)(x坐標(biāo)-2～2，y坐標(biāo)-2～2)可否加入擴(kuò)展列表，判斷鄰接點(diǎn)是否還在網(wǎng)格圖范圍內(nèi).如果一個(gè)鄰接點(diǎn)是內(nèi)圈的點(diǎn)，只要檢查鄰接點(diǎn)本身是否為障礙點(diǎn)(是否在CLOSE表里面)，若不是障礙點(diǎn)，則將此鄰接點(diǎn)加入擴(kuò)展列表中；如果一個(gè)鄰接點(diǎn)是外圈的點(diǎn)，除了檢查鄰接點(diǎn)本身是否為障礙點(diǎn)外，還要檢查輸入點(diǎn)到鄰接點(diǎn)路徑中的1～2個(gè)點(diǎn)是否在CLOSE表里，再判斷途經(jīng)點(diǎn)是否在CLOSE表中，如果判斷失敗，則將相關(guān)的鄰接點(diǎn)納入到OPEN表中.

圖3 傳統(tǒng)A*算法8鄰域示意圖Fig.3 Schematic 8 neighborhoods intraditional A* algorithm

圖4 傳統(tǒng)A*算法規(guī)劃路徑Fig.4 Path planning in traditional A* algorithm

2.2 改進(jìn)A*算法路徑規(guī)劃

以A*算法對(duì)障礙物和移動(dòng)路線進(jìn)行預(yù)判計(jì)算，由于采用新的啟發(fā)式信息因素，則在計(jì)算難度和相對(duì)復(fù)雜程度上要小很多，同時(shí)也提高了效率.另外，在啟發(fā)式信息提供階段，在前期的搜索中刪除了一部分節(jié)點(diǎn)，而且存在實(shí)踐性積累和計(jì)算經(jīng)驗(yàn)等方面的困擾，這些節(jié)點(diǎn)的刪除可能會(huì)對(duì)最終結(jié)果產(chǎn)生不良影響.為了防止產(chǎn)生次優(yōu)解，在之前節(jié)點(diǎn)搜索鄰域定義為24鄰域基礎(chǔ)上再進(jìn)行改進(jìn)：選取OPEN表中具有最小f值和次小f值的節(jié)點(diǎn)生成后繼節(jié)點(diǎn).改進(jìn)A*算法路徑規(guī)劃過程如下：

1) 把源節(jié)點(diǎn)納入OPEN表中，且作為唯一節(jié)點(diǎn)，同時(shí)對(duì)CLOSE表進(jìn)行調(diào)整，該表初始情況下只存入障礙點(diǎn)的相關(guān)數(shù)值.

2) 依據(jù)新的擴(kuò)展算法進(jìn)行擴(kuò)展.

3) 在OPEN表里將f值中最小的數(shù)值節(jié)點(diǎn)作為最佳的節(jié)點(diǎn)best1，然后選擇次小的節(jié)點(diǎn)，并定義為best2，兩個(gè)數(shù)值最佳的節(jié)點(diǎn)在擴(kuò)展后由OPEN表移動(dòng)到CLOSE表，用來預(yù)判best1是否為原定目標(biāo).如判斷正確，即可轉(zhuǎn)入步驟4)；如判斷錯(cuò)誤，那么綜合地圖數(shù)據(jù)庫里的道路與節(jié)點(diǎn)數(shù)據(jù)進(jìn)行運(yùn)算，從而對(duì)兩個(gè)最佳節(jié)點(diǎn)進(jìn)行擴(kuò)展運(yùn)算生成后繼節(jié)點(diǎn).

4) 逆向推理判斷，從最佳節(jié)點(diǎn)回溯到原始節(jié)點(diǎn)，完成最終報(bào)告.

3 模擬實(shí)驗(yàn)

本文實(shí)驗(yàn)條件如下：CPUIntel Core2 Duo計(jì)算機(jī)，內(nèi)存2 048 Mbit，所用編輯代碼工具為Matlab 7.0.分別采用A*算法、24鄰域A*算法和44鄰域A*算法在柵格地圖上進(jìn)行5組不同距離長(zhǎng)度的路徑規(guī)劃實(shí)驗(yàn).表1列舉了傳統(tǒng)A*算法、24鄰域A*算法、44鄰域A*算法三種算法5組實(shí)驗(yàn)路徑長(zhǎng)度的數(shù)據(jù)對(duì)比；表2則列舉了三種算法擴(kuò)展點(diǎn)數(shù)與搜索時(shí)間情況；圖5為傳統(tǒng)A*算法與改進(jìn)A*算法仿真路徑對(duì)比的結(jié)果.

表1 規(guī)劃路徑長(zhǎng)度對(duì)比Tab.1 Comparison in length of planned path m

表2 擴(kuò)展點(diǎn)數(shù)和搜索時(shí)間對(duì)比Tab.2 Comparison in extension points and searching time

圖5 傳統(tǒng)A*和改進(jìn)A*算法仿真路徑對(duì)比Fig.5 Comparison in simulated paths between traditionalA* and improved A* algorithms

模擬實(shí)驗(yàn)的數(shù)據(jù)表明：利用傳統(tǒng)A*算法所得的最終結(jié)果存在轉(zhuǎn)折多、線路復(fù)雜的問題，但這些問題在A*改進(jìn)算法中得到了有效改進(jìn).對(duì)比表1、2數(shù)據(jù)可知，24鄰域A*算法與44鄰域A*算法在規(guī)劃路徑長(zhǎng)度上相差無幾，且均優(yōu)于傳統(tǒng)A*算法路徑.但在規(guī)劃時(shí)間上，44鄰域搜索相較于24鄰域搜索耗費(fèi)了更長(zhǎng)的時(shí)間.擴(kuò)展點(diǎn)數(shù)上，改進(jìn)A*算法相較于A*算法有所增多，避免了原算法最優(yōu)節(jié)點(diǎn)過早刪除.綜上所述，24鄰域A*算法路徑規(guī)劃的效率最高.

4 結(jié) 論

本文提出了改進(jìn)A*算法，將可搜索鄰域個(gè)數(shù)從離散的8個(gè)擴(kuò)增為24個(gè)，因此，可移動(dòng)方向也從π/4的整數(shù)倍變?yōu)檫B續(xù)更多的方向.同時(shí)采用了f最小值和次小值兩點(diǎn)同時(shí)擴(kuò)展，能夠有效解決傳統(tǒng)A*算法在柵格地圖上進(jìn)行路徑規(guī)劃時(shí)，求解得到的路徑長(zhǎng)度不是最優(yōu)以及轉(zhuǎn)折點(diǎn)較多的問題.在移動(dòng)機(jī)器人路徑規(guī)劃中，相比于傳統(tǒng)的A*算法，改進(jìn)算法能規(guī)劃出一條更優(yōu)的可行駛路徑，特別是對(duì)于柵格劃分較為粗糙的柵格地圖，優(yōu)化程度更為明顯.

[1] 張浩杰，龔建偉，姜巖，等.基于變?yōu)槎葼顟B(tài)空間的增量啟發(fā)式路徑規(guī)劃方法研究 [J].自動(dòng)化學(xué)報(bào)，2013，39(10)：1602-1610.

(ZHANG Hao-jie，GONG Jian-wei，JIANG Yan，et al.Research on Incremental heuristic path planner with variable dimensional state space [J].Acta Automatic Sinica，2013，39(10)：1602-1610.)

[2] 張繼文，劉莉，陳懇.面向全方位雙足步行跟隨的路徑規(guī)劃 [J].自動(dòng)化學(xué)報(bào)，2016，42(2)：189-201.

(ZHANG Ji-wen，LIU Li，CHEN Ken.Omni-directional bipedal walking path planning [J].Acta Automatic Sinica，2016，42(2)：189-201.)

[3] 張慧，榮學(xué)文，李貽斌，等.四足機(jī)器人地形識(shí)別與路徑規(guī)劃算法 [J].機(jī)器人，2015，37(5)：547-551.

(ZHANG Hui，RONG Xue-wen，LI Yi-bin，et al.Terrain recognition and path planning for quadruped robot [J].Robot，2015，37(5)：547-551.)

[4] 葛延峰，陳濤，孔祥勇，等.改進(jìn)蟻群算法在城市汽車導(dǎo)航中的應(yīng)用 [J].控制工程，2016，23(1)：134-137.

(GE Yan-feng，CHEN Tao，KONG Xiang-yong，et al.Application of improved ant colony algorithm in car nevigation [J].Control Engineering of China，2016，23(1)：134-137.)

[5] 陳長(zhǎng)征，項(xiàng)宏偉，楊孔碩，等.可變形履帶機(jī)器人跨越臺(tái)階的動(dòng)力學(xué)分析 [J].沈陽工業(yè)大學(xué)學(xué)報(bào)，2015，37(2)：166-170.

(CHEN Chang-zheng，XIANG Hong-wei，YANG Kong-shuo，et al.Dynamic analysis for variable tracked robot in process of climbing steps [J].Journal of Shenyang University of Technology，2015，37(2)：166-170.)

[6] 吳宏超，劉檢華，唐承統(tǒng)，等.基于改進(jìn)A*算法的管路自動(dòng)布局設(shè)計(jì)與優(yōu)化方法 [J].計(jì)算機(jī)集成制造系統(tǒng)，2016，22(4)：946-954.

(WU Hong-chao，LIU Jian-hua，TANG Cheng-tong，et al.Automatic pipe layout design and optimization method based on A* algorithm [J].Computer Integrated Manufacturing Systems，2016，22(4)：946-954.)

[7] de Filippis L，Guglieri G，Quagliotti F.Path planning strategies for UAVS in 3D environments [J].Journal of Intelligent & Robotic Systems，2012(65)：247-264.

[8] 王殿君.基于改進(jìn)A*算法的室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃 [J].清華大學(xué)學(xué)報(bào)(自然科學(xué)版)，2012，52(8)：1085-1089.

(WANG Dian-jun.Indoor mobile-robot path planning based on an improved A* algorithm [J].Journal of Tsinghua University(Science and Technology)，2012，52(8)：1085-1089.)

[9] Pehlivaolu Y V.A new vibrational genetic algorit hm enhanced with a voronoi diagram for path planning of autonomous UAV [J].Aerospace Science and Technology，2012(16)：47-55.

[10]Mei T，Liang H，Kong B，et al.Development of intel-ligent pioneer unmanned vehicle[C]//Intelligent Vehicles Symposium.Piscataway，USA，2012：938-943.