基于圖論節(jié)點(diǎn)的改進(jìn)A-star柵格路徑規(guī)劃算法

趙玉玲，邵騰戊，蘇德國(guó)

（1.河北工程大學(xué)，河北 邯鄲 056000；2.中國(guó)測(cè)繪科學(xué)研究院，北京 100036）

0 引言

室內(nèi)導(dǎo)航的關(guān)鍵在于路徑規(guī)劃，基于柵格地圖的路徑規(guī)劃算法，如Dijkstra［1］、A-star［2］、JPS（Jump Point Search）［3］等算法，它們引入了啟發(fā)函數(shù)，依據(jù)節(jié)點(diǎn)位置信息優(yōu)先搜索代價(jià)較小的節(jié)點(diǎn)，但仍需遍歷大量無(wú)用的柵格點(diǎn)，導(dǎo)致算法效率下降。基于圖論最短問(wèn)題［4］的路徑規(guī)劃算法，節(jié)點(diǎn)數(shù)量較少，但是節(jié)點(diǎn)不包含位置信息，無(wú)法使用啟發(fā)函數(shù)來(lái)決定優(yōu)先遍歷節(jié)點(diǎn)。本文結(jié)合A-star 算法與基于圖論問(wèn)題的路徑規(guī)劃算法形成一種改進(jìn)A-star 路徑規(guī)劃算法，減少柵格遍歷節(jié)點(diǎn)，且利用節(jié)點(diǎn)位置信息構(gòu)建啟發(fā)函數(shù)，從而提高算法效率。

圖論最短問(wèn)題中節(jié)點(diǎn)的構(gòu)建是必要的環(huán)節(jié)，在本文改進(jìn)A-star 算法中需要構(gòu)建道路特殊節(jié)點(diǎn)，Candeloro M［5］等人利用三角剖分思想，將河道空間分為多個(gè)三角形，生成初始路徑，在此基礎(chǔ)上進(jìn)行路徑規(guī)劃。Liu L 等人［6］采用抽象方法，將門抽象成節(jié)點(diǎn)，從而進(jìn)行路徑規(guī)劃。目前對(duì)室內(nèi)路網(wǎng)特殊節(jié)點(diǎn)提取普遍基于矢量地圖，且為手動(dòng)提取，缺少針對(duì)柵格地圖的節(jié)點(diǎn)提取方法，本文提出一種提取室內(nèi)特殊節(jié)點(diǎn)的卷積核，將室內(nèi)墻角、門、走廊、窗、拐角等節(jié)點(diǎn)自動(dòng)提取出來(lái)，把信息加入到地圖中，以便后續(xù)Astar 算法在遍歷柵格時(shí)，選取特殊節(jié)點(diǎn)作為遍歷對(duì)象，從而降低無(wú)用柵格點(diǎn)數(shù)量，提升算法效率。

1 改進(jìn)A-star算法

基于柵格地圖的路徑規(guī)劃已經(jīng)較為成熟，但受限于柵格地圖形式，路徑規(guī)劃有以下缺點(diǎn)：柵格地圖精度受限于柵格分辨率大小，如果柵格分辨率過(guò)大，則無(wú)法顯示室內(nèi)地圖細(xì)節(jié)；若柵格分辨率過(guò)小，則在路徑規(guī)劃時(shí)，需要計(jì)算大量無(wú)用的柵格，導(dǎo)致算法效率降低。圖論路徑規(guī)劃是將地圖表示為一個(gè)圖，將地圖中某些特定點(diǎn)抽象為節(jié)點(diǎn)，邊表示節(jié)點(diǎn)之間的權(quán)重信息，在圖上進(jìn)行路徑規(guī)劃，算法在選取下一個(gè)遍歷的柵格點(diǎn)時(shí)，只針對(duì)節(jié)點(diǎn)進(jìn)行擴(kuò)展，如墻角、門、走廊拐點(diǎn)等，可以跳過(guò)大量不必要的柵格點(diǎn)。

