改進(jìn)A＊算法的水面艦艇靜態(tài)航路規(guī)劃

武善平，黃炎焱，陳天德

南京理工大學(xué) 自動(dòng)化學(xué)院，南京 210094

由于海洋環(huán)境的復(fù)雜性，航路規(guī)劃是船舶智能引導(dǎo)的重要組成部分，主要功能是在已知船舶準(zhǔn)確位置以及周圍靜態(tài)障礙物信息的環(huán)境中，搜索一條從起點(diǎn)到終點(diǎn)的、滿足一定要求的航行路徑，使船舶在航行過程中能夠安全可靠地避開所有障礙區(qū)域并且使航路盡可能短、盡可能平滑。

路徑規(guī)劃問題較早應(yīng)用于自主移動(dòng)機(jī)器人，通過自動(dòng)推理、全局規(guī)劃、移動(dòng)控制等過程，實(shí)現(xiàn)機(jī)器人的自主導(dǎo)航移動(dòng)。A*算法是一種控制性能好且發(fā)展成熟的方法，是移動(dòng)機(jī)器人全局路徑規(guī)劃的常用算法之一[1]。在靜態(tài)場景中，A*算法能更快更有效地求解出最優(yōu)路徑，因此被廣泛用于未知環(huán)境的全局路徑規(guī)劃，但也存在計(jì)算量大、效率低、路徑拐點(diǎn)不平滑等缺陷[1]。

在靜態(tài)環(huán)境下，A*算法具有最優(yōu)性、完備性，但在隨著地圖數(shù)據(jù)變大搜索速度變慢。很多學(xué)者對(duì)A*算法進(jìn)行許多改進(jìn)，探索更高效的算法。文獻(xiàn)[2]通過增加h(n)的權(quán)重來提高A*算法的搜索性能；文獻(xiàn)[3]將柵格模型優(yōu)化為無障礙、靜態(tài)障礙以及動(dòng)態(tài)障礙三種移動(dòng)環(huán)境下進(jìn)行路徑規(guī)劃；文獻(xiàn)[4]提出一種動(dòng)態(tài)改變步長的快速A*算法；文獻(xiàn)[5]提出一種基于關(guān)鍵點(diǎn)選取的策略來代替?zhèn)鹘y(tǒng)A*算法中的Openlist和Closelist兩個(gè)列表；文獻(xiàn)[6]通過加入引導(dǎo)量對(duì)啟發(fā)函數(shù)進(jìn)行改進(jìn)，減少具有相同估價(jià)值的網(wǎng)格數(shù)量來優(yōu)化算法搜索效率；文獻(xiàn)[7]提出了一種對(duì)啟發(fā)函數(shù)h(n)結(jié)合了分區(qū)距離信息和角度變量信息加權(quán)的改進(jìn)啟發(fā)函數(shù)；文獻(xiàn)[8]在改進(jìn)啟發(fā)函數(shù)中加入了n個(gè)父輩的信息，并根據(jù)待擴(kuò)展節(jié)點(diǎn)和目標(biāo)點(diǎn)相對(duì)位置選擇擴(kuò)展象限；文獻(xiàn)[9]在啟發(fā)函數(shù)中引入當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)啟發(fā)函數(shù)；文獻(xiàn)[10-11]引入了雙向搜索機(jī)制，以原始起點(diǎn)、終點(diǎn)和對(duì)向搜索所處的當(dāng)前節(jié)點(diǎn)作為目標(biāo)點(diǎn)進(jìn)行搜索操作。

針對(duì)A*算法在大地圖中復(fù)雜障礙物環(huán)境下存在數(shù)據(jù)量大、尋路效率低、航路不平滑等問題，本文提出一種高效的改進(jìn)A*算法。首先，改進(jìn)A*算法的啟發(fā)函數(shù)，加入自適應(yīng)的啟發(fā)信息，以提高尋路效率；然后，加入安全距離，保證了航路的安全性；最后，對(duì)原始路徑進(jìn)行二次優(yōu)化，進(jìn)一步縮短了航路的長度并提高了航路的平滑性。

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

