基于蟻群-D_star融合算法的自主式水下潛器路徑規(guī)劃方法改進(jìn)研究

楊卓然，王俊雄

（上海交通大學(xué) 船舶海洋與建筑工程學(xué)院，上海 200240）

0 引言

自主式水下潛器（Autonomous Underwater Vehicle，以下簡(jiǎn)稱AUV，或水下機(jī)器人），是水下搜救、水下圍捕、水下養(yǎng)殖等眾多作業(yè)任務(wù)的重要組成部分。因?yàn)锳UV技術(shù)能夠克服很多人類無(wú)法克服的條件，例如低溫、高靜水壓力、生物威脅等，應(yīng)用領(lǐng)域遍布科學(xué)研究、軍事、商業(yè)等重要，具有廣闊的應(yīng)用前景[1，2]。因此，吸引了很多國(guó)家對(duì)其進(jìn)行研究與開發(fā)，隨著AUV在科學(xué)研究、軍事方面等領(lǐng)域的應(yīng)用越來(lái)越多，AUV智能化的需求也越來(lái)越高[3]。與陸地上的各類機(jī)器人可以憑借停止運(yùn)動(dòng)并在遇到突發(fā)障礙物時(shí)及時(shí)轉(zhuǎn)彎從而避障不同，海洋中的AUV需要在各種場(chǎng)景下快速、精準(zhǔn)地規(guī)劃出一條高效路徑以應(yīng)對(duì)場(chǎng)景動(dòng)態(tài)變化。其中路徑規(guī)劃在AUV正確導(dǎo)航和避開障礙物中發(fā)揮著重要作用，即路徑規(guī)劃模塊的性能水平與AUV運(yùn)行路徑的選擇和運(yùn)行的流暢度呈正相關(guān)。并且路徑規(guī)劃的核心是指使用路徑規(guī)劃算法，基于環(huán)境模型生成AUV的可行路徑[4]。

典型的路徑規(guī)劃問(wèn)題是指在有障礙物的情況下，規(guī)劃出一條最佳運(yùn)動(dòng)路徑，該條路徑需要使水下機(jī)器人能從起始點(diǎn)安全穩(wěn)定無(wú)碰撞地抵達(dá)終點(diǎn)，并且在此過(guò)程中滿足一定的評(píng)價(jià)標(biāo)準(zhǔn)（例如能量最小、距離最短等）。針對(duì)路徑規(guī)劃問(wèn)題，許多富有成效的解決方法已經(jīng)被探索出來(lái)，其分類方式多樣。其中按照對(duì)環(huán)境信息的掌握途徑與程度可將路徑規(guī)劃算法劃分為全局算法與局部算法，前者一般是基于先驗(yàn)環(huán)境信息的靜態(tài)路勁規(guī)劃方法，全部環(huán)境信息已知，海域信息的掌握情況較大程度上會(huì)影響算法的準(zhǔn)確性與可靠性；后者則是基于傳感器實(shí)時(shí)信息的動(dòng)態(tài)路徑規(guī)劃算法，一般只能得知AUV附近較近海域的相關(guān)信息的局部路徑規(guī)劃，所得到的信息量和準(zhǔn)確度與傳感器性能高度相關(guān)[5，6]。全局路徑規(guī)劃算法按照原理主要可以分為3類，即傳統(tǒng)算法、智能算法、傳統(tǒng)與智能相結(jié)合的算法[7-10]。

