針對(duì)多障礙陸戰(zhàn)場(chǎng)路徑規(guī)劃的改進(jìn)A*算法研究

張明路，沈祺宗，高春艷，李滿(mǎn)宏

（河北工業(yè)大學(xué)機(jī)械工程學(xué)院，天津 300130）

1 前言

在多障礙陸戰(zhàn)場(chǎng)環(huán)境中，路徑規(guī)劃[1]即為在存在障礙物的前提下，利用柵格法等方法構(gòu)造地圖，根據(jù)搜索算法（A*[2]，D*[3]，遺傳算法[4]，蟻群算法[5]等）生成從初始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的最優(yōu)路徑。其中A*算法作為靜態(tài)啟發(fā)式算法，更適合處理復(fù)雜障礙，并且它在解決靜態(tài)全局規(guī)劃問(wèn)題時(shí)還具有參數(shù)少、效率高等優(yōu)點(diǎn)。

文獻(xiàn)[6]提出利用微分改進(jìn)A*算法，有效降低了轉(zhuǎn)折次數(shù)，但是涉及了大量計(jì)算；文獻(xiàn)[7]一種改進(jìn)的D*LIte算法—Field D*，對(duì)柵格進(jìn)行線性插值使路徑點(diǎn)不局限于柵格點(diǎn)中心，搜索方向也不再受限于π4整數(shù)倍；文獻(xiàn)[8]則將該線性插值法應(yīng)用于A*中，但是需要對(duì)柵格地圖進(jìn)行定義修改。因此提出一種將A*算法與元胞自動(dòng)機(jī)[9]相結(jié)合的路徑規(guī)劃算法，改進(jìn)估價(jià)函數(shù)使當(dāng)前點(diǎn)可以直連擴(kuò)展Moore的第二層鄰域任意點(diǎn)，然后在依據(jù)通行性判定規(guī)則，繼續(xù)修改估價(jià)函數(shù)，添加多組限制條件，進(jìn)行第二通行次可行性鄰域判定，從而對(duì)路徑的可行性提供優(yōu)化。

該混合方法不但減少了路徑長(zhǎng)度與轉(zhuǎn)向角度，同時(shí)能在復(fù)雜環(huán)境中滿(mǎn)足障礙物回避[10]。

經(jīng)仿真驗(yàn)證，路徑滿(mǎn)足陸戰(zhàn)場(chǎng)行軍靈活性的要求，具有可行性與有效性。

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

2.1 傳統(tǒng)A＊算法

傳統(tǒng)A*算法屬于全局路徑規(guī)劃，利用從初始狀態(tài)和當(dāng)前狀態(tài)到目標(biāo)狀態(tài)估計(jì)所需的費(fèi)用等信息，在選擇下一個(gè)節(jié)點(diǎn)時(shí)對(duì)當(dāng)前結(jié)點(diǎn)距離終點(diǎn)的距離作出估計(jì)。

設(shè)計(jì)估價(jià)函數(shù)是A*算法的核心，本實(shí)驗(yàn)估價(jià)函數(shù)參考?xì)W幾里得距離估價(jià)法，假設(shè)起點(diǎn)S的坐標(biāo)（Hx，Hy），終點(diǎn)G的坐標(biāo)（Mx，My），中間點(diǎn)N的坐標(biāo)（Nx，Ny），則歐幾里得距離表示為：

A*算法在搜索中設(shè)置兩個(gè)表。Open表收錄了已知生成而待遍歷的鄰域點(diǎn)，Close表中收錄了已被遍歷的鄰域點(diǎn)。

若檢查到在Open表中留存重復(fù)節(jié)點(diǎn)，則依照歐幾里得估計(jì)函數(shù)計(jì)算并對(duì)照重整Open表中的搜索節(jié)點(diǎn)，并錄入close表中。最后將保留下來(lái)的節(jié)點(diǎn)排列出來(lái)即為規(guī)劃所得路徑。傳統(tǒng)A*算法流程，如圖1所示。