1.1 地圖建模

地圖建模是路徑規(guī)劃的重要環(huán)節(jié)，目的是將實(shí)際物理環(huán)境抽象成便于計(jì)算機(jī)存儲(chǔ)和處理的地圖模型，實(shí)現(xiàn)從物理環(huán)境到虛擬地圖的映射[12-13]。在常用的地圖建模方法中，柵格地圖具有簡單有效、易于實(shí)現(xiàn)、便于更新的特點(diǎn)，是目前研究和應(yīng)用最廣泛的方法。

傳統(tǒng)A*算法一般采用正方形柵格來進(jìn)行地圖建模[14]，在正方形柵格地圖中，當(dāng)前節(jié)點(diǎn)的鄰居節(jié)點(diǎn)通常為8鄰域，節(jié)點(diǎn)可以沿水平、豎直或?qū)蔷€方向移動(dòng)。如圖1所示，地圖中包含障礙物的柵格標(biāo)記為障礙柵格（黑色柵格），表示不可通行的區(qū)域；地圖中不包含障礙物的柵格標(biāo)記為自由柵格（白色柵格），表示可以通行的區(qū)域。

圖1 柵格地圖Fig.1 Raster map

為利用真實(shí)的海洋環(huán)境實(shí)現(xiàn)全局航路規(guī)劃，通過解析電子海圖提取的海洋環(huán)境信息通常由復(fù)雜幾何圖形組成，多數(shù)路徑規(guī)劃算法不能直接使用[15-16]。因此，需要采用柵格法建立基于電子海圖的海洋環(huán)境模型。首先解析電子海圖，提取其中的海域地理信息。然后利用正方形柵格網(wǎng)格劃分進(jìn)行網(wǎng)格化，建立由可航行網(wǎng)格和不可航行網(wǎng)格組成的海洋環(huán)境模型。對(duì)電子海圖的海洋環(huán)境信息進(jìn)行柵格化可以提高路徑規(guī)劃算法的效率[4，6-7]。

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

A*算法是全局路徑規(guī)劃中一種啟發(fā)式搜索算法，在Dijkstra算法的基礎(chǔ)上引入了啟發(fā)式函數(shù)h(n)[3]，h(n)表示了當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)。在保證了路徑最優(yōu)性的同時(shí)，加入了目標(biāo)節(jié)點(diǎn)的距離信息，提升了搜索效率。其估價(jià)函數(shù)表示為：

式中，g(n)表示累積代價(jià)，即對(duì)從初始節(jié)點(diǎn)到節(jié)點(diǎn)n的累計(jì)真實(shí)代價(jià)；h(n)表示目標(biāo)代價(jià)：即節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)；f(n)表示估價(jià)函數(shù)，即從初始節(jié)點(diǎn)經(jīng)過當(dāng)前節(jié)點(diǎn)n，再到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)。

1.3 算法流程

A*算法通過代價(jià)估計(jì)函數(shù)f(n)進(jìn)行路徑規(guī)劃，從初始節(jié)點(diǎn)向附近鄰居節(jié)點(diǎn)進(jìn)行擴(kuò)展，將不超過地圖的非障礙物鄰居節(jié)點(diǎn)加入OpenList。然后選取OpenList中f(n)值最小的節(jié)點(diǎn)選為下一父節(jié)點(diǎn)，并將搜索過的節(jié)點(diǎn)加入CloseList。重復(fù)此過程直到搜索到目標(biāo)節(jié)點(diǎn)，然后沿著父節(jié)點(diǎn)的方向進(jìn)行回溯生成最終路徑，完成路徑規(guī)劃[2-4]。傳統(tǒng)A*算法的偽代碼如下所示：