Dijkstra算法（又稱迪杰斯特拉算法），是1959年由荷蘭著名計(jì)算機(jī)科學(xué)家E.W.Dijkstra[11]提出的最為經(jīng)典的算法之一，它實(shí)現(xiàn)了在指定地圖中搜索并生成從起點(diǎn)到終點(diǎn)的最短路徑。為了提高路徑搜索的效率，同時(shí)保證準(zhǔn)確地找到一條最短路徑。P.E.Hart，N.J.Nilsson&B.Raphael在1968年將優(yōu)先搜索算法（Best First Search Algorithm，BFS）與Dijkstra算法組合形成了A_star算法[12，13]。A_star算法是啟發(fā)式搜索算法，具有搜索時(shí)間短、運(yùn)行速度快、不易陷入“早熟”、可遍及所有可達(dá)到的節(jié)點(diǎn)、便于計(jì)算、易于實(shí)現(xiàn)等優(yōu)點(diǎn)。但傳統(tǒng)A_star算法由于搜索節(jié)點(diǎn)過(guò)多，計(jì)算量會(huì)隨著地圖增大而迅速增加，進(jìn)而造成算法搜索效率下降、規(guī)劃路徑不平滑等問(wèn)題，同時(shí)其僅適用于靜態(tài)環(huán)境[14-16]。由于A_star算法僅適用于靜態(tài)環(huán)境這一特性，Anthony Stentz在此基礎(chǔ)上做出改進(jìn)，并于1994年提出了一種全新的反向增量式算法——D_star算法（又稱Dynamic A_star算法）[17]。它具備與A_star算法相同的完備性，易于計(jì)算和實(shí)現(xiàn)的特點(diǎn)，雖然計(jì)算量與計(jì)算時(shí)間會(huì)略大，路徑平滑性不足與易陷入局部最優(yōu)的問(wèn)題仍未解決，但其反向搜索機(jī)制能夠適應(yīng)動(dòng)態(tài)復(fù)雜環(huán)境下的運(yùn)算[18-20]。結(jié)合D_star算法作為增量算法的優(yōu)點(diǎn)，基于A_star算法，逐漸演變出了一種增量啟發(fā)式搜索算法Life Planning A_star算法，即LPA_star算法，其第一次正式出現(xiàn)于2001年Sven Koenig和Maxim Likhachev的相關(guān)論文[21]。相較A_star算法，LPA_star算法大幅提高了搜索效率，但仍無(wú)法應(yīng)用于動(dòng)態(tài)環(huán)境?；贚PA_star算法，Sven Koenig和Maxim Likhachev于2002年提出了D_star lite算法，它是一種增量啟發(fā)式反向搜索算法，通過(guò)假設(shè)未知地形都是自由空間，以當(dāng)前位置作為新的起始網(wǎng)格點(diǎn)，通過(guò)反復(fù)計(jì)算終點(diǎn)到各個(gè)中間節(jié)點(diǎn)（亦是新的起點(diǎn)）的最短距離，增量式地實(shí)現(xiàn)路徑規(guī)劃[21，22]。D_star Lites相較D_star算法，在需要重新規(guī)劃的情況下，需要重新搜索路徑的次數(shù)明顯減小，受影響的節(jié)點(diǎn)顯著減少，效率明顯提高。此算法已廣泛應(yīng)用于火星探測(cè)車等眾多實(shí)際場(chǎng)景[22，23]。當(dāng)實(shí)際應(yīng)用場(chǎng)景對(duì)應(yīng)面積較大時(shí)，單獨(dú)的D_star Lite算法為了提高路徑規(guī)劃算法的精度，在對(duì)海洋環(huán)境進(jìn)行柵格化建模時(shí)通常選擇將柵格尺寸盡可能地縮小，以更細(xì)粒度的海況建模實(shí)現(xiàn)較優(yōu)的路徑解，但由于規(guī)劃序列的數(shù)量迅速提高，將導(dǎo)致重新進(jìn)行路徑搜索與規(guī)劃以及全新路徑規(guī)劃的執(zhí)行所需的時(shí)間均迅速提高，其與D_star算法共有的搜索效率容易降低、規(guī)劃路徑不平滑、易陷于局部最優(yōu)，難以搜尋到全局最優(yōu)路徑等問(wèn)題仍未得到解決。

智能算法中，蟻群算法（即ACO算法，全稱為Ant Colony Optimization）是借鑒螞蟻覓食方法而設(shè)計(jì)的一種一元啟發(fā)式隨機(jī)搜索尋優(yōu)方法，于1992年由Marco Dorigo[24]提出。該方法具有正反饋、魯棒性強(qiáng)、對(duì)初始路線的選擇要求較低等特性，同時(shí)具有易與其他算法結(jié)合的突出優(yōu)點(diǎn)[25，26]。但是該算法亦有計(jì)算量大、局部搜索能力弱、收斂速度慢、易陷入局部最優(yōu)、初值依賴性較強(qiáng)等缺陷，使其應(yīng)用場(chǎng)景受到一定限制[27，28]。此外，蟻群算法本身并不能適應(yīng)動(dòng)態(tài)復(fù)雜環(huán)境下的運(yùn)算，為了使其具備動(dòng)態(tài)避障功能，Zhao[29]提出通過(guò)在既定的最短路徑上找到合適的節(jié)點(diǎn)進(jìn)行局部路線規(guī)劃從而實(shí)現(xiàn)避障的思路。此方法雖然提供了蟻群算法進(jìn)行動(dòng)態(tài)路徑規(guī)劃的基本思路，但實(shí)際意義并不大，仍有較大改進(jìn)空間。

結(jié)合水下機(jī)器人實(shí)際所需的復(fù)雜工作環(huán)境，在諸如水產(chǎn)養(yǎng)殖的大量實(shí)際應(yīng)用場(chǎng)景中，AUV將同時(shí)面對(duì)大面積的全局最優(yōu)靜態(tài)路徑規(guī)劃與小面積的局部動(dòng)態(tài)實(shí)時(shí)路徑規(guī)劃需求?；谏鲜龇治?，目前已有的單一算法均不能較好地同時(shí)滿足此類需求，故而筆者提出了一種基于蟻群-D_star融合算法的改進(jìn)路徑規(guī)劃算法，較大程度上避免了D_star算法本身搜索效率容易降低、規(guī)劃路徑不平滑、易陷于局部最優(yōu)，難以搜尋到全局最優(yōu)路徑等問(wèn)題，亦滿足了水下復(fù)雜變化環(huán)境所需的動(dòng)態(tài)路徑規(guī)劃能力。同時(shí)文中引入Floyd-Warshall算法[30]，通過(guò)這種簡(jiǎn)單且廣泛使用的動(dòng)態(tài)規(guī)劃算法，任意兩點(diǎn)（一般為起始點(diǎn)與終點(diǎn)或各類轉(zhuǎn)折點(diǎn)）之間在計(jì)算最短路徑時(shí)只需建立兩點(diǎn)間路徑長(zhǎng)度的二維數(shù)組，無(wú)須設(shè)定相應(yīng)的參數(shù)以及估計(jì)函數(shù)，并且可有效改進(jìn)融合算法規(guī)劃路徑不平滑的問(wèn)題[31，32]。

