基于正六邊形建模的無(wú)人水面艇路徑規(guī)劃

程 杰，陳姚節(jié),3

(1.武漢科技大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，湖北 武漢 430065; 2.智能信息處理與實(shí)時(shí)工業(yè)系統(tǒng)湖北省重點(diǎn)實(shí)驗(yàn)室，湖北 武漢 430065; 3.冶金工業(yè)過(guò)程國(guó)家級(jí)虛擬仿真實(shí)驗(yàn)教學(xué)中心，湖北 武漢 430065)

0 引 言

無(wú)人水面艇(unmanned surface vehicle，USV)是指采用遙控方式或自主方式在水面航行的無(wú)人化、智能化平臺(tái)[1]，是船舶智能的一個(gè)典型應(yīng)用領(lǐng)域，在軍事和民用都有廣闊的應(yīng)用前景。路徑規(guī)劃是船舶智能誘導(dǎo)的重要組成部分，其主要目的是在已知船舶準(zhǔn)確船位以及周?chē)o態(tài)和動(dòng)態(tài)障礙物區(qū)域并存的環(huán)境信息中，尋找一條從起點(diǎn)到終點(diǎn)的滿足一定要求的航行路徑，使船舶在航行過(guò)程中能夠安全可靠地避開(kāi)所有障礙區(qū)域并且使路徑盡可能的短[2]。目前已有多種算法應(yīng)用到了路徑規(guī)劃研究中，主要有A*算法、遺傳算法[3-4]、蟻群算法[5-6]等，A*算法被認(rèn)為是在靜態(tài)環(huán)境中求解最短路徑最有效的直接搜索算法[7]。

傳統(tǒng)A*算法僅考慮路徑距離導(dǎo)致規(guī)劃所得路徑轉(zhuǎn)彎次數(shù)較多，為了得到更優(yōu)的路徑，國(guó)內(nèi)外學(xué)者在不同方面對(duì)其進(jìn)行改進(jìn)優(yōu)化。文獻(xiàn)[8]采用雙向搜索的方式,對(duì)傳統(tǒng)A*算法加以改進(jìn),該算法在路徑規(guī)劃過(guò)程中,可同時(shí)進(jìn)行正反向路徑搜索,同時(shí)采用正反向搜索交替機(jī)制,保證了最終目標(biāo)節(jié)點(diǎn)搜索在連線中點(diǎn)區(qū)域內(nèi)相遇,從而縮短了尋路計(jì)算時(shí)間。文獻(xiàn)[9]提出了一種基于權(quán)重的改進(jìn)A*算法航線規(guī)劃方法，根據(jù)風(fēng)浪網(wǎng)格數(shù)據(jù)，篩選出危險(xiǎn)點(diǎn)集，利用基于權(quán)重的改進(jìn)A*算法快速尋找安全航行路徑。文獻(xiàn)[10]對(duì)估價(jià)函數(shù)進(jìn)行指數(shù)衰減的方式加權(quán)，使得A*算法在離目標(biāo)點(diǎn)較遠(yuǎn)時(shí)能夠很快地向目標(biāo)點(diǎn)靠近，在離目標(biāo)點(diǎn)較近時(shí)能夠局部細(xì)致搜索保證目標(biāo)點(diǎn)附近障礙物較多時(shí)可到達(dá)目標(biāo)。文獻(xiàn)[11]在傳統(tǒng)A*算法的基礎(chǔ)上，引入角度計(jì)算模塊拓展目標(biāo)點(diǎn)方向可搜索鄰域，使路徑點(diǎn)不局限于柵格中且轉(zhuǎn)折角度不限制為π/4的整數(shù)倍，再對(duì)求出的路徑進(jìn)行二次平滑處理，消除路徑點(diǎn)中的冗余節(jié)點(diǎn)。以上文獻(xiàn)對(duì)A*算法的改進(jìn)提高了A*算法的性能，但均使用柵格法建立環(huán)境模型使得路徑規(guī)劃受到正方形網(wǎng)格幾何性質(zhì)的約束，同時(shí)也忽略了實(shí)體與障礙物之間的距離容易導(dǎo)致規(guī)劃出的路徑存在與障礙物相鄰的部分。

