吳麟麟，楊俊輝

(江蘇大學(xué) 汽車工程研究院， 江蘇 鎮(zhèn)江 212013)

智能車輛的路徑規(guī)劃是指根據(jù)某一性能指標(biāo)搜索一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或近似最優(yōu)的無(wú)碰路徑[1]。路徑規(guī)劃的傳統(tǒng)方法主要分為兩類[2]：分析法、圖形法。而圖形法又分為路圖法、柵格法等。在搜索算法方面，蟻群算法(GA)[3]、粒子群算法[4]、遺傳算法[5]及其混合算法等眾多主流算法均被運(yùn)用在路徑規(guī)劃中。蟻群算法收斂速度慢，在障礙物包圍情況下易陷入局部最優(yōu)問(wèn)題；粒子群算法計(jì)算效率偏低且抗噪能力較差；遺傳算法實(shí)現(xiàn)比較復(fù)雜，同時(shí)參數(shù)的選擇不夠精確。上述算法均存在計(jì)算效率低、編程復(fù)雜等實(shí)際問(wèn)題，而A*算法作為啟發(fā)式搜索算法的一種，在保留了Dijkstra算法[6]精度高的同時(shí)減少了擴(kuò)展節(jié)點(diǎn)數(shù)目，具有結(jié)構(gòu)簡(jiǎn)單、搜索精度高等優(yōu)點(diǎn)。但在規(guī)劃路徑中A*算法[7]存在擴(kuò)展節(jié)點(diǎn)數(shù)目較多，Weighted A*算法存在路徑精度不佳等問(wèn)題。隨著智能車輛的不斷發(fā)展，環(huán)境地圖日益龐大且復(fù)雜，現(xiàn)有的啟發(fā)式算法[8]無(wú)法滿足搜索耗時(shí)及路徑精度的綜合需求，致使搜索耗時(shí)冗長(zhǎng)或者路徑規(guī)劃精度不佳。針對(duì)上述問(wèn)題，本文在原有Anytime Weighted A*算法(AWA*算法)的基礎(chǔ)上引入動(dòng)態(tài)優(yōu)化因子，建立一種基于擴(kuò)展節(jié)點(diǎn)耗時(shí)的新型動(dòng)態(tài)AWA*算法估價(jià)函數(shù)，重點(diǎn)對(duì)改進(jìn)的AWA*算法與AWA*算法在路徑規(guī)劃效率及規(guī)劃路徑的影響進(jìn)行分析[9]。

1 AWA*算法路徑規(guī)劃

全局路徑規(guī)劃主要分為2個(gè)步驟：首先是建立障礙物及自由空間區(qū)域的環(huán)境地圖，其次是在環(huán)境地圖中選擇合適的搜索算法。

1.1 環(huán)境建模

根據(jù)不同的表示形式，環(huán)境地圖表示方法主要分為度量地圖表示法、拓?fù)涞貓D表示法和混合地圖表示法。

本研究主要運(yùn)用的是度量地圖表示法中的空間分解法。假設(shè)智能車輛的自由空間為矩形，其內(nèi)部分布著多個(gè)靜止障礙物，對(duì)任務(wù)區(qū)域進(jìn)行柵格化，得到如圖1的環(huán)境模型，其中：設(shè)定起點(diǎn)為紅色節(jié)點(diǎn)，終點(diǎn)為藍(lán)色節(jié)點(diǎn)，障礙物為黑色節(jié)點(diǎn)。

圖1 環(huán)境模型

1.2 AWA*算法的基本原理

AWA*算法是在A*算法的基礎(chǔ)上發(fā)展起來(lái)的。A*算法在它的每個(gè)道路節(jié)點(diǎn)均設(shè)計(jì)了一個(gè)估價(jià)函數(shù)：

k(s)=g(s)+h(s)

(1)

而AWA*算法區(qū)別于A*算法的是其初次搜索時(shí)，在估價(jià)函數(shù)的基礎(chǔ)上引入了權(quán)值系數(shù)K：

k(s)=g(s)+K·h(s)

(2)