1 柵格法環(huán)境建模與原始路徑導(dǎo)航算法

1.1 柵格法環(huán)境建模

與AUV的實(shí)際應(yīng)用環(huán)境不同，為了方便仿真實(shí)驗(yàn)，一般采用柵格法進(jìn)行環(huán)境建模，將原有的三維立體水下環(huán)境降維簡(jiǎn)化為含有一定數(shù)量的大小相同均勻排布的柵格方塊的二維空間[33]。

在將復(fù)雜的海洋環(huán)境降維并抽象成均勻柵格地圖后，為表示每個(gè)區(qū)域（海域/障礙物等）的不同屬性，每個(gè)柵格均會(huì)進(jìn)行相應(yīng)的標(biāo)記處理（一般采用顏色標(biāo)記法），除顏色外，每個(gè)柵格的其他屬性（主要是尺寸）均保持相同。

如圖1所示，經(jīng)過(guò)白色標(biāo)記的柵格將用于表示可通行的海域；對(duì)應(yīng)的經(jīng)過(guò)黑色標(biāo)記的柵格則表示障礙物，即不可通行的海域，此外，亦有紅色柵格表示新檢測(cè)到的障礙物，即不再可通行的海域，以便與黑色的原始障礙柵格區(qū)分；綠色、紫色柵格則分別對(duì)應(yīng)起始點(diǎn)與終點(diǎn)等。雖然海洋中各位置對(duì)應(yīng)的障礙物通常形狀不規(guī)則且體積有一定差異，但在路徑規(guī)劃處理時(shí)，一般對(duì)它們采取膨化處理，即用黑色填充滿整個(gè)柵格，整個(gè)柵格都將被視為不可通行的海域，既減少了計(jì)算量，又能在一定程度上降低AUV與障礙物的碰撞風(fēng)險(xiǎn)，提高安全系數(shù)。

圖1 柵格法環(huán)境建模地圖對(duì)應(yīng)關(guān)系

除顏色標(biāo)記外，為方便進(jìn)行程序化處理，在坐標(biāo)化前，一般采取二進(jìn)制參數(shù)對(duì)各柵格進(jìn)行賦值，每個(gè)柵格均可由0表示空，即可通行無(wú)障礙；由1表示非空，即有障礙不可通行。當(dāng)每個(gè)柵格均已被賦值后整個(gè)二維平面便已一一映射成了對(duì)應(yīng)的0-1矩陣，以便進(jìn)一步處理，如圖1所示。

在將柵格坐標(biāo)化后，每個(gè)柵格通常有兩種表示方法，既可采用坐標(biāo)系的橫縱坐標(biāo)表示，又可通過(guò)一定規(guī)則的序號(hào)來(lái)表示，兩者關(guān)系為：

式（1）中i為柵格原始序號(hào)號(hào)，row為坐標(biāo)化后的橫坐標(biāo)，即行序號(hào)，col為縱坐標(biāo)，即列序號(hào)。實(shí)驗(yàn)中水下機(jī)器人航跡的起點(diǎn)和終點(diǎn)分別為ID為21的柵格和ID為322的柵格，如圖2所示。

圖2 實(shí)驗(yàn)所用柵格地圖及對(duì)應(yīng)ID示意圖

AUV每次移動(dòng)的最小距離為柵格邊長(zhǎng)，對(duì)應(yīng)移動(dòng)方向?yàn)槿鐖D3所示的8個(gè)方向，當(dāng)前節(jié)點(diǎn)為O點(diǎn)，在剔除已經(jīng)經(jīng)過(guò)的柵格以及不可通過(guò)的海域，即障礙物對(duì)應(yīng)柵格之后，其余的各柵格對(duì)應(yīng)的相鄰節(jié)點(diǎn)均可用于下一步的對(duì)應(yīng)節(jié)點(diǎn)，對(duì)應(yīng)節(jié)點(diǎn)分別為A、B、C、D、E、S、W、N，如圖3所示。

圖3 柵格法八向行進(jìn)（子節(jié)點(diǎn)選?。┗A(chǔ)

1.2 原始D_star算法