為了提高規(guī)劃路徑的真實(shí)性和安全性，該文通過(guò)解析電子海圖數(shù)據(jù)，采用正六邊形網(wǎng)格劃分建立海洋環(huán)境模型，引入“引導(dǎo)量”對(duì)A*算法啟發(fā)函數(shù)進(jìn)行改進(jìn)，優(yōu)化算法搜索效率，分析不同擴(kuò)展鄰域下A*算法的搜索效率以及規(guī)劃路徑的性能。

1 海洋環(huán)境模型的建立

為利用真實(shí)的海洋環(huán)境實(shí)現(xiàn)USV全局路徑規(guī)劃，采用正六邊形網(wǎng)格劃分建立基于電子海圖的海洋環(huán)境模型。首先解析電子海圖，提取其中的海域地理信息和障礙物信息，然后利用正六邊形網(wǎng)格劃分進(jìn)行網(wǎng)格化，建立由可航行網(wǎng)格和不可航行網(wǎng)格組成的環(huán)境模型，通過(guò)正六邊形網(wǎng)格劃分對(duì)電子海圖的海洋環(huán)境信息進(jìn)行簡(jiǎn)化可以提高路徑規(guī)劃算法的效率。

1.1 電子海圖數(shù)據(jù)提取

電子海圖可以提供詳細(xì)而準(zhǔn)確的全局海洋環(huán)境信息，S-57電子海圖是按照ISO/IEC 8211國(guó)際標(biāo)準(zhǔn)封裝的不可見(jiàn)格式[12]，為了將電子海圖應(yīng)用到USV路徑規(guī)劃當(dāng)中，需要對(duì)電子海圖進(jìn)行解析、讀取和存儲(chǔ)。S-57電子海圖文件里的記錄主要包含特征記錄和空間記錄，利用ISO 8211 lib庫(kù)對(duì)電子海圖進(jìn)行解析，讀取電子海圖文件中的每條記錄，使用vector容器存儲(chǔ)特征記錄，map容器存儲(chǔ)空間記錄，提取出海洋環(huán)境模型所需的內(nèi)容[13]。

1.2 正六邊形網(wǎng)格劃分建立環(huán)境模型

通過(guò)解析電子海圖提取的海洋環(huán)境信息是由復(fù)雜幾何圖形組成的，使得多數(shù)路徑搜索算法不能直接被利用，因此需要對(duì)海洋環(huán)境信息進(jìn)行網(wǎng)格劃分，轉(zhuǎn)換成幾何關(guān)系簡(jiǎn)單的環(huán)境模型，以提高路徑搜索算法的效率[13]。從電子海圖中獲取海洋環(huán)境信息后，通過(guò)正六邊形網(wǎng)格劃分的方法把海圖區(qū)域劃分為若干塊大小相等的正六邊形網(wǎng)格，根據(jù)電子海圖中提取的海洋地理信息，可以判斷一個(gè)網(wǎng)格是否包含大陸、島嶼、半島等障礙物。如果網(wǎng)格中有障礙物，則標(biāo)記為不可航行，否則，網(wǎng)格被標(biāo)記為可航行[14]。圖1是該文使用正六邊形網(wǎng)格劃分建立的環(huán)境模型，淺色區(qū)域?yàn)榭珊叫袇^(qū)域，深色區(qū)域?yàn)椴豢珊叫袇^(qū)域，網(wǎng)格數(shù)為143*159，建立以水平向右為x軸，垂直向下為y軸的笛卡爾坐標(biāo)系，對(duì)每一個(gè)網(wǎng)格編號(hào)，從(0,0)到(142,158)，一個(gè)網(wǎng)格即為一個(gè)節(jié)點(diǎn)。

圖1 正六邊形網(wǎng)格劃分建立的環(huán)境模型

