楊卓然，王俊雄

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖2 實驗所用柵格地圖及對應(yīng)ID示意圖

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

圖3 柵格法八向行進（子節(jié)點選取）基礎(chǔ)

1.2 原始D_star算法

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

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

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

1.3 原始蟻群算法

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

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

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

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

2 蟻群-D_star融合算法與改進

2.1 蟻群-D_star融合算法描述

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖6 改進Floyd-Warshall算法示意圖

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

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

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

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

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

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

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

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

表1 幾種算法的歐式距離與總代價對比

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

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

圖7 不同算法仿真對比試驗示意圖

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

4 結(jié)論

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

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

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