祁玄玄，黃家駿，曹建安

（西安交通大學(xué)電氣工程學(xué)院，西安 710049）

（*通信作者電子郵箱2787477370@qq.com）

0 引言

路徑規(guī)劃在智能車運(yùn)動(dòng)控制中占有核心地位，路徑規(guī)劃算法的效率將直接影響無(wú)人車的尋路效率及實(shí)施規(guī)劃能力。目前路徑規(guī)劃算法基本分為兩種類型：基于圖搜索算法的傳統(tǒng)算法和智能算法。圖搜索算法主要指的是Floyd 算法［1］和Dijkstra 算法［2］；智能算法包括蟻群算法［3］、粒子群算法［4］、遺傳算法［5］、神經(jīng)網(wǎng)絡(luò)［6］、模擬退火［7］等。傳統(tǒng)的圖搜索算法存在隨著環(huán)境信息的增加計(jì)算復(fù)雜性呈現(xiàn)指數(shù)式增加的缺點(diǎn)，智能算法作為一種路徑規(guī)劃的新思路其對(duì)計(jì)算機(jī)性能要求過(guò)高且存在計(jì)算時(shí)間較長(zhǎng)的缺點(diǎn)。A*（A-Star）算法［8-9］是基于傳統(tǒng)圖搜索的思維的智能啟發(fā)式算法，相比傳統(tǒng)圖搜索算法，它具有計(jì)算量小、規(guī)劃路徑相對(duì)最優(yōu)等突出特點(diǎn)。但是A*算法的啟發(fā)函數(shù)考慮維度較為簡(jiǎn)單，導(dǎo)致該算法在尋路過(guò)程中會(huì)出現(xiàn)很多冗余的擴(kuò)展柵格。目前已經(jīng)提出了很多改進(jìn)A*算法，如：文獻(xiàn)［10］中通過(guò)估價(jià)函數(shù)進(jìn)行指數(shù)衰減的方式加權(quán)減少了冗余的擴(kuò)展；文獻(xiàn)［11］中通過(guò)建立禁忌表來(lái)改進(jìn)A*算法的估價(jià)函數(shù)，能快速、有效地實(shí)現(xiàn)越野路徑規(guī)劃。與文獻(xiàn)［10-11］類似地通過(guò)對(duì)A*算法估價(jià)函數(shù)進(jìn)行改進(jìn)以達(dá)到減少計(jì)算時(shí)間的算法還有很多，但是它們的普適性并不好。文獻(xiàn)［12］中通過(guò)起點(diǎn)和終點(diǎn)同時(shí)運(yùn)行時(shí)效A*算法尋找路徑，文獻(xiàn)［13］中通過(guò)并行算法改進(jìn)A*尋路時(shí)間，這兩者在一定程度上可減少計(jì)算時(shí)間，但是都過(guò)于依賴計(jì)算機(jī)的性能。

從上述文獻(xiàn)中可以看出，過(guò)去A*算法的改進(jìn)方法主要是通過(guò)優(yōu)化代價(jià)函數(shù)或提高計(jì)算空間來(lái)減少計(jì)算時(shí)間；但這些方法都不具有很高的普適性，并且部分改進(jìn)方法十分依賴于計(jì)算機(jī)的性能。本文從四個(gè)方面對(duì)A*算法進(jìn)行改進(jìn)，提出一種兼顧普適性和計(jì)算效率的A*算法：首先是優(yōu)化算法的拓展方向，根據(jù)目標(biāo)和擴(kuò)展節(jié)點(diǎn)的相對(duì)位置選擇雙象限方向進(jìn)行節(jié)點(diǎn)拓展；二是目標(biāo)可見(jiàn)性判斷，目標(biāo)可見(jiàn)時(shí)跳出A*算法的探索過(guò)程；其三，通過(guò)加入k個(gè)父輩估計(jì)信息改變算法的啟發(fā)函數(shù)；最后，通過(guò)引入模擬退火法改變待擴(kuò)展節(jié)點(diǎn)的選取方略。通過(guò)Matlab 仿真實(shí)驗(yàn)，在柵格地圖環(huán)境下對(duì)比A*算法與改進(jìn)A*算法的計(jì)算結(jié)果，驗(yàn)證了本文算法計(jì)算時(shí)間更短，擴(kuò)展節(jié)點(diǎn)更少，尋路的目標(biāo)性更強(qiáng)。

1 全局路徑規(guī)劃算法

1.1 A*算法