引入立方體坐標(biāo)系對(duì)正六邊形網(wǎng)格坐標(biāo)進(jìn)行轉(zhuǎn)換可以簡(jiǎn)化坐標(biāo)運(yùn)算，在笛卡爾坐標(biāo)系中的(x,y)對(duì)應(yīng)立方體坐標(biāo)系中的(q,r,s)，轉(zhuǎn)換關(guān)系如下。

由笛卡爾坐標(biāo)轉(zhuǎn)換為立方體坐標(biāo)：

(1)

由立方體坐標(biāo)轉(zhuǎn)換為笛卡爾坐標(biāo)：

(2)

經(jīng)過(guò)立方體坐標(biāo)轉(zhuǎn)換后的正六邊形網(wǎng)格坐標(biāo)示意圖如圖2所示。

海洋環(huán)境信息經(jīng)正六邊形網(wǎng)格劃分后被轉(zhuǎn)化成網(wǎng)格環(huán)境模型，正六邊形網(wǎng)格的一致性和統(tǒng)一性簡(jiǎn)化環(huán)境模型，將全局路徑規(guī)劃問(wèn)題轉(zhuǎn)化為在網(wǎng)格環(huán)境中尋找兩個(gè)網(wǎng)格節(jié)點(diǎn)之間的最優(yōu)路徑問(wèn)題。

2 路徑規(guī)劃方法

2.1 A*算法

首先采用A*算法實(shí)現(xiàn)全局路徑規(guī)劃，尋找無(wú)約束條件的最優(yōu)路徑。A*算法是全局路徑規(guī)劃中一種啟發(fā)式搜索算法，其估價(jià)函數(shù)表示為：

f(i)=g(i)+h(i)

(3)

其中，g(i)表示從初始節(jié)點(diǎn)到節(jié)點(diǎn)i的代價(jià)，啟發(fā)式函數(shù)h(i)表示從節(jié)點(diǎn)i到目標(biāo)點(diǎn)的估計(jì)代價(jià)。

A*算法包含兩個(gè)數(shù)據(jù)集合：open表和closed表，open表保存已生成而未考察的節(jié)點(diǎn)，closed表保存已訪問(wèn)過(guò)的節(jié)點(diǎn)，算法基本流程如下[15]：

(1)把起點(diǎn)加入open表。

(2)遍歷open表,找到f值最小的節(jié)點(diǎn)，把它作為當(dāng)前節(jié)點(diǎn)來(lái)處理，如果當(dāng)前節(jié)點(diǎn)為終點(diǎn)，搜索結(jié)束，否則，把該節(jié)點(diǎn)加入closed表。

(3)遍歷當(dāng)前節(jié)點(diǎn)的8個(gè)相鄰節(jié)點(diǎn)，如果相鄰節(jié)點(diǎn)可行并且不在open表。

(4)計(jì)算相鄰節(jié)點(diǎn)的g值，如果相鄰節(jié)點(diǎn)不在closed表或者g值更小。

(5)更新相鄰節(jié)點(diǎn)的g值，將當(dāng)前節(jié)點(diǎn)設(shè)置為相鄰節(jié)點(diǎn)的父節(jié)點(diǎn)，計(jì)算f值，將相鄰節(jié)點(diǎn)加入open表，轉(zhuǎn)至(2)。

傳統(tǒng)的A*算法規(guī)劃路徑無(wú)約束條件導(dǎo)致計(jì)算時(shí)間較長(zhǎng)以及轉(zhuǎn)折次數(shù)過(guò)多。因此，提出了一種改進(jìn)的A*算法，從啟發(fā)函數(shù)和搜索策略兩方面對(duì)A*算法進(jìn)行優(yōu)化。

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

正六邊形網(wǎng)格之間的實(shí)際距離為立方體坐標(biāo)系中距離的一半，因此節(jié)點(diǎn)u(q,r,s)和節(jié)點(diǎn)v(q,r,s)的曼哈頓距離D(u,v)表示為：

D(u,v)=

(4)