A*算法在擴(kuò)展鄰居節(jié)點(diǎn)時(shí)會(huì)把當(dāng)前節(jié)點(diǎn)所有鄰居節(jié)點(diǎn)都考慮進(jìn)去，該過程中會(huì)生成大量與最終路徑無關(guān)的節(jié)點(diǎn)，或者導(dǎo)致能夠通過但是花費(fèi)的代價(jià)較高的情況，從而消耗大量時(shí)間，所以提高路徑規(guī)劃效率是本文的研究點(diǎn)之一。

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

傳統(tǒng)A*算法能夠?qū)δ繕?biāo)點(diǎn)進(jìn)行有效的全局路徑規(guī)劃，但所規(guī)劃出的路徑存在冗余節(jié)點(diǎn)、轉(zhuǎn)折點(diǎn)多、距離障礙物過近且轉(zhuǎn)折角度過大等問題。針對(duì)這些問題，從3個(gè)方面對(duì)A*算法進(jìn)行改進(jìn)。一是改進(jìn)估價(jià)函數(shù)加快航路搜索速度；二是加入安全距離來保證航路的安全性；三是刪除冗余節(jié)點(diǎn)然后使用貝塞爾曲線平滑轉(zhuǎn)折節(jié)點(diǎn)來減少航路轉(zhuǎn)折次數(shù)和航路長度，增加航路的平滑度。

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

在A*算法中，用于估計(jì)節(jié)點(diǎn)n的啟發(fā)代價(jià)值h(n)決定著A*算法對(duì)其他節(jié)點(diǎn)的擴(kuò)展速度及所擴(kuò)展節(jié)點(diǎn)是否合理。h*(n)是指節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的真實(shí)代價(jià)，h(n)與h*(n)的大小影響算法的精度和速度。A*算法能夠找到最短路徑的原則為h(n)的值不大于h*(n)[1]。

當(dāng)h(n)=h*(n)時(shí)，A*算法可以兼顧精度和速度，是A*算法的最優(yōu)狀態(tài)，但這個(gè)h(n)一般不容易找到；當(dāng)h(n)＜h*(n)時(shí)，A*算法搜索效率略低，h(n)越小，意味著需要擴(kuò)展的節(jié)點(diǎn)就越多，效率上越低，但是精度上越準(zhǔn)確。當(dāng)h(n)=0時(shí)，A*算法退化為Dijkstra算法[7]。

2.1.1 節(jié)點(diǎn)距離

傳統(tǒng)A*算法一般采用歐氏距離或曼哈頓距離來計(jì)算h(n)[9]，但是由于歐氏距離計(jì)算的h(n)很多情況下都小于柵格地圖的真實(shí)目標(biāo)代價(jià)h*(n)，勢必會(huì)擴(kuò)展很多不必要的節(jié)點(diǎn)，且往往造成所計(jì)算的距離過大或過小。為此，本文采用一種更加適合柵格地圖的距離計(jì)算方法。

對(duì)于地圖上任意兩個(gè)不同坐標(biāo)節(jié)點(diǎn)P1(x1,y1)和P2(x2,y2)，定義x軸和y軸的絕對(duì)距離分別為：

在無障礙物環(huán)境中，柵格地圖中的兩個(gè)節(jié)點(diǎn)之間的移動(dòng)距離都可以用圖2中兩條折線的距離相加求得。

圖2 柵格地圖節(jié)點(diǎn)間實(shí)際距離Fig.2 Actual distance between grid map nodes

在柵格地圖中，一般設(shè)置橫向和縱向移動(dòng)一個(gè)單元的距離為1，斜向移動(dòng)一個(gè)單元的距離為2。很容易得到兩個(gè)節(jié)點(diǎn)坐標(biāo)之間的距離計(jì)算公式為：

這種距離也叫切比雪夫距離或棋盤距離。

2.1.2 目標(biāo)方位信息

由A*算法具體搜索過程可知，導(dǎo)致其效率低的一個(gè)重要原因如圖3所示：對(duì)于無障礙物情況下，真實(shí)目標(biāo)代價(jià)h*(n)確定的路徑是閉氏解，往往存在多條等價(jià)路徑，而實(shí)際上從起點(diǎn)到終點(diǎn)只需要取其中一條路徑即可。選擇f(n)值最小的點(diǎn)作為下一個(gè)擴(kuò)展節(jié)點(diǎn)時(shí)，總是會(huì)出現(xiàn)往返搜索的情況。