D_star算法是基于A_star算法改進(jìn)得出的一種反向增量式搜索算法，與A_star算法由起始點(diǎn)向目標(biāo)點(diǎn)逐步正向搜索相反，D_star搜索始于目標(biāo)點(diǎn)，終于起始點(diǎn)，每搜索過(guò)一個(gè)節(jié)點(diǎn)，會(huì)計(jì)算其相應(yīng)的H（x），即該節(jié)點(diǎn)對(duì)應(yīng)的距離度量信息，由于算法的增量式特點(diǎn)，當(dāng)動(dòng)態(tài)環(huán)境中的突發(fā)障礙物阻礙了原有路徑的搜索進(jìn)程時(shí)，算法將會(huì)不再需要重新規(guī)劃，而是可以借助已獲得的各節(jié)點(diǎn)的H（x）實(shí)現(xiàn)動(dòng)態(tài)環(huán)境中的路徑再規(guī)劃。其中，各節(jié)點(diǎn)的距離度量信息為：

式（2）中H（x）為終點(diǎn)與起始點(diǎn)，即x點(diǎn)之間的距離度量，H（y）為終點(diǎn)與當(dāng)前節(jié)點(diǎn)，即y點(diǎn)之間的距離度量，C（y，x）表示當(dāng)前節(jié)點(diǎn)與起始點(diǎn)，即y點(diǎn)與x點(diǎn)之間的距離度量。為求計(jì)算方便，距離度量一般取歐式距離，本研究引入洋流、轉(zhuǎn)彎等因素提出全新的總代價(jià)式，用以表示距離度量。

類似于A_star算法，D-star算法在搜索最優(yōu)路徑的過(guò)程中需要通過(guò)建立并更新一個(gè)優(yōu)先隊(duì)列（Openlist）從而對(duì)實(shí)際海洋工況中的各節(jié)點(diǎn)（State）進(jìn)行搜索。搜索的關(guān)鍵為路徑節(jié)點(diǎn)的傳遞，即由終點(diǎn)向AUV所在位置進(jìn)行搜索的過(guò)程，這種傳遞過(guò)程是以持續(xù)地從當(dāng)前Openlist中取出代價(jià)值最小的State來(lái)實(shí)現(xiàn)的，每當(dāng)一個(gè)State由Openlist中移出時(shí)，它會(huì)將開銷傳遞給它的鄰域State，這些鄰域State會(huì)被置于Openlist中，并不斷進(jìn)行該循環(huán)直至機(jī)器人所在State的狀態(tài)轉(zhuǎn)變?yōu)镃losed（即當(dāng)前路徑節(jié)點(diǎn)已在Closelist中），或者Openlist為空（即不存在到目標(biāo)點(diǎn)的路徑），過(guò)程中存在對(duì)Openlist中各節(jié)點(diǎn)按照代價(jià)降序排列的過(guò)程，其中通常取此時(shí)代價(jià)最小的點(diǎn)為Nextnode（即下一個(gè)節(jié)點(diǎn)）繼續(xù)搜索與判斷，而在檢索Nextnode的過(guò)程中通常會(huì)對(duì)比其鄰域八子節(jié)點(diǎn)（即Subnode），具體步驟與流程見圖4。

1.3 原始蟻群算法

蟻群算法是受自然界螞蟻覓食行為啟發(fā)的一種隨機(jī)搜索尋優(yōu)方法[34]。該算法基于正反饋機(jī)制，核心為信息素的分布與濃度，信息素產(chǎn)生于每只螞蟻進(jìn)行路徑搜索的過(guò)程中，其濃度隨著路徑長(zhǎng)度的減少以及經(jīng)過(guò)的螞蟻的增多而提升，信息素濃度越高，對(duì)應(yīng)的解的質(zhì)量也越高；在整個(gè)覓食過(guò)程中，初始時(shí)各路徑信息素量相同，各螞蟻隨機(jī)移動(dòng)；之后的每次搜索（即每次迭代）的過(guò)程中，大部分螞蟻將逐漸向信息素濃度更高的方向移動(dòng)，而少部分螞蟻則仍會(huì)繼續(xù)隨機(jī)尋找未知但更優(yōu)的行進(jìn)路線。當(dāng)螞蟻搜索次數(shù)足夠多（迭代次數(shù)達(dá)到預(yù)設(shè)上限），或一段時(shí)間內(nèi)螞蟻覓食路線趨于固定，對(duì)全新路徑的搜索趨于停滯，則可認(rèn)為此時(shí)螞蟻們的覓食路線即為蟻群算法得出的全局最優(yōu)路徑。

其概率選擇表達(dá)式為：

信息素濃度更新表達(dá)式為：

式（3式）～（5式）中Pij（t）表示選擇最短路徑ij的概率，表示在最短路徑ij上的螞蟻k會(huì)在經(jīng)過(guò)的區(qū)域釋放一只螞蟻濃度的信息素（一般為螞蟻k本輪構(gòu)建路徑長(zhǎng)度的倒數(shù)），△τij表示t時(shí)間內(nèi)的信息素總增量，N代表螞蟻個(gè)數(shù)，α代表信息素因子，β代表信啟發(fā)式因子，ρ代表信息素因子的更新參數(shù)（未蒸發(fā)率）。

2 蟻群-D_star融合算法與改進(jìn)

2.1 蟻群-D_star融合算法描述

