王 鋒，李凱璇，朱子文,2*，朱 磊，王海迪,2

（1.中北大學(xué)機(jī)電工程學(xué)院，太原 030051；2.中北大學(xué)智能武器研究院，太原 030051；3.西北工業(yè)集團(tuán)有限公司，西安 710043；4.北方自動(dòng)控制技術(shù)研究所，太原 030006）

0 引言

無人車是一種可以自主執(zhí)行預(yù)設(shè)程序的無人平臺(tái)，其功能包括但不限于環(huán)境感知、路徑規(guī)劃、運(yùn)動(dòng)控制、動(dòng)態(tài)決策等。無人車應(yīng)用廣泛，在工業(yè)、農(nóng)業(yè)等民用領(lǐng)域具有重要作用，在未來無人化戰(zhàn)場(chǎng)等軍事領(lǐng)域同樣也發(fā)揮著重要作用。路徑規(guī)劃技術(shù)在無人車運(yùn)動(dòng)中占據(jù)著很大比重，是無人車完成自主導(dǎo)航的核心技術(shù)。

A*算法是一種利用啟發(fā)信息尋找最優(yōu)路徑的啟發(fā)式算法。A*算法需要在地圖中搜索節(jié)點(diǎn)，并以適合的啟發(fā)函數(shù)進(jìn)行指導(dǎo)，通過評(píng)價(jià)各個(gè)節(jié)點(diǎn)的代價(jià)值，選取下一個(gè)代價(jià)值最小的節(jié)點(diǎn)作為拓展的最佳節(jié)點(diǎn)，反復(fù)迭代到達(dá)最終目標(biāo)位置［1］。A*算法對(duì)環(huán)境反應(yīng)迅速，搜索路徑方法直接，但實(shí)時(shí)性差，每一節(jié)點(diǎn)計(jì)算量大、運(yùn)算時(shí)間長(zhǎng)，而且節(jié)點(diǎn)數(shù)越多，算法搜索效率越低［1］，路徑越長(zhǎng)，平滑度越低。近年來，多位學(xué)者以及研究人員在改進(jìn)A*算法的研究中取得了一些進(jìn)展，高民東等提出了一種雙向時(shí)效A*算法規(guī)劃路徑，采用多近鄰柵格距離計(jì)算方案，達(dá)到提高效率、平滑路徑的效果，但得到的路徑較長(zhǎng)［2］?？桌^利等提出了一種引入雙向搜索機(jī)制的改進(jìn)A*算法，但得到的路徑與障礙物距離過近，易發(fā)生碰撞［3］。以上研究雖對(duì)A*算法進(jìn)行了改進(jìn)，但仍未能解決A*算法效率不高，路徑不夠平滑等問題，因此，部分學(xué)者利用其他算法改進(jìn)A*算法。陳繼清等提出了基于人工勢(shì)場(chǎng)的A*算法的移動(dòng)機(jī)器人路徑規(guī)劃，減小傳統(tǒng)A*算法規(guī)劃路徑的缺點(diǎn)，但帶來了人工勢(shì)場(chǎng)的局部震蕩問題［4］。楊明亮等提出了改進(jìn)A*算法和動(dòng)態(tài)窗口法相結(jié)合的混合算法，解決了使用A*算法擴(kuò)展節(jié)點(diǎn)多、容易陷入局部最優(yōu)的問題，但規(guī)劃時(shí)間較長(zhǎng)［5］。

本文提出一種改進(jìn)A*算法，結(jié)合蟻群算法的迭代特性，通過路徑距離及平滑程度等評(píng)價(jià)指標(biāo)對(duì)路徑進(jìn)行優(yōu)化改進(jìn)，提高無人車的路徑規(guī)劃能力。

1 環(huán)境模型建立與標(biāo)準(zhǔn)A*算法

1.1 環(huán)境模型建立