在討論A*算法及AWA*算法的標(biāo)準(zhǔn)術(shù)語(yǔ)中，g(s)表示從初始節(jié)點(diǎn)到任意節(jié)點(diǎn)s的真實(shí)代價(jià)消耗，K為啟發(fā)函數(shù)的權(quán)值系數(shù)，h(s)表示從節(jié)點(diǎn)s到目標(biāo)點(diǎn)的啟發(fā)式評(píng)估代價(jià)消耗。g(s)是可知的：

A*算法一定能搜索到最優(yōu)路徑的前提條件：

h(s)≤cost*(s,sgoal)

(4)

考慮到浮點(diǎn)數(shù)處理較慢的因素，柵格節(jié)點(diǎn)間采用的連接關(guān)系是無(wú)浮點(diǎn)數(shù)計(jì)算的四連接。為了保證搜索路徑的最優(yōu)性，采用以當(dāng)前節(jié)點(diǎn)s到目標(biāo)點(diǎn)goal的曼哈頓距離作為啟發(fā)式函數(shù)：

h(s)=|xs-xgoal|+|ys-ygoal|

(5)

在節(jié)點(diǎn)s的擴(kuò)展子節(jié)點(diǎn)中，由于各個(gè)子節(jié)點(diǎn)g值相同，所以子節(jié)點(diǎn)越靠近終點(diǎn)G(goal)，該子節(jié)點(diǎn)到終點(diǎn)的曼哈頓距離越小，則 f 值就越小。以此引導(dǎo)搜索的方向，使得智能車輛逐步靠近目標(biāo)。

AWA*算法用OPEN表與CLOSED表2個(gè)集合來(lái)管理道路節(jié)點(diǎn)。OPEN表存放待擴(kuò)展的道路子節(jié)點(diǎn)，CLOSED表存放擴(kuò)展過(guò)的子節(jié)點(diǎn)。所有節(jié)點(diǎn)創(chuàng)建時(shí)僅已知特性值(X,Y)，其Cost特性值均設(shè)為0。

AnytimeWeightedA*算法(算法1)是在WeightedA*算法[10]的基礎(chǔ)上，通過(guò)改變其終止條件來(lái)改善路徑精度。算法1的具體框架參考文獻(xiàn)[10]。

算法1的核心思想是在限定的搜索時(shí)間下，基于WeightedA*算法條件進(jìn)行搜索，當(dāng)AWA*算法的OPEN表為空時(shí)，那么算法結(jié)束，沒(méi)有搜索到可行路徑；當(dāng)AWA*算法搜索到了不大于最優(yōu)路徑K倍的可行路徑 f(inc)時(shí)，如果時(shí)間仍有剩余，那么算法會(huì)將當(dāng)前的路徑長(zhǎng)度 f (inc)保存，同時(shí)對(duì)已經(jīng)擴(kuò)展的節(jié)點(diǎn)繼續(xù)搜索，但該搜索滿足的條件相對(duì)于之前WeightedA*算法做了相應(yīng)的改變，后續(xù)擴(kuò)展的節(jié)點(diǎn)除了要滿足k(s)值最小之外，其f(s)值必須要小于f(inc)值，這樣，之后得到的路徑均對(duì)原始可行路徑進(jìn)行了優(yōu)化。AWA*算法終止條件為：OPEN表為空，或者時(shí)間結(jié)束，算法終止。

2 優(yōu)化AWA*算法路徑規(guī)劃