作為一種圖搜索算法，A*算法適合大面積的地圖中路徑搜索。這是一種以Dijkstra 算法和廣度優(yōu)先搜索（Breadth First Search，BFS）算法為基礎(chǔ)的啟發(fā)式搜索算法。因?yàn)槠渚哂蠨ijkstra 算法的特點(diǎn)，所以，A*算法可以用于搜索最短路徑。與此同時(shí)由于A*算法包含BFS 的特點(diǎn)，因此它在搜索路徑的判斷依據(jù)中包含啟發(fā)函數(shù)，該函數(shù)就是BFS 算法之中的得分函數(shù)。在路徑搜索過(guò)程中，A*算法使用代價(jià)函數(shù)來(lái)評(píng)估節(jié)點(diǎn)的質(zhì)量。算法將選擇的代價(jià)函數(shù)數(shù)值最小的節(jié)點(diǎn)作為下一步擴(kuò)展的節(jié)點(diǎn)，然后它將繼續(xù)從下一個(gè)節(jié)點(diǎn)搜索，直到到達(dá)目標(biāo)點(diǎn)。像BFS 一樣，A*可以使用啟發(fā)式函數(shù)來(lái)引導(dǎo)自己。A*算法的代價(jià)函數(shù)如下：

f(n)=g(n)+h(n) （1）

其中：n代表路徑搜索過(guò)程之中最近的一個(gè)節(jié)點(diǎn)，g（n）代表從起始點(diǎn)到n節(jié)點(diǎn)的最短路徑，h（n）代表從n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值。g（n）經(jīng)常都是固定的數(shù)值，這個(gè)數(shù)值就是Dijkstra算法中的到起始節(jié)點(diǎn)的最短路徑；然而h（n）卻是不固定的，它的改變將會(huì)改變優(yōu)化出來(lái)的路徑，因此選擇合適的h（n）可以得出最優(yōu)的路徑。

h（n）代表從n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值，因此最能表達(dá)其含義的就是兩點(diǎn)之間的距離，本文采用曼哈頓距離作為h（n）函數(shù)的估計(jì)。其計(jì)算表達(dá)式如式（2）所示：

h(n)=D(abs(n.x-goal.x)+abs(n.y-goal.y)) （2）

從式（2）可以看出，h（n）函數(shù)表示n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的x軸和y軸相對(duì)距離的絕對(duì)值的和。

和Dijkstra 算法一樣，A*算法在計(jì)算過(guò)程中也存在兩個(gè)核心集合。定義已經(jīng)走過(guò)的點(diǎn)的集合為Close，待選集合為Open，集合里面的每個(gè)元素為node，該元素的屬性是該節(jié)點(diǎn)代價(jià)函數(shù)的估計(jì)值f（n）。每次從Open選出延伸節(jié)點(diǎn)，然后往四個(gè)方向或者八個(gè)方向進(jìn)行擴(kuò)展。圖1 為A*演示模型［14］，其中綠色點(diǎn)是起始點(diǎn)設(shè)為A，紅色點(diǎn)是目標(biāo)點(diǎn)設(shè)為B，3 個(gè)藍(lán)色點(diǎn)是障礙物分別設(shè)為C、D、E。方格的左上角、左下角、右下角分別代表代價(jià)函數(shù)的估計(jì)值f（n），從起始點(diǎn)到n節(jié)點(diǎn)的最短路徑g（n），從n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值h（n）。具體演示流程如下。

1）初始化集合。

首先把A 點(diǎn)加入到Open集合，初始化A 點(diǎn)代價(jià)函數(shù)的估計(jì)值f（n）數(shù)值為0，把障礙物加入到Close集合。

2）擴(kuò)展節(jié)點(diǎn)。

從Open集合中選取f（n）數(shù)值最小的點(diǎn)，將其放入Close集合。然后往該節(jié)點(diǎn)相鄰的八個(gè)方向擴(kuò)展方格，并計(jì)算不在Close集合中的擴(kuò)展方格的f（n）數(shù)值。如果擴(kuò)展方格已經(jīng)在Open集合，那么根據(jù)此時(shí)計(jì)算的擴(kuò)展方格的f（n）數(shù)值更新該節(jié)點(diǎn)，如果較小則更新，否則不更新。如果擴(kuò)展方格不在Open集合，那么根據(jù)計(jì)算的結(jié)果往Open集合中添加該擴(kuò)展節(jié)點(diǎn)。

3）循環(huán)判斷。

重復(fù)步驟2），如果擴(kuò)展到目標(biāo)節(jié)點(diǎn)或者Open集合為空則退出循環(huán)，根據(jù)回溯算法倒推出從起始點(diǎn)到目標(biāo)點(diǎn)的最短路徑。