圖3 閉式解路徑Fig.3 Closed-form solution path

對(duì)于這種情形，可以考慮加入目標(biāo)節(jié)點(diǎn)的方位代價(jià)來打破對(duì)稱性，讓A*算法具有保持當(dāng)前方向的傾向性，可以減少擴(kuò)展沒必要的節(jié)點(diǎn)并平滑路徑。其次，為提高運(yùn)動(dòng)平穩(wěn)性，在路徑規(guī)劃過程中，不僅要減小轉(zhuǎn)角，也要盡可能減少機(jī)器人轉(zhuǎn)向次數(shù)。

傳統(tǒng)A*算法中啟發(fā)信息h(n)只包含了當(dāng)前點(diǎn)到目標(biāo)節(jié)點(diǎn)的距離信息，這也導(dǎo)致出現(xiàn)多條冗余路徑，是A*算法在大地圖數(shù)據(jù)量爆炸時(shí)尋路速度變慢的原因之一。在任意節(jié)點(diǎn)擴(kuò)展鄰居節(jié)點(diǎn)的時(shí)候，不僅希望路徑的長度最短，還總是希望路徑能夠傾向朝著目標(biāo)節(jié)點(diǎn)的前進(jìn)?；诖吮疚脑趩l(fā)信息中加入當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的方位信息，來引導(dǎo)A*算法偏向拓展分布在當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)連線附近的節(jié)點(diǎn)。

取當(dāng)前節(jié)點(diǎn)(xnode,ynode)指向目標(biāo)節(jié)點(diǎn)(xgoal,ygoal)的引導(dǎo)向量a為：

如圖4所示，當(dāng)前節(jié)點(diǎn)指向鄰居節(jié)點(diǎn)的向量為可行向量ti。引導(dǎo)向量a與可行向量ti的夾角θ可以用來表示目標(biāo)節(jié)點(diǎn)的方位信息。夾角θ越小，說明該鄰居節(jié)點(diǎn)位于更靠近目標(biāo)節(jié)點(diǎn)的方位；反之，則說明該鄰居節(jié)點(diǎn)位于更遠(yuǎn)離目標(biāo)節(jié)點(diǎn)的方位。

圖4 引導(dǎo)向量與可行向量的夾角Fig.4 Angle between guiding vector and feasible vector

考慮到計(jì)算方便以及cos函數(shù)在夾角取值范圍內(nèi)的單調(diào)性，本文采用引導(dǎo)向量a與可行向量ti的夾角θ余弦值來計(jì)算方位信息，其計(jì)算公式為：

對(duì)于柵格地圖中搜索路徑的任意節(jié)點(diǎn)都可以分為兩種情況，前方有障礙物和前方?jīng)]有障礙物，如圖5所示。

本文根據(jù)尋路過程將節(jié)點(diǎn)的狀態(tài)分為前方存在障礙物和無障礙物兩個(gè)狀態(tài)。區(qū)分方法就是，從當(dāng)前節(jié)點(diǎn)沿著引導(dǎo)向量的方向前方距離為dmin（一般是給定的安全距離）范圍內(nèi)的節(jié)點(diǎn)(xi,yi)，判斷這些節(jié)點(diǎn)中是否存在障礙物：

式中，round表示四舍五入函數(shù)。

如圖5（a）所示，在無障礙環(huán)境中（預(yù)測節(jié)點(diǎn)(xi,yi)中不存在障礙物），為了加快搜索航路的速度，搜索能力偏向深度優(yōu)先搜索，搜索節(jié)點(diǎn)的期望方向總是向著目標(biāo)點(diǎn)的方向（紅色箭頭方向）進(jìn)行，這時(shí)可以設(shè)置啟發(fā)信息為：