環(huán)境建模是利用相關(guān)的數(shù)學(xué)函數(shù)工具對(duì)環(huán)境信息進(jìn)行數(shù)學(xué)建模，建立無人車的可通行區(qū)域與禁止通行區(qū)域。本文采用HOWDEN W E 在1968 年提出的環(huán)境建模方法［6-8］。以柵格為單位表示環(huán)境信息，通過編碼表示柵格。障礙物區(qū)域使用黑色柵格表示，編碼為1，即禁止通行柵格；非障礙物區(qū)域使用白色柵格表示，編碼為0，即可通行柵格。柵格地圖模型示意圖和對(duì)應(yīng)的編碼環(huán)境矩陣分別如圖1和式（1）所示。

圖1 柵格地圖模型示意圖Fig.1 Schematic diagram of grid map model

為了兼顧路徑規(guī)劃的效率以及路徑規(guī)劃的準(zhǔn)確性，選擇無人車的最小運(yùn)動(dòng)面積作為柵格尺寸的大小。假設(shè)無人車所處的環(huán)境地圖為x、y的矩形區(qū)域，將其以m×n的標(biāo)準(zhǔn)進(jìn)行劃分，從而構(gòu)成柵格地圖。m表示行數(shù)，n表示列數(shù)。一般情況下，無人車的最小運(yùn)動(dòng)面積是以無人車的質(zhì)心為中心進(jìn)行原地旋轉(zhuǎn)所占用的面積［9］，柵格的長(zhǎng)度可表示為：

式中，p為柵格的長(zhǎng)度；l為無人車質(zhì)心至其最遠(yuǎn)點(diǎn)的距離。

1.2 標(biāo)準(zhǔn)A＊算法

A*算法是一種已知障礙物地圖下的最短路徑規(guī)劃算法，也是目前無人車路徑規(guī)劃方面使用最多的啟發(fā)式搜索算法［9］。A*算法的核心公式為：

式中，f(n)為無人車從起始位置到達(dá)目標(biāo)位置的估價(jià)函數(shù)；g(n)為無人車起始位置到當(dāng)前節(jié)點(diǎn)f(n)的實(shí)際代價(jià)函數(shù);h(n)為當(dāng)前節(jié)點(diǎn)n到路徑目標(biāo)的估計(jì)函數(shù)。

代價(jià)函數(shù)g(n)用歐氏距離表示為：

啟發(fā)函數(shù)有多種表示形式，最常用的表示方式為曼哈頓距離，如式（5）：

式中，（xn、yn）表示無人車當(dāng)前節(jié)點(diǎn)n的位置；（xE、yE）為無人車的目標(biāo)節(jié)點(diǎn)位置；n為常系數(shù)，一般定義為柵格間距。

2 改進(jìn)A*蟻群融合算法

2.1 路徑轉(zhuǎn)彎?rùn)C(jī)制改進(jìn)

在標(biāo)準(zhǔn)A*算法路徑規(guī)劃過程中，由于其代價(jià)函數(shù)的計(jì)算特點(diǎn)，往往會(huì)出現(xiàn)與相鄰的障礙物方格接觸的情形，此情形對(duì)應(yīng)現(xiàn)實(shí)的無人車會(huì)發(fā)生碰撞的事故，因此，必須對(duì)標(biāo)準(zhǔn)A*算法路徑規(guī)劃的轉(zhuǎn)彎?rùn)C(jī)制進(jìn)行改進(jìn)，通常的解決方法一般是給車輛與障礙物之間設(shè)置安全距離，但該方法可能會(huì)由于安全距離相互沖突將原本可通行的路徑封死，因此，本文從邏輯層面對(duì)轉(zhuǎn)彎?rùn)C(jī)制進(jìn)行改進(jìn)，避免無人車在路徑規(guī)劃過程中發(fā)生碰撞，對(duì)轉(zhuǎn)彎?rùn)C(jī)制的具體改進(jìn)如圖2和圖3所示。