圖1 傳統(tǒng)A*算法流程圖Fig.1 Traditional A* Algorithm Flowchart

2.2 改進(jìn)估價(jià)函數(shù)

傳統(tǒng)A*算法雖然遍歷效率高，但是首先由于搜索模式為直線搜索，使得傳統(tǒng)A*算法中的最小轉(zhuǎn)角為45°，當(dāng)存在目標(biāo)點(diǎn)與某一路徑點(diǎn)間的相對(duì)角度小于45°時(shí)，路徑會(huì)出現(xiàn)鋸齒式的折疊。為了解決這一問(wèn)題，設(shè)計(jì)將搜索鄰域內(nèi)的偏轉(zhuǎn)角度繼續(xù)縮小，因此參考元胞自動(dòng)機(jī)理論，對(duì)A*算法的搜索領(lǐng)域進(jìn)行改進(jìn)，首先引入擴(kuò)展Moore型鄰域，如圖2所示。其次，由于設(shè)置擴(kuò)展Moore型鄰域存在兩層鄰域點(diǎn)，即內(nèi)層8個(gè)與外層16個(gè)，為了使路徑能夠直接連接最外層鄰域，需要改進(jìn)估價(jià)函數(shù)使其依舊能夠遵循最小代價(jià)排序原則，如圖3所示。

圖2 常見(jiàn)3種元胞鄰域結(jié)構(gòu)Fig.2 Common 3 Cell Neighborhood Structures

圖3 改進(jìn)前后搜索方式對(duì)比Fig.3 Comparison of Search Methods Before and After Improvement

因此需要改進(jìn)A*的估價(jià)函數(shù)，已知路徑點(diǎn)的選擇原則為選擇鄰域中f（x）最小值點(diǎn)，在圖3中，需要使當(dāng)前點(diǎn)A搜索B，C（或C，D）兩點(diǎn)時(shí)，B（或D）點(diǎn)的f（x）小于C點(diǎn)的f（x）。則設(shè)存在一條直線k（x，y）經(jīng)過(guò)目標(biāo)點(diǎn)，分別改變目標(biāo)點(diǎn)的x，y值，使目標(biāo)點(diǎn)存在水平或垂直方向上的平移。設(shè)計(jì)該曲線擬合公式為：

式中：xg,yg—位置目標(biāo)點(diǎn)的坐標(biāo)，A點(diǎn)為起始點(diǎn)。分別計(jì)算B（或D）點(diǎn)與C點(diǎn)的估價(jià)值f（B）與f（C）進(jìn)行對(duì)比，根據(jù)獲取多次曲線擬合圖，如圖4所示。

圖4 系列1（AC）與系列2（AD）曲線擬合Fig.4 Series 1（AC）and Series 2（AD）Curve Fitting

計(jì)算f（C）與f（B）曲線值差，如圖5所示。

圖5 系列1（AC）與系列2（AB）曲線差值Fig.5 Series 1（AC）and Series 2（AB）Curve Difference

由曲線擬合分析可知當(dāng)遍歷點(diǎn)為B或D點(diǎn)時(shí)，均存在一個(gè)固定值t，該值為B點(diǎn)或D點(diǎn)的x，y坐標(biāo)與目標(biāo)點(diǎn)坐標(biāo)的差x1，y1的比值，即t=x1/y1，該值隨B點(diǎn)或D點(diǎn)的選擇而變。

由此，采用逼近原則，多次對(duì)B/D點(diǎn)選擇不同的x1，y1值，計(jì)算其f值與t值，則多次計(jì)算獲得的數(shù)值逼近，如表1所示。

表1 數(shù)值逼近Tab.1 Numerical Approximation