蟻群-D_star融合算法步驟見圖4。

圖4 蟻群-D_star融合算法流程邏輯框圖

步驟1：如上文1.1中所述，利用柵格法將原有的三維水下環(huán)境抽象成存在隨機(jī)障礙物的二維柵格模型，建立起始點(diǎn)與目標(biāo)點(diǎn)，引入蟻群算法，初始化各項(xiàng)信息：包括路徑總長(zhǎng)度、各表項(xiàng)信息、信息素?cái)?shù)量與分布情況、節(jié)點(diǎn)數(shù)目與對(duì)應(yīng)距離度量、迭代次數(shù)等蟻群算法核心參數(shù)。開啟D_star算法，初始化各對(duì)應(yīng)參數(shù)，從目標(biāo)點(diǎn)開始進(jìn)行反向增量式搜索，并在Openlist表中添加目標(biāo)點(diǎn)。

步驟2：如上文1.2中所述，判斷Openlist表是否為空，若是，則表示無(wú)可行路徑；若非空，則將Openlist表中節(jié)點(diǎn)按成本估計(jì)排序，取其中對(duì)應(yīng)值最小的節(jié)點(diǎn)作為Nextnode，即下一個(gè)節(jié)點(diǎn)，同時(shí)將對(duì)應(yīng)節(jié)點(diǎn)從Openlist表中移除，并在Closelist表中添加對(duì)應(yīng)節(jié)點(diǎn)。判斷Nextnode是否為起始點(diǎn)，若是，則結(jié)束D_star算法，并跳轉(zhuǎn)到步驟3；若否，則繼續(xù)此步驟。判斷完成后，將此時(shí)Nextnode的子節(jié)點(diǎn)，即其周圍的8個(gè)相鄰節(jié)點(diǎn)添加到Subnode列表中，計(jì)算各自對(duì)應(yīng)的代價(jià)估計(jì)，并與它們的父節(jié)點(diǎn)，即此時(shí)的Nextnode節(jié)點(diǎn)比較，取其中的最小值對(duì)應(yīng)節(jié)點(diǎn)為新的Nextnode節(jié)點(diǎn)，并更新各項(xiàng)對(duì)應(yīng)數(shù)據(jù)，重復(fù)此步驟。

步驟3：D_star算法完成，已實(shí)現(xiàn)路徑規(guī)劃并已建立全局信息場(chǎng)信息，將得到的路徑轉(zhuǎn)換為蟻群算法的初始路徑，將其對(duì)應(yīng)的節(jié)點(diǎn)的信息素濃度增大，其余節(jié)點(diǎn)信息素濃度保持不變均勻分布。

步驟4：如上文1.3中所述，利用蟻群算法進(jìn)行全局靜態(tài)路徑優(yōu)化，將各節(jié)點(diǎn)代入轉(zhuǎn)移概率函數(shù)[式（3）]來(lái)計(jì)算其對(duì)應(yīng)的轉(zhuǎn)移概率數(shù)據(jù)，并通過(guò)輪盤賭法實(shí)現(xiàn)節(jié)點(diǎn)選擇。更新路徑表與路徑總長(zhǎng)度，即在路徑表中添加已被選擇的節(jié)點(diǎn)ID。然后判斷禁忌表中是否仍含有目標(biāo)點(diǎn)，如有，則在迭代次數(shù)上增加一，并重復(fù)此步驟。

步驟5：每次循環(huán)將得到若干路徑，計(jì)算各自對(duì)應(yīng)的距離度量，并進(jìn)行比較，取其中最小值，同時(shí)完成其與對(duì)應(yīng)路徑的記錄。信息素亦將根據(jù)規(guī)則進(jìn)行對(duì)應(yīng)揮發(fā)與更新。

步驟6：比較當(dāng)前迭代次數(shù)與預(yù)設(shè)的最大迭代次數(shù)，若前者大于等于后者，則結(jié)束蟻群算法，跳轉(zhuǎn)到步驟7；若前者仍小于后者，則重置路徑信息、禁忌列表等，并將迭代次數(shù)加一，并跳轉(zhuǎn)到步驟3進(jìn)行新一輪全局靜態(tài)路徑規(guī)劃。

步驟7：根據(jù)傳感器反饋的最新數(shù)據(jù)進(jìn)行判斷，是否探測(cè)到突發(fā)障礙物，若不存在，則跳到步驟9；若存在，則基于D_star算法已形成的全局信息場(chǎng)信息，結(jié)合新發(fā)現(xiàn)的障礙物，更新信息場(chǎng)，具體方式為將新障礙物所在節(jié)點(diǎn)與其對(duì)應(yīng)父節(jié)點(diǎn)的代價(jià)值提高到障礙物的對(duì)應(yīng)值，更新完畢后，可迅速形成全新的全局動(dòng)態(tài)路徑。

步驟8：D_star算法完成，已實(shí)現(xiàn)路徑規(guī)劃更新，根據(jù)傳感器反饋的最新數(shù)據(jù)進(jìn)行判斷，當(dāng)再次探測(cè)到突發(fā)障礙物時(shí)，重復(fù)步驟7以再次更新優(yōu)化路徑。