圖2 不允許轉(zhuǎn)彎方式
Fig.2 No turning allowed

圖3 允許轉(zhuǎn)彎方式
Fig.3 Turning allowed

當(dāng)規(guī)劃路徑為斜向路徑時(shí)，只有當(dāng)前所在位置柵格與下一位置柵格的兩側(cè)柵格均不為障礙物柵格時(shí)，此規(guī)劃路徑才成立。根據(jù)上述改進(jìn)方法，可構(gòu)建轉(zhuǎn)移矩陣dij，如式（6）所示。

式中，為直向柵格標(biāo)號(hào)；為斜向柵格標(biāo)號(hào)；i'和i"分別為與斜向連線垂直的兩個(gè)直向柵格標(biāo)號(hào)；∞為無法前進(jìn)或出界的情況，mod(j,2)用于判斷j的奇偶性，并判斷下一步為直行或斜行。

2.2 基于改進(jìn)A＊蟻群的融合算法

蟻群算法是由意大利學(xué)者DORIGO等于1991年提出的一種模擬螞蟻覓食行為的模擬優(yōu)化算法，具有迭代特性，可以通過迭代優(yōu)化其他算法［9］。

2.2.1 信息素初始化與柵格選擇方式改進(jìn)

將標(biāo)準(zhǔn)A*算法得到的路徑記為R，并將路徑R上的信息素初始化為：

式中，N為大于1 的常數(shù)，其余區(qū)域的信息素初始化為常數(shù)C。

為了讓螞蟻更容易地選擇最佳的下一個(gè)柵格，對(duì)螞蟻選擇柵格的方式進(jìn)行改進(jìn)。常規(guī)蟻群算法的柵格選擇方式與標(biāo)準(zhǔn)A*算法的柵格選擇方式一致，對(duì)轉(zhuǎn)彎?rùn)C(jī)制進(jìn)行改進(jìn)后，標(biāo)準(zhǔn)A*算法的路徑長(zhǎng)度會(huì)有增加，在此基礎(chǔ)上利用常規(guī)蟻群算法對(duì)標(biāo)準(zhǔn)A*算法的路徑進(jìn)行迭代優(yōu)化，可能會(huì)沒有明顯的效果，因此，本文將常規(guī)蟻群算法的柵格選擇方式進(jìn)行改進(jìn)，如圖4所示。

圖4 蟻群算法柵格選擇方式改進(jìn)Fig.4 Improvement of grid selection method in ant colony algorithm

圖4中，藍(lán)色柵格為蟻群算法常規(guī)的選擇方式，以此為基礎(chǔ)另外添加8 個(gè)柵格，即圖4 中的綠色柵格，二者共同構(gòu)成蟻群算法新的柵格選擇方式。

2.2.2 啟發(fā)函數(shù)改進(jìn)

在新的柵格選擇方式的基礎(chǔ)上進(jìn)行柵格選擇，下一個(gè)柵格必須是可通行柵格，并且有多個(gè)出口指向目標(biāo)。這個(gè)概率取決于柵格周圍禁止通行柵格的數(shù)量。柵格周圍的禁止通行柵格越少，概率越大，柵格就越有吸引力，這里引入刺激概率Kij。某柵格j周圍禁止通行柵格所有可能分布情況的個(gè)數(shù)可以計(jì)算為：

其中，Nobs為柵格j周圍的禁止通行柵格個(gè)數(shù)。

柵格j的出口分布情況個(gè)數(shù)可以計(jì)算為：

刺激概率Kij計(jì)算如下：

新的啟發(fā)函數(shù)定義如下：

其中，djs為下一節(jié)點(diǎn)j與終點(diǎn)E之間的距離，增加了柵格j與目標(biāo)位置的距離對(duì)啟發(fā)函數(shù)的影響，同時(shí)增加了目標(biāo)位置對(duì)路徑的吸引力。Kij增加了禁止通行柵格個(gè)數(shù)對(duì)節(jié)點(diǎn)選擇的影響，節(jié)點(diǎn)j周圍的禁止通行柵格越多，Kij越小，啟發(fā)函數(shù)就越小，選擇該節(jié)點(diǎn)的概率就越小。