通過(guò)A*算法可以從某一點(diǎn)到另外一點(diǎn)的最短路徑，圖1中紅色圓點(diǎn)連接就是擴(kuò)展得出的路徑。

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

A*算法無(wú)疑是一種高效的算法，但傳統(tǒng)的A*仍存在一些不足，例如擴(kuò)展方向盲目性、啟發(fā)函數(shù)過(guò)于簡(jiǎn)單等，導(dǎo)致A*算法計(jì)算效率較低。因此，本文從以下四個(gè)方面對(duì)傳統(tǒng)的A*算法進(jìn)行改進(jìn)。

1.2.1 自適應(yīng)擴(kuò)展方向

在固定的環(huán)境中A*在擴(kuò)展時(shí)能夠在任何一個(gè)方向上擴(kuò)展，即分別進(jìn)行上下左右的擴(kuò)展。而改進(jìn)的A*算法提出的是確定目標(biāo)所在的象限，一旦確定目標(biāo)象限，就會(huì)向該象限擴(kuò)展從而忽略其他沒(méi)必要的象限。因此改進(jìn)的A*算法在擴(kuò)展時(shí)只能往上下左右的某一個(gè)方向擴(kuò)展。確定好象限之后就可以確定擴(kuò)展的相鄰節(jié)點(diǎn)。由于擴(kuò)展方向縮減為原先的四分之一，因此在一定程度上可以減少算法的計(jì)算時(shí)間。

定義當(dāng)前擴(kuò)展節(jié)點(diǎn)的坐標(biāo)為（n.x，n.y），目標(biāo)節(jié)點(diǎn)的坐標(biāo)為（goal.x，goal.y）。通過(guò)目標(biāo)節(jié)點(diǎn)和擴(kuò)展節(jié)點(diǎn)的做差得到Dx，Dy。即式（3）所示：

根據(jù)Dx，Dy的數(shù)值本文可以確定擴(kuò)展方向所在的象限以及所擴(kuò)展的點(diǎn)。具體判斷依據(jù)如式（4）所示：

如圖2 象限分布所示，當(dāng)目標(biāo)在第一象限時(shí)，擴(kuò)展節(jié)點(diǎn)的擴(kuò)展鄰居為｛1，2，3，4，5｝；當(dāng)目標(biāo)在第二象限時(shí)，擴(kuò)展節(jié)點(diǎn)的擴(kuò)展鄰居為｛1，2，3，7，8｝；當(dāng)目標(biāo)在第三象限時(shí)，擴(kuò)展節(jié)點(diǎn)的擴(kuò)展鄰居為｛1，5，6，7，8｝；當(dāng)目標(biāo)在第四象限時(shí)，擴(kuò)展節(jié)點(diǎn)的擴(kuò)展鄰居為｛3，4，5，6，7｝。

圖2 象限分布Fig.2 Quadrant distribution

1.2.2 判斷目標(biāo)可見(jiàn)

傳統(tǒng)的A*在循環(huán)擴(kuò)展的時(shí)候，循環(huán)截止條件是靠近目標(biāo)點(diǎn)或者是Open集合為空，這將導(dǎo)致在靠近目標(biāo)點(diǎn)的時(shí)候沒(méi)有任何障礙物依然會(huì)有額外的擴(kuò)展節(jié)點(diǎn)的開(kāi)銷，這顯然是不合理的?；诖耍疚奶岢鲈跀U(kuò)展節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)之間連線沒(méi)有任何障礙物的時(shí)候，可以跳出A*的啟發(fā)式探索過(guò)程。判斷的依據(jù)如式（5）所示：

Newpos=(n.x，n.y)+r(cosφ，sinφ) （5）

其中：r代表更迭步長(zhǎng)，φ代表擴(kuò)展節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)之間的夾角。Newpos代表在柵格地圖中的坐標(biāo)，因此只需要判斷Newpos位置處是不是存在障礙物就可以判斷擴(kuò)展節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)之間連線有無(wú)任何障礙物。

1.2.3 改變啟發(fā)函數(shù)

代價(jià)函數(shù)的估計(jì)值f（n）是從起始點(diǎn)到n節(jié)點(diǎn)的最短路徑g（n）和從n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值h（n）的和。起始點(diǎn)到n節(jié)點(diǎn)的最短路徑是在探索過(guò)程中從父節(jié)點(diǎn)累加的，因此g（n）是和前面的探索節(jié)點(diǎn)是有關(guān)聯(lián)的。而h（n）只是求n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離估計(jì)值，因此和前面的探索節(jié)點(diǎn)是沒(méi)有任何關(guān)聯(lián)的?；诖吮疚脑诖鷥r(jià)函數(shù)的估計(jì)值f（n）中添加當(dāng)前n節(jié)點(diǎn)父節(jié)點(diǎn)及其祖輩節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值h（n）。因此代價(jià)函數(shù)的估計(jì)值f（n）如式（6）所示：