WeightedA*算法[10]基于A*算法的擴(kuò)展特點(diǎn)，引入了權(quán)值系數(shù)K，通過(guò)擴(kuò)大啟發(fā)函數(shù)使其擴(kuò)展節(jié)點(diǎn)數(shù)目減少，加快搜索速度。但由于啟發(fā)函數(shù)的過(guò)分增大，最優(yōu)子節(jié)點(diǎn)選取不充分，導(dǎo)致路徑精度折損過(guò)多。AnytimeWeightedA*算法在WeightedA*算法的基礎(chǔ)上加入了終止條件，使AWA*算法可在時(shí)間充裕的條件下繼續(xù)搜索更優(yōu)路徑。但AWA*算法如若初次搜索到最優(yōu)路徑，仍會(huì)繼續(xù)搜索，擴(kuò)展多余無(wú)用節(jié)點(diǎn)。綜上所述，本文基于WeightedA*算法的估價(jià)函數(shù)，在AnytimeWeightedA*算法的啟發(fā)下引入了動(dòng)態(tài)優(yōu)化因子，提出了一種新型動(dòng)態(tài)估價(jià)函數(shù)。根據(jù)搜索過(guò)程中當(dāng)前耗時(shí)T與當(dāng)前擴(kuò)展節(jié)點(diǎn)數(shù)目的線性關(guān)系，通過(guò)獲取當(dāng)前搜索耗時(shí)參數(shù)T，時(shí)刻更新啟發(fā)函數(shù)的權(quán)值系數(shù)，保證AWA*算法在初次搜索路徑時(shí)便可找到較優(yōu)路徑，進(jìn)而可在限定時(shí)間內(nèi)找到最優(yōu)路徑。

2.1 優(yōu)化AWA*算法的流程步驟

優(yōu)化AWA*算法大體分為3個(gè)階段：

開(kāi)始階段：將起始節(jié)點(diǎn)放入OPEN表中，同時(shí)初始化所有變量。

擴(kuò)展階段：判斷是否找到初始路徑，是則繼續(xù)搜索，但是新增條件為當(dāng)前搜索的路徑 f 值要小于初始路徑inc的 f 值；否則選取OPEN表中最小k值節(jié)點(diǎn)，放入CLOSED表中。同時(shí)擴(kuò)展該節(jié)點(diǎn)，判斷其4個(gè)子節(jié)點(diǎn)是否在OPEN表中，計(jì)算這些子節(jié)點(diǎn)的k值及 f 值后，放入或更新OPEN表；

動(dòng)態(tài)優(yōu)化階段：根據(jù)AWA*算法每次擴(kuò)展子節(jié)點(diǎn)后均檢查最優(yōu)子節(jié)點(diǎn)的機(jī)制，獲取當(dāng)前搜索耗時(shí)，更新動(dòng)態(tài)優(yōu)化因子ε*值。

優(yōu)化AWA*算法框架如算法2所示，其中：OPEN表示A*算法中存放待擴(kuò)展的節(jié)點(diǎn)集合；CLOSED表示存放擴(kuò)展過(guò)的節(jié)點(diǎn)集合。sstart為起始柵格節(jié)點(diǎn)；sgoal為目標(biāo)柵格節(jié)點(diǎn)；s′是s的擴(kuò)展子節(jié)點(diǎn)；K為啟發(fā)函數(shù)的權(quán)值系數(shù)；inc為找到目標(biāo)點(diǎn)的節(jié)點(diǎn)集合；g(s)表示從起始節(jié)點(diǎn)sstart到當(dāng)前節(jié)點(diǎn)s的真實(shí)代價(jià)值；c(s,s′)為節(jié)點(diǎn)s到s′節(jié)點(diǎn)的距離值；k(s)為本文公式中當(dāng)前節(jié)點(diǎn)s的估價(jià)函數(shù)值； f(s)是當(dāng)前節(jié)點(diǎn)的估價(jià)長(zhǎng)度； f(inc)表示第1次找到的從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的可行路徑長(zhǎng)度；h(s′)為當(dāng)前子節(jié)點(diǎn)s′的啟發(fā)函數(shù)值；ε*表示動(dòng)態(tài)優(yōu)化因子；T1表示限定的搜索時(shí)間；P1表示開(kāi)始時(shí)記錄的計(jì)算機(jī)時(shí)間截點(diǎn)；P2表示擴(kuò)展當(dāng)前子節(jié)點(diǎn)后的計(jì)算機(jī)時(shí)間截點(diǎn)；T為當(dāng)前搜索消耗的時(shí)間；α表示觸發(fā)搜索極限時(shí)間比率；B表示搜索極限速度常數(shù)。

算法2 優(yōu)化Anytime Weighted A*算法

2.2 優(yōu)化AWA*算法的具體思路