2.2.3 信息素更新方式改進(jìn)以及信息素?fù)]發(fā)因子調(diào)整

傳統(tǒng)蟻群算法信息素更新規(guī)則如下：

式中，ρ為信息素?fù)]發(fā)因子，ρ∈[0,1)；(t,t+n)表示經(jīng)過n次步驟后螞蟻完成一次遍歷；Δτij(t,t+n)表示節(jié)點(diǎn)i、j之間在完成一次遍歷后的信息素增量；Δ(t,t+n)表示螞蟻k在一次遍歷后，其通過路徑的信息素增量。Δ(t,t+n)按照式（14）進(jìn)行更新。

為使蟻群能更快地搜索到最優(yōu)路徑，本文將信息素更新規(guī)則修改如下：

若當(dāng)前路徑Lk長(zhǎng)度小于上次路徑Lk-1，則?。?，以增加Lk的信息素含量；若當(dāng)前路徑Lk長(zhǎng)度大于上次路徑Lk-1，則?。?，以減少Lk的信息素含量。

信息素?fù)]發(fā)因子ρ通常為常數(shù)，在全局搜索時(shí)ρ若為常數(shù)會(huì)降低搜索效率，現(xiàn)將ρ改進(jìn)為：

其中，GDmT、GDmT-1分別為第T次迭代與第T-1次迭代中平滑度參數(shù)平均值。將無人車路徑中的銳角、直角、鈍角分別賦予值3、2、1，路徑平滑度參數(shù)值即所有角度值之和。

引入平滑度參數(shù)來影響信息素的揮發(fā)，當(dāng)GDmT小于GDmT-1時(shí)，路徑更平滑，此時(shí)減小ρ值，加快算法收斂速度，當(dāng)GDmT大于GDmT-1時(shí)，增加ρ值，加強(qiáng)算法全局搜索能力。

2.3 基于改進(jìn)A＊蟻群融合算法的路徑規(guī)劃

改進(jìn)A*蟻群融合算法的路徑規(guī)劃步驟如下：

1）利用標(biāo)準(zhǔn)A*算法得到原始路徑，進(jìn)行信息素初始化；

2）初始化蟻群算法參數(shù)；

3）將蟻群放置在起始位置；

4）通過轉(zhuǎn)移概率，選擇蟻群的下一柵格；

5）其中一只螞蟻到達(dá)目標(biāo)位置后，更新信息素，重復(fù)步驟4），直到所有螞蟻到達(dá)目標(biāo)位置；

6）判斷是否達(dá)到迭代次數(shù)，若達(dá)到迭代次數(shù)，則輸出最優(yōu)路徑；若沒有，繼續(xù)返回步驟3）。

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

3.1 仿真條件

本實(shí)驗(yàn)建立的環(huán)境模型為30×30 的柵格地圖，如圖5所示。

圖5 柵格地圖Fig.5 Grid map

圖5（a）是簡(jiǎn)單地圖，障礙物簡(jiǎn)單分布在柵格地圖中，圖5（b）是復(fù)雜地圖，隨機(jī)生成動(dòng)態(tài)障礙物，隨機(jī)產(chǎn)生障礙柵格的概率為0.75［10］，障礙物較為復(fù)雜的分布在柵格地圖中。本文選擇的無人車質(zhì)心至其最遠(yuǎn)點(diǎn)的距離為0.5 m，每一柵格長(zhǎng)度為1 m。