f(n)=g(n)+D×(h(n)+h(p)+h(p2)+...+h(pk)) （6）

其中：p是n節(jié)點(diǎn)父節(jié)點(diǎn)，pk代表n節(jié)點(diǎn)的k父輩，h（p）是n節(jié)點(diǎn)父節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值，D是啟發(fā)函數(shù)的系數(shù)。D和k這兩個(gè)數(shù)都會(huì)影響A*算法的計(jì)算效率，因此選擇合適的D和k是十分重要的。

1.2.4 改變擴(kuò)展節(jié)點(diǎn)選取方略

傳統(tǒng)的A*算法在從Open集合選取待擴(kuò)展節(jié)點(diǎn)的原則是最小的代價(jià)函數(shù)的估計(jì)值的節(jié)點(diǎn)。從起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)搜索最短路徑時(shí)，g（n）的數(shù)值是從父節(jié)點(diǎn)累加過(guò)來(lái)的，因此它可以盡可能地保證前面走過(guò)的路徑是最短的。而h（n）代表從n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值，如果可以盡可能地保證h（n）是逐漸減小的，那么在一定程度上就是保證n節(jié)點(diǎn)后面的路徑是在接近目的地。

為了實(shí)現(xiàn)n節(jié)點(diǎn)后面的路徑是在接近目的地，如果每次都以貪婪策略，即h（n）最小值節(jié)點(diǎn)來(lái)作為擴(kuò)展節(jié)點(diǎn)是最合適的，但是這樣就忽略了g（n）的影響，該算法也就變成了傳統(tǒng)的深度優(yōu)先搜索了，很有可能優(yōu)化出來(lái)的路徑只是局部最優(yōu)。為了擺脫局部異常的束縛，本文引入模擬退火算法來(lái)選擇擴(kuò)展節(jié)點(diǎn)。

模擬退火［15］是一種解決無(wú)約束和有邊界約束的優(yōu)化問(wèn)題的方法。該方法模擬了加熱材料然后緩慢降低溫度以減少缺陷的物理過(guò)程，從而最大限度地降低了系統(tǒng)能耗。模擬退火算法包含兩個(gè)部分，即Metropolis算法和退火過(guò)程。

1）Metropolis算法過(guò)程。

圖3 是模擬退火的過(guò)程的示意圖，A 點(diǎn)是迭代起始點(diǎn)，隨著迭代次數(shù)增加，到達(dá)局部最優(yōu)解B點(diǎn)，此時(shí)若根據(jù)梯度下降準(zhǔn)則再更新的話是不被允許的，而模擬退火算法會(huì)在此時(shí)以一定的概率跳出這個(gè)局限，這個(gè)概率和物質(zhì)能量以及迭代次數(shù)息息相關(guān)，類似情形通過(guò)C點(diǎn)，最終到達(dá)全局最優(yōu)解D點(diǎn)。

圖3 模擬退火的過(guò)程Fig.3 Simulated annealing process

概率準(zhǔn)則如式（7）所示。其中n代表迭代次數(shù)，E（n）代表第n次迭代的值。

從式（7）可以看到：如果能量減小了，那么這種轉(zhuǎn)移就被接受（概率為1）；如果能量增大了，就說(shuō)明系統(tǒng)偏離全局最優(yōu)值位置更遠(yuǎn)了，此時(shí)算法不會(huì)立刻將其拋棄，而是進(jìn)行概率操作：首先在區(qū)間［0，1］產(chǎn)生一個(gè)均勻分布的隨機(jī)數(shù)ε，如果ε＜P，則此種轉(zhuǎn)移被接受，否則拒絕轉(zhuǎn)移，進(jìn)入下一步，往復(fù)循環(huán)。其中P以能量的變化量和T進(jìn)行決定概率P的大小，所以這個(gè)值是動(dòng)態(tài)的，而且隨著迭代次數(shù)的推移往往P逐漸變小。

2）退火過(guò)程。

退火意思就是溫度降低的過(guò)程，主要包含初始溫度、退火速率和終止溫度三個(gè)內(nèi)容。初始溫度如果給得太高則獲得高質(zhì)量的解的概率越大，耗費(fèi)的時(shí)間越長(zhǎng)。退火速率的設(shè)置形式較多，常使用指數(shù)式下降型，具體如式（8）所示。其中退火速率r是小于1的數(shù)，一般取值［0.8，0.99］。如果在若干次迭代后沒(méi)有可以更新的新?tīng)顟B(tài)或者達(dá)到用戶設(shè)定的閾值，則退火完成，此時(shí)的溫度稱為終止溫度。