計(jì)算可得t（AD）約為0.6667，t（AB）約為1.4721。下一步則需要將該t值融入到改進(jìn)估價(jià)函數(shù)中。A*算法為啟發(fā)式算法，具有方向性，其遍歷方向一定指向目標(biāo)點(diǎn)。由圖3可知，BFGJ四點(diǎn)均相較于CKLM點(diǎn)發(fā)生了x軸方向位移，DEHI相較CKLM發(fā)生了y軸方向位移。因此均可適用于t（AB）和t（AD）。

根據(jù)曲線擬合結(jié)果，f（B）或F（D）與F（C）的值大小對(duì)比根據(jù)t值的變化而變化。因此在A*算法中添加流程，即每遍歷鄰域內(nèi)的各點(diǎn)時(shí)，計(jì)算該點(diǎn)的t值并于t（AB）或t（AD）進(jìn)行比較，同時(shí)考慮到內(nèi)層可能存在障礙物，如圖3 當(dāng)C，O，N中存在一點(diǎn)為障礙物時(shí)，A點(diǎn)無(wú)法直接連接到B或D點(diǎn)。因此設(shè)置m（x），計(jì)算遍歷點(diǎn)與A點(diǎn)直線經(jīng)過(guò)的障礙數(shù)，設(shè)置n（x），計(jì)算遍歷點(diǎn)與A點(diǎn)的直線距離。

至此歸納改進(jìn)后的A*算法估價(jià)函數(shù)如下：

經(jīng)過(guò)此步改進(jìn)后，A*算法的搜索鄰域?qū)⒊尸F(xiàn)為擴(kuò)展Moore型，并且當(dāng)鄰域內(nèi)無(wú)障礙時(shí)，外層節(jié)點(diǎn)的f（x）將小于內(nèi)層節(jié)點(diǎn)，即滿(mǎn)足A*規(guī)則且實(shí)現(xiàn)路徑跨層連接。

2.3 添加通過(guò)性選擇功判定

在復(fù)雜陸戰(zhàn)場(chǎng)中，部隊(duì)車(chē)輛需要隨時(shí)臨時(shí)調(diào)整，當(dāng)通過(guò)狹隘路段時(shí)，可能出現(xiàn)被狹隘路口阻攔無(wú)法通行及掉頭的問(wèn)題。傳統(tǒng)的A*算法只負(fù)責(zé)計(jì)算路徑，為追求最短路徑而可能規(guī)劃出實(shí)際無(wú)法通行的路徑，而沒(méi)有考慮通過(guò)性問(wèn)題。本實(shí)驗(yàn)考慮在路徑搜索過(guò)程中添加篩選功能。由圖2可見(jiàn)，擴(kuò)展Moore型鄰域具有24個(gè)遍歷鄰域，本實(shí)驗(yàn)假設(shè)部隊(duì)車(chē)輛能夠通過(guò)加上本車(chē)寬度換算到柵格地圖上一共3個(gè)像素寬的路口，因此需要該拓展節(jié)點(diǎn)放入closel‐ist之前附近8個(gè)鄰域必須為無(wú)障礙，即相鄰8個(gè)柵格的值都為0。

因此將通行性判定簡(jiǎn)化為轉(zhuǎn)換為大小為Moore型的車(chē)隊(duì)能夠行駛出擴(kuò)展Moore型的最外層16個(gè)元胞的問(wèn)題。在此歸納代表車(chē)隊(duì)的9個(gè)元胞組成的Moore型數(shù)學(xué)集合為：

式中：Z—整數(shù)集合。歸納擴(kuò)展Moore型外部16元胞數(shù)學(xué)集合為：

根據(jù)問(wèn)題列出集合1能夠從集合2的包圍中正常行駛出的路線，即部分cj需要滿(mǎn)足元胞值為0，分析可行路徑得出cj需要滿(mǎn)足如下條件：

歸納上述集合1集合2與通行條件，構(gòu)造map函數(shù)，須同時(shí)滿(mǎn)足集合1中c1=0與式（6），如式（7）所示。