在復(fù)雜陸戰(zhàn)場(chǎng)中環(huán)境是多變的，無人車需要隨時(shí)臨時(shí)調(diào)整［11］，若最優(yōu)路徑上出現(xiàn)新增障礙物，則需要重新進(jìn)行路徑規(guī)劃。本文在路徑規(guī)劃時(shí)，當(dāng)各路徑長(zhǎng)度差d滿足式（18）時(shí)，將當(dāng)前所有已知路徑存入數(shù)據(jù)庫(kù)內(nèi)，不再進(jìn)行路徑優(yōu)化。當(dāng)環(huán)境改變后，優(yōu)先在數(shù)據(jù)庫(kù)中搜索可行路徑，若無，則重新進(jìn)行路徑規(guī)劃。本文選擇路徑長(zhǎng)度差為3 m。

蟻群算法初始化參數(shù)設(shè)置如表1所示。

表1 蟻群算法初始化參數(shù)設(shè)置Table 1 Initialization parameters setting for ant colony optimization algorithm

其中，迭代次數(shù)以50 次為變化量進(jìn)行迭代次數(shù)增加，其余參數(shù)不變。

3.2 仿真結(jié)果與分析

本實(shí)驗(yàn)基于簡(jiǎn)單地圖和復(fù)雜地圖設(shè)計(jì)兩組實(shí)驗(yàn)，設(shè)置起點(diǎn)坐標(biāo)為（0.5，0.5），目標(biāo)點(diǎn)坐標(biāo)為（29.5，29.5）。未改進(jìn)轉(zhuǎn)彎?rùn)C(jī)制情況下，基于標(biāo)準(zhǔn)A*算法的路徑規(guī)劃仿真結(jié)果如圖6所示。

圖6 未改進(jìn)轉(zhuǎn)彎?rùn)C(jī)制的標(biāo)準(zhǔn)A*算法路徑規(guī)劃仿真結(jié)果Fig.6 The simulation results of standard A*algorithm path planning without improved turning mechanism

由圖6可以看出，未改進(jìn)轉(zhuǎn)彎?rùn)C(jī)制情況下，在簡(jiǎn)單地圖環(huán)境中，標(biāo)準(zhǔn)A*算法能夠成功完成搜索出從起始點(diǎn)到目標(biāo)點(diǎn)最佳路徑的任務(wù)，但是在復(fù)雜地圖環(huán)境中發(fā)生路徑跨越障礙物的情形，該情形在無人車路徑規(guī)劃過程中是不允許的。

因此，本文在標(biāo)準(zhǔn)A*算法的基礎(chǔ)上，修正A*算法的轉(zhuǎn)彎?rùn)C(jī)制，并引入蟻群算法。在簡(jiǎn)單地圖和復(fù)雜地圖兩種環(huán)境下，分別進(jìn)行100、400 和800 次迭代，對(duì)修正轉(zhuǎn)彎?rùn)C(jī)制的標(biāo)準(zhǔn)A*算法與改進(jìn)A*蟻群融合算法進(jìn)行仿真分析并對(duì)比，結(jié)果如圖7～下頁(yè)圖8所示。

圖7 簡(jiǎn)單地圖仿真結(jié)果Fig.7 The simulation results of a simple map

圖8 復(fù)雜地圖仿真結(jié)果Fig.8 The simulation results of a complex map

圖7～圖8 中藍(lán)色線條為修正轉(zhuǎn)彎?rùn)C(jī)制的標(biāo)準(zhǔn)A*算法，紅色線條為改進(jìn)A*蟻群融合算法。由仿真結(jié)果看出，修正轉(zhuǎn)彎?rùn)C(jī)制的標(biāo)準(zhǔn)A*算法與改進(jìn)A*蟻群融合算法，在復(fù)雜地圖中均能成功避開障礙物，完成搜索出從起始點(diǎn)到目標(biāo)點(diǎn)最佳路徑的任務(wù)。

改進(jìn)A*蟻群融合算法進(jìn)行800 次迭代的路徑長(zhǎng)度如圖9～圖10所示。

圖9 簡(jiǎn)單地圖中迭代800次路徑長(zhǎng)度Fig.9 The path length after 800 iterations in a simple map