根據(jù)模擬退火算法的基本特點(diǎn)本文設(shè)計(jì)選擇擴(kuò)展節(jié)點(diǎn)的流程如下。首先把從n節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)h（n）視作為模擬退火的能量函數(shù)。

1）初始化各個(gè)參數(shù)。

初始化初始溫度T=3，退火速率r=0.98，能量函數(shù)初始數(shù)值為起始點(diǎn)到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值。擴(kuò)展初始節(jié)點(diǎn)，更新Close集合和Open集合，即把起始節(jié)點(diǎn)放入Close集合，根據(jù)自適應(yīng)擴(kuò)展方向算法把起始點(diǎn)相鄰節(jié)點(diǎn)放入Open集合。

2）產(chǎn)生新解n'。

從Open集合之中根據(jù)最小代價(jià)函數(shù)的估計(jì)值f（n）選取新的節(jié)點(diǎn)n'，計(jì)算新的節(jié)點(diǎn)目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值h(n')。

3）計(jì)算增量。

因?yàn)槟繕?biāo)函數(shù)差僅由變換部分產(chǎn)生，所以目標(biāo)函數(shù)差的計(jì)算最好按增量計(jì)算。事實(shí)表明，對(duì)大多數(shù)應(yīng)用而言，這是計(jì)算目標(biāo)函數(shù)差的最快方法。根據(jù)式（9）計(jì)算能量函數(shù)的變化量：

4）判斷是否接受當(dāng)前解n'。

判斷的依據(jù)是一個(gè)接受準(zhǔn)則，最常用的接受準(zhǔn)則是Metropolis 準(zhǔn)則：若ΔT＜0 則接受n'作為新的當(dāng)前解n；當(dāng)ΔT＞0時(shí)，產(chǎn)生一個(gè)均勻分布的隨機(jī)數(shù)ε，若exp(-ΔT/T) ＞ε接受n'作為新的當(dāng)前解n，若exp(-ΔT/T) ≤ε不接受n'作為新的當(dāng)前解n，根據(jù)最小到目標(biāo)節(jié)點(diǎn)的啟發(fā)函數(shù)的預(yù)測(cè)值h(n)選取新的節(jié)點(diǎn)n'。

5）更新各個(gè)參數(shù)。

改變退火溫度T=T*r，退火速率r=0.98，擴(kuò)展新的節(jié)點(diǎn)n'，更新Close集合和Open集合，即把新的節(jié)點(diǎn)n'放入Close集合，根據(jù)自適應(yīng)擴(kuò)展方向算法把新的節(jié)點(diǎn)n'相鄰節(jié)點(diǎn)放入Open集合。

本節(jié)從四個(gè)方面具體介紹了改進(jìn)A*算法的細(xì)節(jié)。改進(jìn)后的A*算法偽代碼如下所示：

改進(jìn)后的A*算法在搜尋從一個(gè)源點(diǎn)到另一個(gè)源點(diǎn)的最短路徑的過(guò)程中更加有目的性，可以在一定程度上減少遍歷的柵格點(diǎn)的數(shù)目，選擇更加優(yōu)化的路徑。尤其要注意的是式（6）中pk代表n節(jié)點(diǎn)的k父輩。雖然越多的父輩被加入到代價(jià)函數(shù)之中可以更加優(yōu)化尋找的最短路徑，但是這樣也會(huì)增加計(jì)算負(fù)擔(dān)和內(nèi)存，因此需要綜合考慮k的取值。

2 仿真和分析

為了測(cè)試改進(jìn)的A*算法，本文采用26 ×27 的柵格地圖作為模擬對(duì)象，如圖4 所示。圖中黑色部分為障礙物，白色空白部分為可行走地段。在仿真過(guò)程中，起始點(diǎn)和目標(biāo)點(diǎn)都是固定的，障礙物位置也是固定的。在柵格地圖坐標(biāo)系（橫軸為從左到右，縱軸為從下到上）下，設(shè)置規(guī)劃起始點(diǎn)為（24，3）。一旦確定目標(biāo)點(diǎn)將會(huì)開(kāi)始路徑規(guī)劃。本文從三個(gè)方面進(jìn)行評(píng)價(jià)：一是計(jì)算時(shí)間；二是優(yōu)化路徑長(zhǎng)度；三是經(jīng)歷的柵格數(shù)目。