如圖5（b）所示，在遇到障礙物時(shí)（預(yù)測節(jié)點(diǎn)(xi,yi)中存在障礙物），為了加快繞開障礙物，搜索能力偏向廣度優(yōu)先搜索，搜索節(jié)點(diǎn)時(shí)的期望方向總是向著引導(dǎo)向量的兩側(cè)方向（藍(lán)色箭頭方向）進(jìn)行，這時(shí)可以設(shè)置啟發(fā)信息為：

圖5 柵格地圖中的兩種情況Fig.5 Two situations in grid map

可以得到柵格地圖中的自適應(yīng)啟發(fā)函數(shù)為：

綜上，改進(jìn)的A*算法估價(jià)函數(shù)為：

為了驗(yàn)證改進(jìn)啟發(fā)函數(shù)搜索路徑的快速性，使用傳統(tǒng)A*算法和改進(jìn)A*算法進(jìn)行了50次隨機(jī)實(shí)驗(yàn)，在相同地圖中的起點(diǎn)和終點(diǎn)下，仿真時(shí)間對(duì)比如圖6所示。

圖6 算法時(shí)間對(duì)比Fig.6 Comparison of algorithm pathfinding time

從圖6可以看出，改進(jìn)啟發(fā)函數(shù)后的算法都比傳統(tǒng)A*算法更快，具有更加高效的尋路速度。

2.2 安全距離

在傳統(tǒng)A*算法中，航路存在大量緊貼著障礙物的危險(xiǎn)節(jié)點(diǎn)。若在實(shí)際航路跟蹤時(shí)，艦艇沿著障礙物邊緣行進(jìn)時(shí)，安全性較差。針對(duì)這個(gè)問題，在搜索航路時(shí)需要設(shè)置一定的安全距離，迫使航路遠(yuǎn)離障礙物。

本文提出一種保持航路與障礙物安全距離的方法。在對(duì)某個(gè)節(jié)點(diǎn)搜索鄰居節(jié)點(diǎn)時(shí)，若已經(jīng)判斷該鄰居節(jié)點(diǎn)可以擴(kuò)展，則沿著當(dāng)前方向ti繼續(xù)搜索dmin（最短安全距離）步，判斷到達(dá)的節(jié)點(diǎn)是否為障礙物。若該節(jié)點(diǎn)是障礙物，則直接放棄這個(gè)鄰居節(jié)點(diǎn)；若不是，則將該鄰居節(jié)點(diǎn)加入OpenLsit。這樣可以使得航路跳過那些距離障礙物小于安全距離的節(jié)點(diǎn)，保證了航路的安全性。

2.3 二次航路優(yōu)化

在傳統(tǒng)的A*算法中，路徑只進(jìn)行一次規(guī)劃，得到航路是一條在A*算法模型約束條件下的最優(yōu)路徑。由于在搜尋節(jié)點(diǎn)的過程中限制了節(jié)點(diǎn)的移動(dòng)方向，只能向周邊八個(gè)點(diǎn)進(jìn)行擴(kuò)展搜索，使得路徑中依然存在著冗余路徑點(diǎn)，并且轉(zhuǎn)折次數(shù)多且很多拐點(diǎn)處轉(zhuǎn)向難，使艦艇行駛轉(zhuǎn)彎效率低下，不能很好地跟蹤所規(guī)劃的路徑。因此對(duì)原始航路進(jìn)行二次優(yōu)化，對(duì)冗余節(jié)點(diǎn)進(jìn)行刪除，對(duì)轉(zhuǎn)折處進(jìn)行平滑，獲得由起點(diǎn)、終點(diǎn)和關(guān)鍵轉(zhuǎn)折點(diǎn)組成的平滑航路。

2.3.1 提取路徑轉(zhuǎn)折點(diǎn)