本文在AWA*算法的初次搜索中估價(jià)函數(shù)中引入動(dòng)態(tài)優(yōu)化因子ε*，根據(jù)A*算法循環(huán)擴(kuò)展子節(jié)點(diǎn)的特點(diǎn)來(lái)不斷變化啟發(fā)函數(shù)的權(quán)值系數(shù)Kε*，以保證初始路徑時(shí)可快速地選擇較優(yōu)的路徑，同時(shí)可進(jìn)一步在限定時(shí)間內(nèi)尋找更優(yōu)路徑。

優(yōu)化AWA*算法的估價(jià)函數(shù)如下：

河南省戰(zhàn)略性新興產(chǎn)業(yè)發(fā)明專利數(shù)量不多、質(zhì)量不高的現(xiàn)實(shí)狀況，反映出河南省自主創(chuàng)新能力不足。這些問(wèn)題的形成，既與河南省經(jīng)濟(jì)發(fā)展、科技創(chuàng)新、人才培養(yǎng)等方面的歷史欠賬有關(guān)，又與新時(shí)期河南省知識(shí)產(chǎn)權(quán)戰(zhàn)略實(shí)施不徹底、知識(shí)產(chǎn)權(quán)布局不科學(xué)、知識(shí)產(chǎn)權(quán)潛力挖掘不充分等有關(guān)。具體來(lái)說(shuō)，主要有以下幾個(gè)方面的問(wèn)題。

k(s)=g(s)+Kε*×h(s)

(6)

本文定義了動(dòng)態(tài)優(yōu)化因子ε*，其具體計(jì)算公式如下：

式中： K為初始權(quán)值系數(shù)；T1為限定搜索時(shí)間(ms)； T為獲取的當(dāng)前搜索耗時(shí)(ms)； B為搜索極限速度常數(shù)； α為觸發(fā)搜索極限時(shí)間比率。

加入了搜索耗時(shí)T1后，ε*值在開(kāi)始時(shí)每次擴(kuò)展子節(jié)點(diǎn)后均會(huì)增大，同時(shí)優(yōu)化AWA*算法的估價(jià)函數(shù)也會(huì)相應(yīng)改變，減少了可選擇節(jié)點(diǎn)數(shù)目，保證了搜索效率。隨著搜索耗時(shí)T的不斷增大，當(dāng)T大于或等于αT1時(shí)，此時(shí)距離目標(biāo)節(jié)點(diǎn)較近，通過(guò)使ε*變?yōu)槌?shù)B，來(lái)確保搜索路徑精度。

又由式(7)可知：

K

(8)

由式(4)(8)可證得：

(9)

推導(dǎo)結(jié)果證明了優(yōu)化AWA*算法初次規(guī)劃出的路徑要小于KB倍的最優(yōu)路徑，以此保證了優(yōu)化AWA*算法路徑的次優(yōu)性。

3 仿真實(shí)驗(yàn)及結(jié)果分析

本文針對(duì)路徑規(guī)劃搜索耗時(shí)問(wèn)題，對(duì)AWA*算法及優(yōu)化AWA*算法進(jìn)行了路徑規(guī)劃耗時(shí)誤差仿真實(shí)驗(yàn)，驗(yàn)證了優(yōu)化AWA*算法在不同單一環(huán)境地圖下，仿真平臺(tái)具有一定穩(wěn)定性，搜索耗時(shí)誤差具有一定局限性，實(shí)驗(yàn)數(shù)據(jù)具有一定可靠性。同時(shí)，本文進(jìn)行了低百分比障礙物環(huán)境地圖路徑規(guī)劃普適性仿真實(shí)驗(yàn)，對(duì)比分析了優(yōu)化AWA*算法與傳統(tǒng)AWA*算法的耗時(shí)情況和路徑精度。為進(jìn)一步驗(yàn)證優(yōu)化AWA*算法的普適性，本文進(jìn)行了高百分比障礙物的環(huán)境地圖路徑規(guī)劃普適性仿真實(shí)驗(yàn)，并對(duì)仿真實(shí)驗(yàn)總體上進(jìn)行了系統(tǒng)性地分析。2種算法均在VitualStudio2013上實(shí)現(xiàn)，仿真實(shí)驗(yàn)采用的計(jì)算機(jī)CPU型號(hào)為Intel酷睿i5 3230M，主頻均為2.6GHz，RAM為4GB。在2種仿真實(shí)驗(yàn)中，優(yōu)化AWA*算法均采用相同的參數(shù)，且該參數(shù)均由實(shí)際需要自行設(shè)定，如表1所示。