圖4 測(cè)試柵格地圖Fig.4 Grid map for testing

2.1 確定啟發(fā)函數(shù)D和k的數(shù)值

為了確定D和k的最優(yōu)數(shù)值，本文用傳統(tǒng)的A*算法測(cè)試不同D和k取值時(shí)系統(tǒng)規(guī)劃路徑時(shí)經(jīng)歷的柵格數(shù)目。由于測(cè)試算法都是A*，因此經(jīng)歷越多的柵格那么算法的執(zhí)行時(shí)間也就越長(zhǎng)。

圖5是D和k取不同數(shù)值時(shí)經(jīng)歷柵格數(shù)目測(cè)試圖。

圖5 D和k取不同數(shù)值時(shí)經(jīng)歷柵格數(shù)目測(cè)試圖Fig.5 Test chart of experienced grid number with different D and k values

從圖5中可以看出當(dāng)k數(shù)值越大那么算法的執(zhí)行效率基本也是越高，但是當(dāng)k增加到第6代的時(shí)候算法效率的提升效果就不是很明顯了。而更多的父代數(shù)據(jù)加入進(jìn)來(lái)會(huì)增加計(jì)算的復(fù)雜性，因此本文選定k的取值范圍是6。同樣的，從圖5中可以看出D的數(shù)值越大算法的執(zhí)行效率基本也是越高，但是其在等于3 的時(shí)候效率提升的效果遇到了瓶頸。所以綜合考慮本文選取的D和k的數(shù)值分別是3和6。

2.2 改進(jìn)的A*和A*對(duì)比仿真

圖6是改進(jìn)的A*和A*路徑規(guī)劃過(guò)程對(duì)比圖，圖中灰色部分代表的是路徑規(guī)劃過(guò)程中經(jīng)歷的柵格，紅色線條是規(guī)劃的路徑。起點(diǎn)和終點(diǎn)都是一樣的分別為（24，3），（6，21）。圖6（a）中灰色柵格個(gè)數(shù)為161，程序運(yùn)行時(shí)間為5.58 s，計(jì)算出的最優(yōu)化路徑長(zhǎng)度為27.799 0。圖6（b）中灰色柵格個(gè)數(shù)為12，程序運(yùn)行時(shí)間為0.66 s，計(jì)算出的最優(yōu)化路徑長(zhǎng)度為26.937 9。通過(guò)數(shù)據(jù)可以很清晰地看出改進(jìn)的A*算法在保證最短路徑的前提下，灰色柵格個(gè)數(shù)和程序計(jì)算時(shí)間上都有了明顯下降，同時(shí)優(yōu)化而出的路徑長(zhǎng)度也較小于傳統(tǒng)A*算法。因此在計(jì)算效率上本文提出的算法還是得到很好的印證。

圖6 改進(jìn)的A*算法和A*算法路徑規(guī)劃過(guò)程對(duì)比Fig.6 Comparison of improved A*algorithm and A*algorithm in path planning process

從圖6中還能看得出兩點(diǎn)：一是改進(jìn)的A*算法在前一擴(kuò)展節(jié)點(diǎn)指向后一擴(kuò)展方向的向量與前一節(jié)點(diǎn)指向目標(biāo)點(diǎn)的向量夾角大多數(shù)滿足銳角關(guān)系，這個(gè)特點(diǎn)就是自適應(yīng)擴(kuò)展方向?qū)е碌慕Y(jié)果；二是在擴(kuò)展后半段當(dāng)目標(biāo)可見(jiàn)時(shí)改進(jìn)的A*算法可以直接跨過(guò)搜索過(guò)程。以上兩個(gè)特點(diǎn)可以一定程度提高A*算法的計(jì)算效率。

圖7是改進(jìn)的A*和A*迭代過(guò)程對(duì)比，從圖中可以看出傳統(tǒng)的A*算法迭代過(guò)程中整體上到目標(biāo)點(diǎn)的距離是逐漸減小的，但是在局部過(guò)程存在震蕩，也就是說(shuō)其局部并沒(méi)有優(yōu)化。而改進(jìn)的A*算法到達(dá)至目標(biāo)點(diǎn)附近所需要的迭代次數(shù)大幅減少，且雖然前期仍存在局部震蕩，但后期就不存在震蕩現(xiàn)象了，這一點(diǎn)正好印證了本文改進(jìn)的A*措施中的第4）條，即模擬退火過(guò)程中，前期溫度較高，可以在一定概率上接受局部違背梯度下降的原則，隨著溫度逐漸降低，這種情況被接受的概率降低到幾乎為零的地步。