步驟9：獲得最優(yōu)路徑，進(jìn)行平滑性處理，輸出最優(yōu)路徑，算法結(jié)束。

2.2 子節(jié)點(diǎn)選擇策略優(yōu)化

傳統(tǒng)D_star算法在考量當(dāng)前柵格周圍的8個(gè)方向，即8個(gè)子節(jié)點(diǎn)時(shí)，僅考慮是否為障礙物以及是否為已路過(guò)的節(jié)點(diǎn)，會(huì)忽略兩大重要因素。其一是海洋環(huán)境中有洋流、水流等因素存在，本身向8個(gè)方向移動(dòng)代價(jià)并不相同；其二是由于經(jīng)過(guò)柵格法建模，通常不會(huì)再考慮AUV本身的形狀、體積、可操縱性等因素，而是將其簡(jiǎn)化成一個(gè)純粹的質(zhì)點(diǎn)進(jìn)行路徑規(guī)劃。由此形成對(duì)質(zhì)點(diǎn)非常理想且最優(yōu)的路徑[35]。但這對(duì)于在水下工作的AUV則并不完全匹配，常會(huì)有諸如從兩障礙物頂點(diǎn)間穿越、緊貼障礙物側(cè)面頂點(diǎn)的路徑存在，對(duì)全局路徑規(guī)劃的安全性與可靠性造成了一定的威脅。規(guī)劃路徑風(fēng)險(xiǎn)如圖5所示。

當(dāng)AUV從柵格A移動(dòng)到柵格B時(shí)，考慮到AUV具有一定的體積，若障礙物的體積較AUV自身體積大且未經(jīng)過(guò)明顯膨化處理，則在如圖5所示紅點(diǎn)位置，AUV存在較高與障礙物側(cè)壁發(fā)生摩擦甚至碰撞的風(fēng)險(xiǎn)，對(duì)AUV的水密性能與操縱性造成了一定的負(fù)擔(dān)。

圖5 典型路徑規(guī)劃風(fēng)險(xiǎn)情況示意圖

對(duì)此，筆者在原D_star算法子節(jié)點(diǎn)的選取策略的基礎(chǔ)上，引入洋流等重要影響因素，其核心是在原有的檢索順序中引入分級(jí)分組的方法。具體分組方法分3級(jí)：（1）根據(jù)與中心柵格接觸方式對(duì)周圍8節(jié)點(diǎn)預(yù)分為兩組（前面圖3），其中W、E、N、S這4個(gè)與中心柵格有公共邊的子節(jié)點(diǎn)劃入原始高級(jí)組，并取其中與洋流或主要水流方向夾角小于90°的歸入高級(jí)組；（2）夾角大于等于90°的歸入低級(jí)組，另外的4個(gè)與中心柵格僅有公共頂點(diǎn)的子節(jié)點(diǎn)，劃入原始中級(jí)組，檢索原始高級(jí)組中是否存在障礙物，如存在，則將其兩側(cè)節(jié)點(diǎn)從原始低級(jí)組去除（前面圖3）。（3）如N為障礙物，則去除A、B子節(jié)點(diǎn)；如M、N為障礙物，則去除A、B、C子節(jié)點(diǎn)，剩余子節(jié)點(diǎn)組成中級(jí)組，取其中與洋流或主要水流方向夾角小于90°的歸入高級(jí)組，夾角大于等于90°的歸入低級(jí)組。分組后的子節(jié)點(diǎn)在選取時(shí)不再進(jìn)行等價(jià)處理，而是優(yōu)先檢索高級(jí)組中的鄰域節(jié)點(diǎn)、再對(duì)低級(jí)組中的鄰域節(jié)點(diǎn)進(jìn)行檢索。

此外，在原始用于判斷路徑最優(yōu)的歐幾里得度量基礎(chǔ)上，引入洋流等因素，將洋流方向單位化后按照橫縱軸矢量分解，引入單位洋流方向向量為：

筆者為度量路徑總代價(jià)，引入總代價(jià)C*，并提出新度量式：

式（7）中m、n取自式（6）對(duì)應(yīng)系數(shù)，x、y表示路徑每?jī)纱无D(zhuǎn)彎點(diǎn)坐標(biāo)的差值，o表示轉(zhuǎn)彎總次數(shù)，Rk表示第k次轉(zhuǎn)彎轉(zhuǎn)過(guò)的弧度，為轉(zhuǎn)彎代價(jià)系數(shù)1，取1，R為轉(zhuǎn)彎代價(jià)系數(shù)2，本文取2，新總代價(jià)度量公式相比路徑總長(zhǎng)度更具實(shí)際意義。

2.3 操縱性引入與路徑平滑性處理