在A*算法搜索流程中，可能會(huì)有多個(gè)具有相同估價(jià)值的網(wǎng)格被搜索，但只想選擇靠近從起點(diǎn)到目標(biāo)點(diǎn)直線的網(wǎng)格，通過(guò)“引導(dǎo)量”對(duì)啟發(fā)函數(shù)進(jìn)行改進(jìn)，可以減少具有相同估價(jià)值的網(wǎng)格數(shù)量，選擇期望的最優(yōu)路徑。

假設(shè)起點(diǎn)S到目標(biāo)點(diǎn)E的直線為L(zhǎng)1，節(jié)點(diǎn)i到目標(biāo)點(diǎn)E的直線為L(zhǎng)2。

起點(diǎn)S到目標(biāo)點(diǎn)E的歐氏距離為：

(5)

節(jié)點(diǎn)i到目標(biāo)點(diǎn)E的歐氏距離為：

(6)

L1和L2之間的夾角θ的計(jì)算方法如下：

θ=

(7)

節(jié)點(diǎn)i的引導(dǎo)量p(i)可表示為：

(8)

假設(shè)從起點(diǎn)S到節(jié)點(diǎn)i的規(guī)劃路徑為S→n0→n1→…→ni，則g(i)和h(i)可表示為：

(9)

h(i)=D(i,E)·p(i)

(10)

2.3 搜索策略的改進(jìn)

在正六邊形網(wǎng)格模型中，傳統(tǒng)的A*算法搜索下一個(gè)節(jié)點(diǎn)采用6鄰域方式，每個(gè)搜索方向之間夾角為π/3，此時(shí)規(guī)劃路徑可能不是最短且因轉(zhuǎn)折點(diǎn)較多不夠平滑。為優(yōu)化這個(gè)問(wèn)題，對(duì)搜索鄰域進(jìn)行層級(jí)擴(kuò)展，向外擴(kuò)展一層可將當(dāng)前節(jié)點(diǎn)n周?chē)谝粚雍偷诙涌傆?jì)18個(gè)節(jié)點(diǎn)納入算法下一步待擴(kuò)展節(jié)點(diǎn)，向外擴(kuò)展兩層可將當(dāng)前節(jié)點(diǎn)n周?chē)谝粚印⒌诙雍偷谌龑涌傆?jì)36個(gè)節(jié)點(diǎn)納入算法下一步待擴(kuò)展節(jié)點(diǎn)。

A*算法的可擴(kuò)展節(jié)點(diǎn)數(shù)與可搜索層級(jí)成正比，設(shè)x表示當(dāng)前節(jié)點(diǎn)n搜索下一節(jié)點(diǎn)的可搜索層級(jí)，Nx表示可擴(kuò)展節(jié)點(diǎn)數(shù)，則：

(11)

其中，x≥1,x∈Z。

由此得到與x取值對(duì)應(yīng)的不同Nx的值，如表1所示。

表1 可擴(kuò)展節(jié)點(diǎn)數(shù)與可搜索層級(jí)的對(duì)應(yīng)關(guān)系

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

為驗(yàn)證正六邊形網(wǎng)格化模型的合理性和A*算法優(yōu)化方法的可行性，對(duì)已解析的電子海圖分別使用柵格法和正六邊形網(wǎng)格進(jìn)行劃分，其中使用柵格法劃分的網(wǎng)格左右相鄰的單位代價(jià)與使用正六邊形網(wǎng)格劃分的單位代價(jià)相同。假設(shè)單位代價(jià)為1，設(shè)定起點(diǎn)S位置為(112.521 000°E，21.635 000°N)，在柵格法劃分的環(huán)境模型下的笛卡爾坐標(biāo)為(99，6)，在正六邊形網(wǎng)格劃分的環(huán)境模型下的笛卡爾坐標(biāo)為(114，5)，立方體坐標(biāo)為(-15，-62，114)，終點(diǎn)E位置為(112.901 000°E，21.669 000°N)，在柵格法劃分的環(huán)境模型下的笛卡爾坐標(biāo)為(89，115)，在正六邊形網(wǎng)格劃分的環(huán)境模型下的笛卡爾坐標(biāo)為(103，115)，立方體坐標(biāo)為(63，-166，103)，使用A*算法進(jìn)行仿真實(shí)驗(yàn)。