傳統(tǒng)柵格地圖路徑規(guī)劃算法在擴(kuò)展節(jié)點(diǎn)的時(shí)候，會(huì)向周圍8個(gè)方向擴(kuò)展，每次擴(kuò)展需要判斷每個(gè)節(jié)點(diǎn)是否為障礙物，這消耗整個(gè)算法一半以上的時(shí)間。A-star 算法加入了啟發(fā)函數(shù)，在遍歷節(jié)點(diǎn)時(shí)優(yōu)先朝著終點(diǎn)擴(kuò)展，公式（1）為估價(jià)函數(shù)，f(n)為從起點(diǎn)經(jīng)過(guò)節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)d的估計(jì)成本，g(n)為從起點(diǎn)到當(dāng)前節(jié)點(diǎn)的實(shí)際成本，h(n)為啟發(fā)函數(shù)，表示節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)成本。

但是A-star 還是會(huì)遍歷大量無(wú)用的柵格節(jié)點(diǎn)，且需要計(jì)算所有已遍歷柵格的估價(jià)函數(shù)，導(dǎo)致算法效率降低。因此本文提出了一種結(jié)合A-star 與圖論最短問(wèn)題的算法，將室內(nèi)柵格路網(wǎng)的特殊節(jié)點(diǎn)提取出來(lái)，加入A-star 算法中，用來(lái)減少需要計(jì)算的柵格點(diǎn)數(shù)量。

本文改進(jìn)后的A-star 算法，首先構(gòu)建室內(nèi)特殊節(jié)點(diǎn)，之后構(gòu)建節(jié)點(diǎn)可視性信息并存入到鏈表中。在A-star 算法擴(kuò)展節(jié)點(diǎn)時(shí)，查詢鏈表中與當(dāng)前節(jié)點(diǎn)可視的特殊節(jié)點(diǎn)，設(shè)定啟發(fā)函數(shù)，向可視節(jié)點(diǎn)擴(kuò)展，避免計(jì)算不必要柵格節(jié)點(diǎn)的估計(jì)函數(shù)，且不需再去判斷是否為障礙物，提高了算法效率。鏈表中存放各個(gè)節(jié)點(diǎn)以及節(jié)點(diǎn)之間可視性，相當(dāng)于圖論問(wèn)題中的節(jié)點(diǎn)與邊，因此需要構(gòu)建出節(jié)點(diǎn)鏈表。改進(jìn)Astar算法見(jiàn)圖1。

圖1 改進(jìn)A-star算法圖

1.1 節(jié)點(diǎn)與邊構(gòu)建

室內(nèi)特殊節(jié)點(diǎn)的選取需要覆蓋整個(gè)柵格地圖，且在室內(nèi)任意位置都至少有一個(gè)特殊節(jié)點(diǎn)可視。室內(nèi)門、道路拐點(diǎn)、墻角點(diǎn)這種特殊的節(jié)點(diǎn)都存在于墻體的端點(diǎn)處，且無(wú)論處于室內(nèi)何處，都可以直接通向這些特殊的節(jié)點(diǎn)。因此設(shè)計(jì)四種卷積核（圖2）用來(lái)提取室內(nèi)特殊節(jié)點(diǎn)。

圖2 四種卷積核

室內(nèi)特殊節(jié)點(diǎn)選取規(guī)則為，在柵格地圖上從左上角開(kāi)始卷積，設(shè)定卷積核步長(zhǎng)為1，卷積規(guī)則為每個(gè)像素點(diǎn)與卷積核對(duì)應(yīng)位置相乘，若結(jié)果與卷積核構(gòu)造一致，則將地圖特殊位置柵格點(diǎn)標(biāo)記為節(jié)點(diǎn)。例如使用第一種卷積核卷積，圖3（a）中綠色卷積核所處位置滿足卷積規(guī)則，則藍(lán)色節(jié)點(diǎn)選取為特殊節(jié)點(diǎn)；紅色卷積核不滿足卷積規(guī)則，則略過(guò)。室內(nèi)局部結(jié)果如下圖3（b），綠色為室內(nèi)隨機(jī)節(jié)點(diǎn)，藍(lán)色節(jié)點(diǎn)為卷積核尋找到的特殊節(jié)點(diǎn)。