在對(duì)蟻群-D_star融合算法進(jìn)行子節(jié)點(diǎn)選取策略優(yōu)化后，雖然全局路徑規(guī)劃的安全性與可靠性明顯提升，但并未克服規(guī)劃路徑轉(zhuǎn)彎次數(shù)多，平滑性不足、不便于AUV實(shí)際操縱等問(wèn)題[36]，甚至為提高路徑的安全性，轉(zhuǎn)彎次數(shù)可能略有提升。因此本文基于Floyd-Warshall算法中對(duì)蟻群-D_star融合算法進(jìn)行進(jìn)一步改進(jìn)，同時(shí)，在不改變質(zhì)點(diǎn)設(shè)定的前提下，引入安全距離參數(shù)，進(jìn)一步避免碰撞風(fēng)險(xiǎn)。

通過(guò)Floyd-Warshall算法這種簡(jiǎn)單且廣泛使用的動(dòng)態(tài)規(guī)劃算法，任意兩點(diǎn)（一般為起始點(diǎn)與終點(diǎn)或各類轉(zhuǎn)折點(diǎn)）之間在計(jì)算最短路徑時(shí)只需建立兩點(diǎn)間路徑長(zhǎng)度的二維數(shù)組，無(wú)須設(shè)定相應(yīng)的參數(shù)以及估計(jì)函數(shù)，并且可有效改進(jìn)融合算法規(guī)劃路徑不平滑的問(wèn)題[30]。在原始Floyd-Warshall算法進(jìn)行單相路徑優(yōu)化的基礎(chǔ)上，引入從目標(biāo)點(diǎn)T到起始點(diǎn)S的反向優(yōu)化，實(shí)現(xiàn)對(duì)蟻群-D_star融合算法的雙向平滑優(yōu)化。

步驟1：刪除冗余節(jié)點(diǎn)：如圖6所示，從目標(biāo)點(diǎn)T開始，將其與起始點(diǎn)S都視為拐點(diǎn)，刪除所規(guī)劃路徑中每相鄰兩拐點(diǎn)中間的其它節(jié)點(diǎn)，規(guī)劃路徑處理后仍存在的節(jié)點(diǎn)依次分別為T、P3、P2、P1、S。

圖6 改進(jìn)Floyd-Warshall算法示意圖

步驟2：設(shè)置安全距離D：首先從從起始點(diǎn)S開始，在目前保留的T、P3、P2、P1、S每相鄰兩節(jié)點(diǎn)間檢索是否存在障礙物，如存在，則保留現(xiàn)節(jié)點(diǎn)；如不存在，則連接此二節(jié)點(diǎn)，取附近最近的障礙物頂點(diǎn)，計(jì)算其與兩節(jié)點(diǎn)連線間的歐氏距離。

令待選取的路徑S Pi所在直線為

與之最接近的障礙物頂點(diǎn)為O（x0，y0），則該頂點(diǎn)與對(duì)應(yīng)路徑的最短距離為

安全距離D一般取柵格邊長(zhǎng)的0.5-1倍，本研究中取0.5 m，比較最短距離與安全距離D大小關(guān)系，當(dāng)且僅當(dāng)安全距離更小時(shí)，對(duì)應(yīng)路徑方可使用，完成安全距離校核后保留的節(jié)點(diǎn)依次為T、P*3、S，依次連接即為此時(shí)的路徑。