式中：∑fMoore—以車(chē)輛為中心的Moore型鄰域；f∑proMoore—擴(kuò)展Moore 型外部16 個(gè)元胞。當(dāng)式（7）滿(mǎn)足式（4）與式（6）時(shí)，map（x，y）=0，判定當(dāng)前區(qū)域可以通過(guò)。根據(jù)數(shù)學(xué)條件，建立通行模型，如圖6所示。

圖6 路口通行邏輯模型Fig.6 Intersection Traffic Logic Model

至此，獲取符合通過(guò)性分析的鄰域點(diǎn)，將此點(diǎn)存入closelist，繼續(xù)循環(huán)流程直到完成路徑規(guī)劃。

修改過(guò)后的A*算法整體流程圖，如圖7所示。

圖7 添加通過(guò)性分析后后的A*流程圖Fig.7 A* Flow Chart After Passing Analysis

3 Matlab仿真實(shí)驗(yàn)

為驗(yàn)證A*算法改進(jìn)后的效果，本實(shí)驗(yàn)根據(jù)戰(zhàn)場(chǎng)環(huán)境模擬形成柵格地圖，利用Matlab2018進(jìn)行路徑規(guī)劃仿真。

3.1 擴(kuò)展搜索鄰域驗(yàn)證

改進(jìn)后的A*算法能夠?qū)崿F(xiàn)跨網(wǎng)格的偏轉(zhuǎn)，相較于改進(jìn)前減少了路徑長(zhǎng)度與偏轉(zhuǎn)距離，如圖8所示。

圖8 改進(jìn)前后路徑對(duì)比Fig.8 Path Comparison Before and After Improvement

應(yīng)用于100*100地圖上，改進(jìn)前后A*算法路徑規(guī)劃效果，如圖9、圖10所示。

圖9 改進(jìn)前Fig.9 Before Improvement

圖10 改進(jìn)后Fig.10 After Improvement

根據(jù)上述路徑對(duì)比，獲得數(shù)據(jù)分析結(jié)果，如表2所示。

表2 傳統(tǒng)A＊與改進(jìn)后A＊對(duì)比Tab.2 Comparison of Traditional A＊ and Improved A＊

通過(guò)對(duì)比，經(jīng)過(guò)對(duì)搜索鄰域進(jìn)行改進(jìn)后，路徑平滑度提升，同時(shí)路徑長(zhǎng)度縮短了13.16%，累計(jì)偏折角度減少26.8%。

可見(jiàn)改進(jìn)搜索鄰域后確實(shí)對(duì)算法實(shí)現(xiàn)優(yōu)化。

3.2 通過(guò)選擇性驗(yàn)證

為驗(yàn)證改進(jìn)通過(guò)性選擇分析前后效果，均在地圖右下角設(shè)置一道狹隘路口，如圖11、圖12所示。

圖11 傳統(tǒng)A*算法Fig.11 Traditional A* Algorithm

圖12 改進(jìn)A*算法Fig.12 Improved A* Algorithm

可見(jiàn)相對(duì)于改進(jìn)前，改進(jìn)后的A*可以繞過(guò)狹隘路段重新規(guī)劃路線，表明改進(jìn)具有可行性。

4 結(jié)論

這里提出的改進(jìn)A*算法搜索鄰域、改進(jìn)估價(jià)函數(shù)與添加通行條件判定函數(shù)，可將搜索鄰域個(gè)數(shù)從單層的8個(gè)變?yōu)殡p層的24個(gè)，最小轉(zhuǎn)交減小為18.3°，能夠有效解決傳統(tǒng)A*算法存在的路徑非最短及轉(zhuǎn)折角度過(guò)大的問(wèn)題，增加的通過(guò)性邏輯判定函數(shù)能夠有效繞過(guò)直徑小于3 的狹隘路段，提高行駛的安全性與應(yīng)變能力，相較于傳統(tǒng)A*算法，更加適應(yīng)實(shí)際道路環(huán)境。