圖3 選取特殊節(jié)點(diǎn)

根據(jù)上述卷積規(guī)則，將室內(nèi)柵格地圖從左上開(kāi)始掃描得到如下結(jié)果，圖4（a）為二值化柵格地圖，圖4（b）為特殊節(jié)點(diǎn)提取掃描結(jié)果，白色表示墻體，紅色表示特殊節(jié)點(diǎn)，進(jìn)而可以將柵格地圖從356x300分辨率抽象成2328個(gè)特殊節(jié)點(diǎn)。

圖4 特殊節(jié)點(diǎn)選取結(jié)果

在本文改進(jìn)A-star 算法進(jìn)行每一次節(jié)點(diǎn)擴(kuò)展時(shí)，需要判斷與當(dāng)前節(jié)點(diǎn)可視的下一個(gè)節(jié)點(diǎn)，因此需要構(gòu)建節(jié)點(diǎn)可視性。利用全路徑柵格化可以得到兩個(gè)節(jié)點(diǎn)經(jīng)過(guò)的柵格點(diǎn)坐標(biāo)，若柵格化之后的柵格坐標(biāo)中存在障礙物，則兩個(gè)節(jié)點(diǎn)不可視，若不存在則可視。如圖5（a），紅色方框表示節(jié)點(diǎn)5到節(jié)點(diǎn)8的全路徑柵格化，它的柵格化路徑中包含障礙物，則兩個(gè)節(jié)點(diǎn)之間不可視；圖5（b）中，紅色表示節(jié)點(diǎn)1到節(jié)點(diǎn)5、綠色節(jié)點(diǎn)到節(jié)點(diǎn)6 的全路徑柵格化，之間不存在障礙物，因此節(jié)點(diǎn)之間可視。

圖5 節(jié)點(diǎn)可視性

圖6 改進(jìn)A-star算法流程

根據(jù)可視化規(guī)則將二值化地圖中可視節(jié)點(diǎn)提取出來(lái)，存放入鏈表中，以供后續(xù)改進(jìn)A-star算法在擴(kuò)展節(jié)點(diǎn)時(shí)，選取可視的特殊節(jié)點(diǎn)。

1.2 圖論與A-star結(jié)合算法

原有A-star 算法在擴(kuò)展結(jié)點(diǎn)時(shí)，會(huì)向周圍8 個(gè)方向搜索，之后根據(jù)估計(jì)函數(shù)計(jì)算下一次優(yōu)先遍歷的節(jié)點(diǎn)，但是會(huì)計(jì)算大量不必要柵格的估價(jià)函數(shù)，使得算法效率降低。在得到室內(nèi)柵格路網(wǎng)特殊節(jié)點(diǎn)之后，改進(jìn)后的A-star算法在擴(kuò)展節(jié)點(diǎn)時(shí)，先從鏈表中獲取與當(dāng)前節(jié)點(diǎn)可視的節(jié)點(diǎn)，并計(jì)算每個(gè)節(jié)點(diǎn)的估價(jià)函數(shù)，之后再判斷下一次優(yōu)先遍歷的節(jié)點(diǎn)，只針對(duì)特殊節(jié)點(diǎn)進(jìn)行擴(kuò)展，從而大大減少遍歷柵格數(shù)量，提高算法效率。本文改進(jìn)后的A-star 算法流程圖如下。

改進(jìn)后的A-star具體步驟如下。

1）創(chuàng)建用于存放已遍歷節(jié)點(diǎn)的Open 表和存放未遍歷節(jié)點(diǎn)的Closed 表，并向Open 表加入起始節(jié)點(diǎn)。

2）重復(fù)以下操作：查詢Open表中代價(jià)最小的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn)，存入Closed表。