圖10 復(fù)雜地圖中迭代800次路徑長(zhǎng)度Fig.10 The path length after 800 iterations in a complex map

由圖9～圖10 可知，在一定的迭代范圍內(nèi)，迭代次數(shù)越多，路徑長(zhǎng)度越短，總轉(zhuǎn)彎角度越小，路徑越平滑。但隨著迭代次數(shù)的增加，節(jié)點(diǎn)數(shù)量和運(yùn)行時(shí)間也隨之增加，且路徑的優(yōu)化效果并不明顯。

統(tǒng)計(jì)兩組實(shí)驗(yàn)的路徑長(zhǎng)度和總轉(zhuǎn)彎角度如下頁(yè)表2和表3所示。

表2 簡(jiǎn)單地圖仿真數(shù)據(jù)統(tǒng)計(jì)結(jié)果Table 2 The statistical results of simulating data in a simple map

表3 復(fù)雜地圖仿真數(shù)據(jù)統(tǒng)計(jì)結(jié)果Table 3 The statistical results of simulating data in a complex map

由以上兩組仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比可知，改進(jìn)A*轉(zhuǎn)彎?rùn)C(jī)制可以避免碰撞障礙物的情況，但帶來路徑變長(zhǎng)、總轉(zhuǎn)彎角度變大的問題。利用改進(jìn)A*蟻群融合算法進(jìn)行迭代優(yōu)化后，路徑長(zhǎng)度變短、總轉(zhuǎn)彎角度也變小，使得路徑更加平滑，在實(shí)際應(yīng)用中，無人車將使用更少的能量，花費(fèi)更短的時(shí)間，完成從起始位置到目標(biāo)位置的任務(wù)。意味著改進(jìn)后的A*蟻群融合算法在實(shí)際應(yīng)用中更高效。

綜合對(duì)比仿真結(jié)果可知，400 次迭代的效果較好。以400 次迭代優(yōu)化的仿真數(shù)據(jù)可知，簡(jiǎn)單地圖中，改進(jìn)A*蟻群融合算法相較于改進(jìn)轉(zhuǎn)彎?rùn)C(jī)制后的A*算法路徑規(guī)劃長(zhǎng)度減少2.34%，總轉(zhuǎn)彎角度減小5.62%；復(fù)雜地圖中，改進(jìn)A*蟻群融合算法相較于改進(jìn)轉(zhuǎn)彎?rùn)C(jī)制后的A*算法路徑規(guī)劃長(zhǎng)度減少2.62%，總轉(zhuǎn)彎角度減小26.3%。

4 結(jié)論

本文主要針對(duì)標(biāo)準(zhǔn)A*路徑規(guī)劃算法展開研究，利用柵格地圖法進(jìn)行環(huán)境建模，針對(duì)標(biāo)準(zhǔn)A*算法進(jìn)行全局路徑規(guī)劃時(shí)發(fā)生碰撞障礙物的問題，改進(jìn)其轉(zhuǎn)彎?rùn)C(jī)制，同時(shí)又針對(duì)其路徑較長(zhǎng)、路徑不夠平滑等問題，引入蟻群算法，并引入刺激概率對(duì)蟻群算法啟發(fā)函數(shù)進(jìn)行改進(jìn)，引入自適應(yīng)信息素更新方式改進(jìn)其更新規(guī)則，最后通過仿真實(shí)驗(yàn)驗(yàn)證改進(jìn)A*蟻群融合算法的優(yōu)越性。仿真結(jié)果表明，基于改進(jìn)A*蟻群融合算法的路徑規(guī)劃方法能夠避開障礙物，且路徑長(zhǎng)度和路徑平滑度得到改善。該算法能夠?yàn)槲磥頍o人戰(zhàn)場(chǎng)上無人車在復(fù)雜環(huán)境中的自主行駛提供理論依據(jù)。