A*算法得到的原始路徑存在大量的冗余節(jié)點(diǎn)，占據(jù)了大部分的非必要內(nèi)存。因此，需要對(duì)路徑轉(zhuǎn)折點(diǎn)進(jìn)行提取，只保留起點(diǎn)、終點(diǎn)和路徑轉(zhuǎn)折點(diǎn)。若原始航路在某一段路徑上保持同一個(gè)方向趨勢不變時(shí)，可以認(rèn)為除了兩端之外的節(jié)點(diǎn)都是冗余節(jié)點(diǎn)。遍歷每一個(gè)航路節(jié)點(diǎn)，計(jì)算父節(jié)點(diǎn)進(jìn)入它的相對(duì)方向（子節(jié)點(diǎn)在父節(jié)點(diǎn)鄰域中編號(hào)），起點(diǎn)的方向默認(rèn)為0。

若航路是一條線段時(shí)，即在一段路徑上航路的方向保持不變并且航路長度大于閾值時(shí)，則保留這段航路的起點(diǎn)和終點(diǎn)；若航路是一條折線段時(shí)，即在一段路徑上節(jié)點(diǎn)的方向出現(xiàn)反復(fù)震蕩，則判斷震蕩的起點(diǎn)和終點(diǎn)是否能直線到達(dá)，若可以，則保留震蕩航路的起點(diǎn)和終點(diǎn)。最終獲得只包括起點(diǎn)、終點(diǎn)和轉(zhuǎn)折點(diǎn)的航路。

2.3.2 優(yōu)選關(guān)鍵轉(zhuǎn)折點(diǎn)

由起點(diǎn)、終點(diǎn)和轉(zhuǎn)折點(diǎn)表示的航路可以大幅度減少航路的冗余節(jié)點(diǎn)，但還在冗余轉(zhuǎn)折點(diǎn)會(huì)導(dǎo)致航路不必要轉(zhuǎn)彎，航路的實(shí)際距離并不是最優(yōu)。如圖7所示，黑色航路是在柵格地圖中規(guī)劃的最優(yōu)航路；而在實(shí)際應(yīng)用中，若兩節(jié)點(diǎn)直線可達(dá)，艦艇只需沿著紅色航線的路線便可進(jìn)一步減少轉(zhuǎn)向次數(shù)和航程。因此，需要采用Dijkstra算法來刪除冗余轉(zhuǎn)折點(diǎn)，優(yōu)選關(guān)鍵轉(zhuǎn)折點(diǎn)，減少航路長度和航路轉(zhuǎn)折次數(shù)。

圖7 柵格地圖航路與實(shí)際航線對(duì)比Fig.7 Comparison of grid map route and actual route

Dijkstra（迪杰斯特拉）算法是典型的單源最短路徑算法，用于計(jì)算一個(gè)節(jié)點(diǎn)到其他所有節(jié)點(diǎn)的最短路徑。主要特點(diǎn)是以起始點(diǎn)為中心向外層層擴(kuò)展，直到擴(kuò)展到終點(diǎn)為止。

為了進(jìn)一步優(yōu)化航路，對(duì)提取的轉(zhuǎn)折點(diǎn)之間進(jìn)行直線可達(dá)性判斷。遍歷每一個(gè)轉(zhuǎn)折節(jié)點(diǎn)，若兩個(gè)轉(zhuǎn)折節(jié)點(diǎn)之間可以直線到達(dá)，則記錄它們之間的實(shí)際距離（歐式距離）；若不是直線可達(dá)，則記錄它們的距離為無窮大。將所有可以直線互通的節(jié)點(diǎn)距離轉(zhuǎn)換為一個(gè)無向圖（距離矩陣）。然后使用Dijkstra算法刪除航路中不必要的轉(zhuǎn)折點(diǎn)，優(yōu)取從起點(diǎn)到終點(diǎn)的最短路徑的關(guān)鍵轉(zhuǎn)折點(diǎn)。這樣進(jìn)一步縮短航路長度，并減少航路的轉(zhuǎn)折次數(shù)。

2.3.3 貝塞爾曲線平滑路徑

由關(guān)鍵轉(zhuǎn)折點(diǎn)組成的路徑轉(zhuǎn)折次數(shù)大幅度減小，當(dāng)由于轉(zhuǎn)折處都是折線，還存在且拐點(diǎn)處轉(zhuǎn)向難、轉(zhuǎn)彎效率低下的問題。因此，本文采用貝塞爾曲線對(duì)由關(guān)鍵轉(zhuǎn)折點(diǎn)進(jìn)行平滑處理。