3）根據(jù)當(dāng)前節(jié)點(diǎn)坐標(biāo)，查詢鏈表中與之可視的節(jié)點(diǎn)，判斷是否為終點(diǎn)。若為終點(diǎn)則結(jié)束；若不為終點(diǎn)，判斷是否在Closed 表中。若存在則進(jìn)行下一個(gè)節(jié)點(diǎn)；若不存在，則計(jì)算當(dāng)前節(jié)點(diǎn)估價(jià)函數(shù)并加入到Open表中。

4）保存路徑，依據(jù)Closed 表，目標(biāo)節(jié)點(diǎn)沿著父節(jié)點(diǎn)追溯到起點(diǎn)，便可以生成起點(diǎn)到由終點(diǎn)的路徑規(guī)劃。

2 仿真實(shí)驗(yàn)

為了驗(yàn)證改進(jìn)后的A-star 算法效果，進(jìn)行對(duì)比試驗(yàn)，實(shí)驗(yàn)環(huán)境為AMD5800H處理器、16G內(nèi)存、Python3.6。對(duì)比原有A-star 算法、JPS 跳點(diǎn)算法、改進(jìn)后的A-star 算法在300×356 分辨率地圖中，相同起點(diǎn)和終點(diǎn)的條件下，路徑規(guī)劃結(jié)果、算法遍歷的節(jié)點(diǎn)數(shù)以及所消耗的時(shí)間，如圖7-圖9所示。

圖7 A-star算法

仿真實(shí)驗(yàn)中，起點(diǎn)終點(diǎn)坐標(biāo)統(tǒng)一為（200，68）、（92，134），紅色線段為最短路徑。圖7 為A-star 算法，藍(lán)色節(jié)點(diǎn)為所有遍歷的節(jié)點(diǎn)，遍歷節(jié)點(diǎn)數(shù)為6078 個(gè)，基本遍布了路徑周圍節(jié)點(diǎn)，算法消耗總時(shí)間為0.72s。圖8為JPS算法，JPS算法為目前在高數(shù)量級(jí)柵格節(jié)點(diǎn)地圖中最快的廣度優(yōu)先算法，引入了強(qiáng)迫鄰居及跳點(diǎn)，只需用計(jì)算感興趣的柵格點(diǎn)的估價(jià)函數(shù)，從而加快算法效率，圖中綠色為遍歷的跳點(diǎn)節(jié)點(diǎn)，數(shù)量為220個(gè)，時(shí)間為0.76s。圖9為A-star改進(jìn)算法，遍歷節(jié)點(diǎn)總數(shù)為57 個(gè)，時(shí)間為0.15s，改進(jìn)后的A-star 效率不受柵格分辨率、數(shù)量的影響，只與特殊節(jié)點(diǎn)數(shù)量有關(guān)。實(shí)驗(yàn)結(jié)果如表1所示。

表1 實(shí)驗(yàn)結(jié)果

圖8 JPS算法

圖9 改進(jìn)A-star算法

實(shí)驗(yàn)結(jié)果表明，改進(jìn)后的A-star相比于原有Astar、JPS 時(shí)間效率有明顯的提升，較原有A-star 算法，所遍歷節(jié)點(diǎn)減少99.4%，時(shí)間消耗減少79.2%，且全局特殊節(jié)點(diǎn)數(shù)量與柵格分辨率、數(shù)量無(wú)關(guān)，意味著算法效率與柵格地圖柵格屬性無(wú)關(guān)，證明改進(jìn)方法可行。

3 結(jié)語(yǔ)

本文提出一種A-star算法與圖論最短問(wèn)題結(jié)合的路徑規(guī)劃算法，消除了柵格地圖分辨率對(duì)路徑規(guī)劃算法的影響，避免了在柵格地圖路徑規(guī)劃中遍歷大量不必要的節(jié)點(diǎn)，使得算法時(shí)間較A-star 提升79.2%。經(jīng)實(shí)驗(yàn)驗(yàn)證，該算法在室內(nèi)導(dǎo)航中具有一定實(shí)際意義。改進(jìn)后的A-star占用內(nèi)存小、速度快，期望將來(lái)在多目標(biāo)同時(shí)導(dǎo)航上發(fā)揮更大的作用。