步驟3：雙向優(yōu)化：將搜索方向?qū)φ{(diào)，即目標(biāo)點(diǎn)T開始檢索障礙物，重復(fù)步驟2操作，再次完成安全距離校核后保留的節(jié)點(diǎn)依次為T、P*2、S，依次連接即為雙向平滑優(yōu)化后的目標(biāo)路徑。

步驟4：輸出優(yōu)化后路徑。

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

通過(guò)Matlab2019b[37]對(duì)在引入靜態(tài)與動(dòng)態(tài)障礙物的AUV全局路徑規(guī)劃進(jìn)行了仿真實(shí)驗(yàn)并進(jìn)行了分析，同時(shí)對(duì)比幾種算法在仿真地圖下的歐氏距離與總代價(jià)，結(jié)果見表1。

表1 幾種算法的歐式距離與總代價(jià)對(duì)比

對(duì)環(huán)境地圖進(jìn)行降維與柵格化建模處理，取柵格邊長(zhǎng)為1 m，水流縱向向下，約1 m/s，黑色柵格為初始障礙物區(qū)域，占地圖總面積的18.42%，斜線紋理柵格為起始點(diǎn)與目標(biāo)點(diǎn)，如圖7（a）所示，縱橫交叉線紋理形柵格為新增的動(dòng)態(tài)障礙物區(qū)域，占地圖總面積的2.05%，如圖7（b）所示，圖7（c）（d）（e）為幾次不同初值情況下的D_star算法路徑規(guī)劃示意圖，其中圖7（d）所示第二次路徑規(guī)劃與蟻群-D_star算法路徑規(guī)劃軌跡一致，結(jié)合幾次路徑的總長(zhǎng)度與總代價(jià)，可以看出D_star算法對(duì)初值依賴性較強(qiáng)，無(wú)法較為穩(wěn)定地得出全局最優(yōu)解，而蟻群-D_star算法則可較大程度上改善這一問(wèn)題，可以較好地兼顧全局最優(yōu)性以及時(shí)效性，規(guī)劃出較優(yōu)秀路徑。

圖7（f）為引入動(dòng)態(tài)障礙物區(qū)域后通過(guò)蟻群-D_star算法進(jìn)行路徑規(guī)劃的效果圖，圖7（g）為引入子節(jié)點(diǎn)選擇策略優(yōu)化方法后的路徑示意圖，圖7（h）為操縱性引入與路徑平滑性處理后的路徑示意圖。結(jié)合表1中數(shù)據(jù)，可以看出引入優(yōu)化子節(jié)點(diǎn)策略后，總長(zhǎng)度與總代價(jià)均略有增加，再進(jìn)行路徑平滑處理后，總長(zhǎng)度重新減少，但仍比原始長(zhǎng)度略大，由于刪除了大量冗余節(jié)點(diǎn)，轉(zhuǎn)彎次數(shù)大幅減少，相較最初減少了37.5%，相較子節(jié)點(diǎn)選取策略優(yōu)化后減少了50%，引入了洋流與轉(zhuǎn)彎因素的總代價(jià)亦明顯減少，較最初減少了23.53%，相較子節(jié)點(diǎn)選取策略優(yōu)化后減少了31.23%，優(yōu)勢(shì)十分明顯，具有較大實(shí)際意義。

圖7 不同算法仿真對(duì)比試驗(yàn)示意圖

由圖7的仿真波形可知，以傳統(tǒng)蟻群-D_star算法為基礎(chǔ)規(guī)劃得出的路徑常出現(xiàn)斜穿障礙物頂點(diǎn)的情況，普適性較差。改進(jìn)后的蟻群-D_star算法在設(shè)置安全距離后，規(guī)劃路徑與障礙物的距離始終大于Dmin，避免了AUV距障礙物過(guò)近而發(fā)生碰撞的風(fēng)險(xiǎn)，提高了路徑的安全性。同時(shí)，借助路徑平滑處理所增加的總距離幾乎可忽略不計(jì)，在實(shí)際應(yīng)用中，可以大幅度減少AUV運(yùn)動(dòng)過(guò)程中的轉(zhuǎn)動(dòng)次數(shù)和總角度，有效提高移動(dòng)機(jī)器人運(yùn)動(dòng)的平穩(wěn)性和工作效率，具有較高的實(shí)用價(jià)值。

4 結(jié)論

（1）針對(duì)實(shí)際環(huán)境中的AUV的工作路徑規(guī)劃可能會(huì)受突發(fā)障礙物干擾，本研究基于各類全局靜態(tài)路徑規(guī)劃算法，在蟻群算法的基礎(chǔ)上，引入D_star算法，并將兩者有機(jī)結(jié)合起來(lái)，發(fā)揮各自的優(yōu)勢(shì)，在面對(duì)諸如諸如水產(chǎn)養(yǎng)殖等實(shí)際動(dòng)態(tài)環(huán)境時(shí)，既保留了全局路徑規(guī)劃的最優(yōu)性，又實(shí)現(xiàn)了局部實(shí)時(shí)路徑規(guī)劃的時(shí)效性。

（2）為提高蟻群-D_star融合算法的安全性、可靠性與搜索速度，有效降低全局路徑規(guī)劃所需時(shí)間，并引入實(shí)際工作環(huán)境中洋流、水流等的真實(shí)干擾影響，本文提出將D_star算法的子節(jié)點(diǎn)選取策略進(jìn)行優(yōu)化，將地圖中任一節(jié)點(diǎn)周圍的8個(gè)子節(jié)點(diǎn)分層處理，經(jīng)預(yù)分組與校核再分組后劃分為高級(jí)組和低級(jí)組，從而有效縮短尋優(yōu)時(shí)間，減少諸如穿越兩障礙物間頂點(diǎn)等危險(xiǎn)路徑的存在，提高路徑規(guī)劃的安全性與可靠性，亦能盡量避免因不必要逆流造成的過(guò)多能量損失與航行風(fēng)險(xiǎn)。

（3）針對(duì)蟻群-D_star融合算法所規(guī)劃路徑通常以折現(xiàn)為主、平滑性不足、轉(zhuǎn)折點(diǎn)較多、不利于AUV的實(shí)際操縱等問(wèn)題，尤其是引入子節(jié)點(diǎn)選取策略優(yōu)化后轉(zhuǎn)折點(diǎn)存在一定程度的增加，引入Floyd-Warshall算法，基于其對(duì)原融合算法所得全局路徑進(jìn)行正反雙倍平滑性處理，處理后改進(jìn)后的路徑長(zhǎng)度幾乎不變，而轉(zhuǎn)彎次數(shù)與實(shí)際操縱總代價(jià)方面均明顯優(yōu)于優(yōu)化前算法，同時(shí)，所規(guī)劃路徑由近似折線轉(zhuǎn)化為近似擬合曲線，曲率保持連續(xù)，與AUV的操縱指標(biāo)適配性更好，實(shí)際應(yīng)用價(jià)值較高。