貝塞爾曲線由于操作簡單且有極強(qiáng)的圖像平滑能力，因此在圖形設(shè)計(jì)和路徑規(guī)劃中的應(yīng)用都非常廣泛。貝塞爾曲線完全由其控制點(diǎn)（端點(diǎn)）決定其形狀，n個(gè)控制點(diǎn)對(duì)應(yīng)著n-1階的貝塞爾曲線。通用的n階貝塞爾曲線的公式為：

式中，n是階數(shù)，t是參數(shù)，取值范圍是[0，1]。

為了保證航路在拐點(diǎn)處能夠平滑地轉(zhuǎn)向并且不失真，本文使用二階貝塞爾曲線對(duì)關(guān)鍵轉(zhuǎn)折點(diǎn)的每一個(gè)轉(zhuǎn)折處進(jìn)行平滑處理。

如圖8所示，二階貝塞爾曲線需要三個(gè)端點(diǎn)來控制，對(duì)應(yīng)航路上構(gòu)成一個(gè)轉(zhuǎn)折處的三個(gè)節(jié)點(diǎn)P0、P1和P2。在線段P0P1和P1P2分別取兩個(gè)動(dòng)點(diǎn)并使它們滿足如下比例關(guān)系：

圖8 二階貝塞爾曲線Fig.8 Second-order Bezier curve

然后過點(diǎn)P0和P2做一條與相切的拋物線，相切點(diǎn)為，并滿足如下比例等式：

引入?yún)?shù)t，令式（13）的比值為t:(1-t)，則：

當(dāng)t從0變到1，即點(diǎn)向點(diǎn)P1移動(dòng)、點(diǎn)向點(diǎn)P2移動(dòng)時(shí)，點(diǎn)的移動(dòng)軌跡就是由三節(jié)點(diǎn)P0、P1和P2三點(diǎn)確定的一條二階Bezier曲線，這就是式（11）的二階形式：

二次航路優(yōu)化的總體效果如圖9所示，綠色節(jié)點(diǎn)是原始航路，藍(lán)色節(jié)點(diǎn)是提取的路徑轉(zhuǎn)折點(diǎn)，紅色節(jié)點(diǎn)是由Dijkstra算法篩選的關(guān)鍵轉(zhuǎn)折點(diǎn)，紅色路徑是使用貝塞爾曲線平滑的最終路徑。可以看到經(jīng)過二次優(yōu)化后的航路轉(zhuǎn)折次數(shù)大幅減少，縮短了路徑長度，提高了軌跡的平滑度，且增加了航路與障礙物之間的距離。

圖9 二次航路優(yōu)化效果Fig.9 Optimization of secondary route

2.4 算法流程

改進(jìn)算法在傳統(tǒng)A*算法的基礎(chǔ)首先增加的非障礙鄰居節(jié)點(diǎn)是否滿足安全距離判斷，篩選出大于安全距離的非障礙鄰居節(jié)點(diǎn)；其次增加了搜索路徑時(shí)當(dāng)前節(jié)點(diǎn)前方有無障礙物判斷，并根據(jù)判斷結(jié)果采用對(duì)應(yīng)的公式計(jì)算非障礙鄰居節(jié)點(diǎn)的f(n)，并記錄父節(jié)點(diǎn)進(jìn)入當(dāng)前節(jié)點(diǎn)的相對(duì)方向；最后，對(duì)原始航路進(jìn)行二次優(yōu)化，包括根據(jù)節(jié)點(diǎn)的父節(jié)點(diǎn)進(jìn)入方向提取航路轉(zhuǎn)折點(diǎn)、判斷任意兩個(gè)航路轉(zhuǎn)折點(diǎn)的直線可達(dá)性生成距離矩陣，根據(jù)距離矩陣采用Dijkstra算法刪除冗余航路轉(zhuǎn)折點(diǎn)、對(duì)優(yōu)選后的航路轉(zhuǎn)折點(diǎn)采用二階貝塞爾曲線進(jìn)行平滑處理等操作。改進(jìn)算法偽代碼如下所示：

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