圖7 改進(jìn)的A*算法和A*算法迭代過(guò)程對(duì)比Fig.7 Comparison of improved A*algorithm and A*algorithm in iterative process

為了驗(yàn)證本文算法的普適性，以起始點(diǎn)的三個(gè)不同方位作為目標(biāo)點(diǎn)進(jìn)行傳統(tǒng)A*和改進(jìn)A*算法的路徑規(guī)劃，表2是不同目標(biāo)方位統(tǒng)計(jì)結(jié)果。從表中可以看出，改進(jìn)的A*算法在運(yùn)行時(shí)間、經(jīng)歷柵格數(shù)都少于傳統(tǒng)的A*算法，且優(yōu)化路徑長(zhǎng)度幾乎是一樣的。

表2 不同目標(biāo)方位下的路徑規(guī)劃統(tǒng)計(jì)Tab.2 Statistics of path planning with different target directions

從平均值的角度分析，本文提出的改進(jìn)的A*算法在運(yùn)行時(shí)間上減少67.06%，經(jīng)歷的柵格數(shù)減少73.53%，優(yōu)化路徑長(zhǎng)度浮動(dòng)范圍在±0.6%。

2.3 復(fù)合地圖下仿真

在傳統(tǒng)柵格地圖上，通過(guò)仿真可以看出，應(yīng)用本文的改進(jìn)的A*算法是非常高效的，但是當(dāng)障礙物較多或者地圖面積較大的時(shí)候即使是改進(jìn)的A*算法也會(huì)產(chǎn)生很多無(wú)用的擴(kuò)展，這是十分消耗時(shí)間的。其次柵格地圖的缺點(diǎn)是如果分辨率太高，那么尋路過(guò)程會(huì)被拖慢，柵格地圖如果分辨率太低，或喪失很多關(guān)鍵障礙物信息，那么尋路結(jié)果會(huì)變得不準(zhǔn)確。

基于此問(wèn)題本文引入多層地圖的概念，上層地圖是節(jié)點(diǎn)和連線組成的拓?fù)涞貓D，下層是柵格大小相同的柵格地圖。圖8 是復(fù)合地圖的一個(gè)示意圖。上層地圖由黑色的點(diǎn)和黑色連線組成，分別代表采樣路標(biāo)，以及路標(biāo)是否連通。下層地圖由大小相等的黑白柵格組成，黑色為障礙物，白色為可行走區(qū)域。

圖8 復(fù)合地圖Fig.8 Composite map

基于復(fù)合地圖的概念，A*改進(jìn)算法的擴(kuò)展規(guī)則從原來(lái)的柵格的相鄰柵格變?yōu)橥負(fù)涞貓D下路標(biāo)的毗鄰路標(biāo)，其他過(guò)程和柵格地圖是一致的。

圖9 是在起始點(diǎn)為（24，3），目標(biāo)點(diǎn)為（24，21）時(shí)，復(fù)合地圖和柵格地圖下使用本文提出的改進(jìn)的A*算法得到的路徑規(guī)劃圖。選擇圖形右下角作為目標(biāo)點(diǎn)的原因是其尋路過(guò)程較為復(fù)雜，因此可以更加清晰測(cè)試出復(fù)合地圖下改進(jìn)的A*算法的優(yōu)勢(shì)。

從圖9中可以清晰看到復(fù)合地圖下經(jīng)歷柵格數(shù)只有9，而柵格地圖下經(jīng)歷的柵格數(shù)達(dá)到了80，這將大幅度縮短程序的計(jì)算時(shí)間。而優(yōu)化路徑的長(zhǎng)度幾乎是沒(méi)有差別，復(fù)合地圖下是34.192 7，柵格地圖下是34.385 4。而且組合地圖計(jì)算的路徑更加遠(yuǎn)離障礙物，一定程度上較為合理。

以起始點(diǎn)的三個(gè)方位作為目標(biāo)點(diǎn)進(jìn)行不同地圖模式下改進(jìn)的A*算法的路徑規(guī)劃，表3 是不同目標(biāo)方位統(tǒng)計(jì)結(jié)果。從表3中可以看出在復(fù)合地圖下改進(jìn)的A*算法在運(yùn)行時(shí)間、經(jīng)歷柵格數(shù)都少于柵格地圖下A*算法，且優(yōu)化路徑長(zhǎng)度幾乎是一樣的。從平均值的角度分析，本文提出的在復(fù)合地圖下運(yùn)用改進(jìn)的A*算法在運(yùn)行時(shí)間上減少90%以上，經(jīng)歷的柵格數(shù)減少86%左右，優(yōu)化路徑長(zhǎng)度浮動(dòng)范圍在±0.1%。