3.1 正六邊形網(wǎng)格與柵格法劃分實(shí)驗(yàn)

對(duì)基于柵格法劃分和基于正六邊形網(wǎng)格劃分的電子海圖模型進(jìn)行路徑規(guī)劃，仿真實(shí)驗(yàn)結(jié)果分別如圖3和圖4所示。

圖3 柵格法劃分實(shí)驗(yàn)

圖4 正六邊形網(wǎng)格劃分實(shí)驗(yàn)

統(tǒng)計(jì)兩次仿真實(shí)驗(yàn)的路徑參數(shù)數(shù)據(jù)可以更加直觀地看出兩次實(shí)驗(yàn)的區(qū)別，統(tǒng)計(jì)表如表2所示。

表2 正六邊形網(wǎng)格與柵格法劃分實(shí)驗(yàn)對(duì)比

由表2可以看出，基于正六邊形網(wǎng)格劃分模型相比柵格法劃分模型優(yōu)勢(shì)在于算法的遍歷點(diǎn)數(shù)減少27.93%，搜索時(shí)間減少49.76%，雖然轉(zhuǎn)彎次數(shù)和路徑總長(zhǎng)有所增加，但是曲線的平滑度更好。綜合來(lái)看，基于正六邊形網(wǎng)格劃分模型的算法性能更好，但是依然存在轉(zhuǎn)彎次數(shù)較多的問(wèn)題。

3.2 鄰域?qū)蛹?jí)擴(kuò)展實(shí)驗(yàn)

在基于正六邊形網(wǎng)格劃分模型的基礎(chǔ)上，對(duì)A*算法的搜索策略進(jìn)行鄰域?qū)蛹?jí)擴(kuò)展，圖5和圖6分別是雙層18鄰域擴(kuò)展、三層36鄰域擴(kuò)展仿真實(shí)驗(yàn)結(jié)果。

圖5 18鄰域擴(kuò)展實(shí)驗(yàn)

圖6 36鄰域擴(kuò)展實(shí)驗(yàn)

統(tǒng)計(jì)6鄰域擴(kuò)展、18鄰域擴(kuò)展、36鄰域擴(kuò)展三次仿真實(shí)驗(yàn)的路徑參數(shù)數(shù)據(jù)，統(tǒng)計(jì)表如表3所示。

表3 鄰域?qū)蛹?jí)擴(kuò)展實(shí)驗(yàn)數(shù)據(jù)對(duì)比

由表3可以看出，隨著可擴(kuò)展鄰域?qū)蛹?jí)的增大，算法搜索時(shí)間增加，路徑總長(zhǎng)減少，轉(zhuǎn)彎次數(shù)和轉(zhuǎn)角總和減少。本組實(shí)驗(yàn)中，基于正六邊形網(wǎng)格劃分模型并使用18鄰域擴(kuò)展搜索的路徑相比柵格法建模搜索的路徑，轉(zhuǎn)彎次數(shù)和轉(zhuǎn)角總和顯著減少且搜索時(shí)間更短。

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

以無(wú)人水面艇路徑規(guī)劃為背景，針對(duì)A*算法規(guī)劃路徑的安全問(wèn)題，提出基于正六邊形網(wǎng)格建模的方法和一種A*算法的優(yōu)化方法，通過(guò)仿真實(shí)驗(yàn)得到以下結(jié)論：

(1)基于正六邊形網(wǎng)格建模相比柵格法建模可以提升算法搜索性能以及規(guī)劃路徑的平滑度；

(2)在保證算法搜索時(shí)間的前提下可以盡量增加鄰域擴(kuò)展層級(jí)以減少轉(zhuǎn)彎次數(shù)得到更優(yōu)的路徑。