為了驗(yàn)證本文提出的改進(jìn)A*算法的航路搜索效率和航路優(yōu)化效果，結(jié)合電子海圖進(jìn)行仿真實(shí)驗(yàn)。在硬件平臺(tái)為i5-7500，主頻3.4 GHz，運(yùn)行內(nèi)存8 GB的臺(tái)式機(jī)，軟件平臺(tái)為Matlab R2018b的實(shí)驗(yàn)平臺(tái)上進(jìn)行實(shí)驗(yàn)，并與傳統(tǒng)A*算法進(jìn)行比較。

首先，從電子海圖中獲取海洋環(huán)境信息，使用正方形柵格建立的環(huán)境模型如圖11所示，白色區(qū)域?yàn)榭珊叫袇^(qū)域，黑色區(qū)域?yàn)椴豢珊叫袇^(qū)域。建立以水平向右為正方向的x軸，豎直向下為正方向的y軸的笛卡爾坐標(biāo)系，網(wǎng)格數(shù)為200×87，一個(gè)網(wǎng)格即為一個(gè)節(jié)點(diǎn)。

圖10 改進(jìn)A*算法流程圖Fig.10 Chart of improved A*algorithm process

圖11 電子海圖柵格建模圖Fig.11 Grid modeling diagram of electronic chart

如圖12所示，是在兩組柵格化電子海圖環(huán)境模型下的航路結(jié)果對(duì)比。藍(lán)色航路為傳統(tǒng)A*算法規(guī)劃結(jié)果，紅色航路為改進(jìn)A*算法規(guī)劃結(jié)果。

圖12 算法航路對(duì)比圖Fig.12 Comparison of route of two algorithms

從圖12（a）和表1的統(tǒng)計(jì)數(shù)據(jù)可以明顯看出，改進(jìn)算法的航路比傳統(tǒng)算法的航路轉(zhuǎn)折點(diǎn)更少，轉(zhuǎn)彎處更加平滑，距離障礙物更遠(yuǎn)，使得艦艇更加容易進(jìn)行跟蹤。

表1 改進(jìn)A*算法和傳統(tǒng)A*算法航路規(guī)劃結(jié)果Table 1 Comparison of traditional and improved A*algorithm

從圖12（b）和表1的統(tǒng)計(jì)數(shù)據(jù)可以看出，改進(jìn)算法的航路比傳統(tǒng)算法的航路長度略長，但避開了狹窄通道，安全性更高，轉(zhuǎn)折點(diǎn)更少，航路更加平滑。

4 總結(jié)

針對(duì)復(fù)雜的海洋工作環(huán)境下，傳統(tǒng)A*算法規(guī)劃航路時(shí)出現(xiàn)的規(guī)劃速度慢、沿障礙物邊緣走安全性較差以及航路轉(zhuǎn)折過多且不平滑等問題，本文結(jié)合柵格化后的電子海圖環(huán)境模型，提出了一種加入目標(biāo)方位信息的自適應(yīng)啟發(fā)函數(shù)改進(jìn)A*算法，然后刪除冗余節(jié)點(diǎn)后使用Dijkstra算法進(jìn)一步優(yōu)選航路的關(guān)鍵轉(zhuǎn)折點(diǎn)，最后再使用貝塞爾曲線平滑法對(duì)航路轉(zhuǎn)折處進(jìn)一步優(yōu)化。仿真實(shí)驗(yàn)結(jié)果表明，相較于傳統(tǒng)A*算法，改進(jìn)算法能準(zhǔn)確地、迅速地在給定地圖中內(nèi)任意兩個(gè)節(jié)點(diǎn)之間生成安全無碰撞且平滑全局航路，較傳統(tǒng)A*算法優(yōu)勢明顯。