表1 優(yōu)化AWA*算法主要參數(shù)

3.1 路徑規(guī)劃耗時(shí)誤差仿真實(shí)驗(yàn)

本文為研究每次搜索耗時(shí)的誤差特性，對(duì)2種算法分別進(jìn)行了單一地圖在10%～50%障礙物百分比下的100次重復(fù)規(guī)劃仿真實(shí)驗(yàn)，并以20%障礙物下的單一環(huán)境地圖為例，其結(jié)果如圖2所示。

仿真實(shí)驗(yàn)結(jié)果顯示：在環(huán)境地圖障礙物百分比為20%時(shí)，AWA*算法每次搜索耗時(shí)之間的誤差值為32ms，優(yōu)化AWA*算法搜索耗時(shí)誤差分別為16ms。同理，在10%、30%、40%、50%障礙物的環(huán)境地圖中，AWA*算法與優(yōu)化AWA*算法搜索耗時(shí)誤差也均保持在32ms以內(nèi)，說(shuō)明2種算法在各個(gè)環(huán)境地圖中的耗時(shí)誤差均具有一定局限性，且重復(fù)規(guī)劃的路徑結(jié)果均一致，驗(yàn)證了2種算法路徑結(jié)果重復(fù)規(guī)劃的一致性和仿真平臺(tái)數(shù)據(jù)結(jié)果的可靠性。

3.2 算法普適性仿真實(shí)驗(yàn)分析

3.2.1低百分比障礙物環(huán)境地圖的普適性仿真實(shí)驗(yàn)

為分析優(yōu)化AWA*算法在特定障礙物百分比但不同環(huán)境地圖下的各項(xiàng)性能，本文分別在障礙物百分比為10%、20%、30%、40%、50%下，均隨機(jī)生成100種環(huán)境地圖。路徑規(guī)劃后的搜索耗時(shí)仿真實(shí)驗(yàn)結(jié)果如圖3～7所示。

仿真實(shí)驗(yàn)結(jié)果顯示：優(yōu)化AWA*算法與AWA*算法相比，在障礙物為10%、20%、30%、40%、50%下的環(huán)境地圖中搜索耗時(shí)分別下降了69.3%、73.5%、49.8%、21.4%、19.9%，擴(kuò)展節(jié)點(diǎn)數(shù)目減少了55%、62%、43%、22%、12%，路徑平均長(zhǎng)度分別減少了0.47%、1.3%、1.2%、1.7%、0。綜上可知，在10%～50%障礙物環(huán)境地圖下，優(yōu)化AWA*算法的各項(xiàng)指標(biāo)均優(yōu)于AWA*算法。此仿真實(shí)驗(yàn)驗(yàn)證了優(yōu)化AWA*算法在保證路徑次優(yōu)性的同時(shí)仍具有搜索耗時(shí)少的優(yōu)勢(shì)。

通過(guò)對(duì)比圖3(c)、圖4(c)、圖5(c)、圖6(c)、圖7(c)，發(fā)現(xiàn)隨著環(huán)境地圖障礙物百分比逐步增大，優(yōu)化AWA*算法相比于AWA*算法的搜索耗時(shí)優(yōu)勢(shì)逐步減少這一現(xiàn)象。為此，本文通過(guò)進(jìn)一步地仿真實(shí)驗(yàn)來(lái)驗(yàn)證此發(fā)現(xiàn)。

3.2.2高百分比障礙物環(huán)境地圖的普適性仿真實(shí)驗(yàn)