分析其如此高效的原因是復(fù)合地圖下，改進(jìn)的A*算法擁有了較大的步長(zhǎng)，因此可以更快地尋找到目標(biāo)點(diǎn)。但是其步長(zhǎng)還不能自適應(yīng)地去改變，因此這也是本課題下一步需要改進(jìn)的地方。

圖9 不同地圖模式下改進(jìn)的A*算法結(jié)果Fig.9 Results of improved A*algorithm in different map modes

表3 不同目標(biāo)方位在不同地圖模式下的路徑規(guī)劃統(tǒng)計(jì)Tab.3 Statistical table of path planning with different target directions

2.4 實(shí)際場(chǎng)景驗(yàn)證

為驗(yàn)證本文改進(jìn)的A*算法的有效性，本文在開(kāi)源的硬件平臺(tái)Turtlebot3 上進(jìn)行測(cè)試，Turtlebot3 是一個(gè)小型、低成本、完全可編程的移動(dòng)機(jī)器人。其大小是138 mm×178 mm×192 mm，CPU 是32 位ARM Cortex-M7，具備里程計(jì)、360°激光距離傳感器LDS-01 和IMU 等基本導(dǎo)航設(shè)備。測(cè)試環(huán)境是實(shí)驗(yàn)室環(huán)境，其范圍大概在5 m×4 m 的范圍，路上沒(méi)有障礙物。測(cè)試的過(guò)程是從室內(nèi)某一點(diǎn)運(yùn)行到走廊上某一點(diǎn)。圖10 是實(shí)際場(chǎng)景下改進(jìn)的A*算法結(jié)果，圖中黑色曲線是全局規(guī)劃的路線，路徑的曲率較小的原因是在A*算法規(guī)劃出路徑后進(jìn)行了路徑平滑處理。通過(guò)4 步，Turtlebot3 可以自主行進(jìn)到指定位置。實(shí)驗(yàn)說(shuō)明本文提出的改進(jìn)的A*算法具備實(shí)用價(jià)值，可以在規(guī)避障礙物的條件下高效地進(jìn)行全局路徑規(guī)劃。

圖10 實(shí)際場(chǎng)景下改進(jìn)的A*算法結(jié)果Fig.10 Results of improved A*algorithm in real scene

3 結(jié)語(yǔ)

在進(jìn)行無(wú)人車路徑規(guī)劃設(shè)計(jì)時(shí)，傳統(tǒng)A*算法存在一些不足，比如擴(kuò)展方向盲目性、啟發(fā)函數(shù)只考慮當(dāng)前節(jié)點(diǎn)的信息等，導(dǎo)致A*算法尋路過(guò)程包含很多冗余的節(jié)點(diǎn)，計(jì)算效率較低。本文從四個(gè)方面改進(jìn)傳統(tǒng)A*算法：首先越靠近目標(biāo)節(jié)點(diǎn)越以目標(biāo)節(jié)點(diǎn)的估計(jì)距離作為選取待擴(kuò)展節(jié)點(diǎn)的原則，為了實(shí)現(xiàn)這個(gè)目的，本文引入根據(jù)模擬退火從Open列表中選取待擴(kuò)展節(jié)點(diǎn)以避免陷入局部最優(yōu)解；其次改進(jìn)的A*算法的啟發(fā)函數(shù)，加入了n個(gè)父輩的信息，以此作為啟發(fā)尋路的“歷史經(jīng)驗(yàn)”；然后根據(jù)待擴(kuò)展節(jié)點(diǎn)和目標(biāo)點(diǎn)相對(duì)位置選擇擴(kuò)展象限，以此來(lái)避免待擴(kuò)展節(jié)點(diǎn)盲目往四面八方擴(kuò)展；最后判斷目標(biāo)可見(jiàn)時(shí)跳出尋路過(guò)程，以此避免接近目標(biāo)點(diǎn)時(shí)還存在盲目的冗余擴(kuò)展。

在Matlab環(huán)境下，對(duì)不同目標(biāo)方位下改進(jìn)的A*算法和A*算法的路徑規(guī)劃算法進(jìn)行仿真，結(jié)果顯示，本文提出的改進(jìn)的A*算法在運(yùn)行時(shí)間上減少67.06%，經(jīng)歷的柵格數(shù)減少73.53%，優(yōu)化路徑長(zhǎng)度浮動(dòng)范圍在±0.6%?？梢钥闯霰疚母倪M(jìn)的A*算法在不同情況下都能夠有效地縮短尋路時(shí)間，減少冗余擴(kuò)展，算法的計(jì)算效率較高。