由于障礙物百分比為90%時(shí)，自由空間不足以生成起點(diǎn)到達(dá)終點(diǎn)的可行駛路徑，故隨機(jī)生成路徑的障礙物百分比最高設(shè)定為80%。本文針對(duì)優(yōu)化AWA*算法相比于AWA*算法，在高百分比障礙物環(huán)境地圖下的搜索耗時(shí)優(yōu)勢(shì)減少的問(wèn)題，設(shè)定60%、80%百分比障礙物，分別隨機(jī)生成100種環(huán)境地圖，仿真實(shí)驗(yàn)結(jié)果見(jiàn)圖8、9。

仿真實(shí)驗(yàn)結(jié)果顯示：優(yōu)化AWA*算法與AWA*算法相比，在60%、80%障礙物下的環(huán)境地圖中，搜索耗時(shí)分別下降了3.27%、4.56%，路徑精度分別下降了0.1%、0。此仿真實(shí)驗(yàn)結(jié)果與上述發(fā)現(xiàn)的環(huán)境地圖障礙物百分比逐步增大，優(yōu)化AWA*算法相比于AWA*算法的搜索耗時(shí)優(yōu)勢(shì)逐步減少的現(xiàn)象一致。

圖2 20%障礙物地圖下的路徑規(guī)劃

圖3 10%障礙物地圖下的路徑規(guī)劃

圖4 20%障礙物地圖下的路徑規(guī)劃

圖5 30%障礙物地圖下的路徑規(guī)劃

圖6 40%障礙物地圖下的路徑規(guī)劃

圖7 50%障礙物地圖下的路徑規(guī)劃

圖8 60%障礙物地圖下的路徑規(guī)劃

圖9 80%障礙物地圖下的路徑規(guī)劃

本文對(duì)此現(xiàn)象的原因進(jìn)行了合理推斷：

耗時(shí)大小的具體體現(xiàn)是OPEN表中存放的待擴(kuò)展節(jié)點(diǎn)數(shù)目。在低百分比障礙物的環(huán)境地圖中，障礙物數(shù)量少，自由空間相對(duì)多。在未搜索到初始路徑時(shí)，由于優(yōu)化AWA*算法相比于A*算法啟發(fā)函數(shù)值偏大，故在OPEN表中添入可選擇的待擴(kuò)展子節(jié)點(diǎn)數(shù)目偏少，導(dǎo)致擴(kuò)展節(jié)點(diǎn)過(guò)程中計(jì)算k值的次數(shù)少，規(guī)劃路徑時(shí)間更短。

而隨著環(huán)境地圖障礙物逐步增多，自由空間相對(duì)逐步減小。在擴(kuò)展子節(jié)點(diǎn)時(shí)，AWA*算法OPEN表中添入可選擇的待擴(kuò)展節(jié)點(diǎn)被動(dòng)減少，導(dǎo)致優(yōu)化AWA*算法相比于AWA*算法擴(kuò)展數(shù)目的差距逐步減小，故優(yōu)化AWA*算法在搜索耗時(shí)方面的優(yōu)勢(shì)逐步減少，直至持平。

4 結(jié)束語(yǔ)

本文提出了一種基于AWA*算法的新型估價(jià)函數(shù)。在原有AWA*算法估價(jià)函數(shù)基礎(chǔ)上，引入了動(dòng)態(tài)優(yōu)化因子ε*，建立了新型的估價(jià)函數(shù)，根據(jù)AWA*算法流程中循環(huán)擴(kuò)展節(jié)點(diǎn)的特點(diǎn)不斷更新啟發(fā)函數(shù)的權(quán)值系數(shù)Kε*，加快路徑規(guī)劃最優(yōu)性收斂，改善智能車輛路徑規(guī)劃在限定時(shí)間下路徑精度不佳問(wèn)題。

在全局工況下，優(yōu)化AWA*算法相比于AWA*算法，在保證路徑次優(yōu)性的同時(shí)降低了搜索路徑耗時(shí)，尤其是在低障礙物百分比地圖下，效果更為明顯。

當(dāng)環(huán)境地圖障礙物百分比逐步增大時(shí)，優(yōu)化AWA*算法相比于AWA*算法的搜索耗時(shí)優(yōu)勢(shì)逐步減小，最后兩者搜索耗時(shí)基本一致。