基于改進蟻群算法的水下自主航行機器人路徑規劃*

劉雨青，向 軍，曹守啟

(上海海洋大學工程學院，上海 201306)

1 引言

水下機器人AUV(Autonomous Underwater Vehicle)路徑規劃是在環境已知或者部分已知的情況下，依據能耗最低、用時最少和路程最短等指標為AUV規劃出一條從起始點到目標點的無碰撞最優路徑，路徑規劃是AUV能夠獨立自主完成各種任務的首要前提。目前國內外已有很多用于路徑規劃的算法，如人工勢場法[1]、A*搜索法[2]、粒子群優化算法[3-5]和遺傳算法[6,7]等。

蟻群算法ACO(Ant Colony Algorithm)[8-14]是一種基于模型的構造型隨機算法，具有自組織性、較強的魯棒性與尋優能力，通過與其他啟發式算法相結合，可很容易地改善算法的求解能力。該算法可以通過施加約束條件，求解水下三維空間路徑規劃的優化問題。武小年等[7]針對啟發式算法在數字高程模型DEM(Digital Elevation Model)路徑規劃中數據量巨大、效率較低等問題，提出一種基于遺傳和蟻群的混合路徑規劃算法。封聲飛等[9]針對傳統蟻群算法在路徑規劃中存在收斂速度和尋優能力不平衡，算法易陷入局部最優等問題，提出一種自適應改進蟻群算法。金飛虎等[10]采用根據學習次數和與最近障礙物的距離來調節信息激素物質的自適應蟻群算法，彌補了傳統路徑規劃方法缺乏足夠魯棒性的問題，修改了信息激素物質的更新方法，解決了算法在計算初期出現停滯的現象。柳長安等[11]在改進蟻群算法時，用粒子群優化算法對重要參數進行優化選擇，借鑒狼群分配原則對信息素進行更新，改進后的算法可以避免搜索陷入局部最優，但改進后的算法運行時間并未有明顯縮短。史恩秀等[12]詳細分析了蟻群算法的幾個重要參數對規劃路徑長度及效率的影響，但僅限于找出具體環境下的最佳匹配參數組，仍未解決因信息素揮發系數等參數取值不準而影響規劃效果的問題。

為提高AUV三維路徑規劃效率，優化規劃路徑，本文針對具有復雜時空要求的水下AUV路徑規劃問題，使用蟻群算法為基本優化搜索算法，在多重約束條件下以能耗最低為優化目標，建立了AUV運動的能耗模型并獲得AUV運動的總能耗，通過改進的蟻群算法，實現了路徑規劃能耗最低的優化目標。仿真實驗結果表明，采用改進的蟻群算法進行水下自主航行機器人的路徑規劃，能夠提升路徑規劃的實時性，規劃出的路徑便于AUV控制跟蹤，符合能耗最優的目標。

2 水下機器人數學建模

2.1 水下環境建模

AUV水下環境模型直接影響到路徑規劃算法所規劃出的路徑優劣，因此合理地將三維環境抽象表達出來是AUV在三維環境中自主航行和進行作業的關鍵。本文將三維環境空間模型的3個軸依次設為經度、緯度和海拔增加的方向，采用柵格法將二維空間擴展到三維空間，在三維海底地形數據的基礎上進行建模，以柵格為單位離散化水下機器人工作區域。

2.2 搜索模式

從二維平面到三維空間中，由于選擇節點數量成倍增加，導致算法搜索最優解變得異常復雜，而且很容易出現死鎖現象，使得算法收斂性差，全局搜索能力降低。為此，在搜索最優的過程中采用一種分層前進與柵格平面法相結合的搜索模式(如圖1所示)。以y軸為主前進方向，設AUV起始點為PS,目標點為PG，規定機器人最大橫向移動距離為Xmax，最大縱向移動距離為Zmax。首先螞蟻從PS出發，在第1個平面搜索到可行節點P1(x1,y1,z1)，接著在第2個平面搜索到第2個可行節點P2(x2,y2,z2)，螞蟻依次在各個平面選擇路徑節點，直到搜索到目標點PG結束，即可搜索到一條最優路徑。

Figure 1 Search mode of AUV path

2.3 能耗模型

為了求解AUV跟蹤所規劃出的路徑總能耗，首先根據航行狀態分別建立AUV水下移動速度模型，然后利用水阻力和推進器推力的計算公式依據功能關系求得AUV跟蹤規劃出的路徑的總能耗。

2.3.1 巡航速度模型

為了降低計算過程的復雜度，本文忽略AUV在航行過程中的加減速過程，并作如下假設：(1)AUV以勻速直線狀態開始航行，中間轉彎根據轉角大小采用相應的低速勻速航行，最終以勻速直線航行狀態到達終點；(2)路徑關鍵點前后各一個單位長度路程均為勻速航行狀態；(3)勻速直線航行狀態與勻速轉彎航行狀態間存在一個單位長度的加減速航行狀態。

故AUV的航行狀態為勻速直線航行狀態、勻速轉彎航行狀態和加減速航行狀態的疊加，如圖2所示。設勻速直線航行狀態的速度為v=μm/s；AUV在轉彎時轉彎速度低于做直線運動時的速度，同時在不同轉彎處，轉彎半徑不一樣速度會不同，轉彎半徑越大，轉彎航行狀態的速度也就越大；從勻速直線航行狀態到轉彎航行狀態為加減速航行狀態，取其平均速度作為該過程的行駛速度。不同航行狀態下的速度計算方法如式(1)所示：

(1)

Figure 2 Model of AUV speed

2.3.2 水阻力的計算

AUV在水下運動時必定受到水阻力，由于AUV是一個非線性動力學系統，計算阻力需要確定的參數較多，本文根據水動力計算公式，將AUV受到的水阻力計算簡化為式(2)所示:

(2)

其中，F為AUV在航行過程中受到的水阻力；C為水動力系數，該系數的取值與AUV形狀、迎流面等一系列要素有關；ρ為水的密度；v為AUV的航行速度，S為AUV的橫截面積。

2.3.3 能耗計算

AUV在作勻速運動時加速度為零，水阻力F與推進器產生的推力相等，故由推進器的推力根據式(2)可求得勻速航行狀態下AUV的航行速度。AUV在水下巡航時克服水阻力做功，為簡化計算，本文忽略AUV推進器之外的所有能耗，故在計算能量消耗時所有能耗均用來克服水阻力做功。

AUV在勻速航行過程中消耗的能量如式(3)所示：

E=Pt/η=Fvt/η=FL/η

(3)

將式(2)代入式(3)中得到:

E=Cρv2SL/2η

(4)

其中，L代表AUV航行的路徑長度，η為機械效率，P為效率，由式(4)可知，AUV運動消耗的能量與其運行的速度和工作的路徑正相關。

2.4 規劃數學模型

AUV路徑規劃問題可以定義為：在某個規劃空間中，滿足一定約束條件g(γ)=0的前提下(γ為約束條件變量)，根據某種航行性能評價指標J，規劃得到AUV的航行路線，即為從起始點PS到目標點PG且滿足一定約束條件的一系列路徑節點Γ={PS,P1,P2,…,Pn,PG}。

minγJ=Cρv2SL/2η

(5)

s.t. ?(x,y)?Rff(x,y)=0

(6)

Lg+δg

(7)

(8)

(9)

在AUV路徑規劃中，主要的約束條件包括：

(1)地形與威脅約束。AUV在航行過程中，必須避開一些地形障礙和威脅區域，以保證AUV的安全性，在水下三維環境中，地形障礙和威脅區域均可表示為AUV不可穿越的區域。假設在規劃區域中，AUV不可穿越區域集合為R′f，由于AUV具有一定的尺寸,所規劃出的路徑還要經過平滑處理，為保證路徑的安全性，需要對障礙物和威脅區域進行一定的膨化處理，經膨化處理的集合Rf=εR′f，即擴大原始障礙物及威脅區域，膨化系數ε根據AUV的實際尺寸給定。所規劃出的路徑曲線為f(x,y)=0，其中，(x,y)表示路徑上任意一點的經緯度坐標，則規劃區域中的地形與威脅約束如式(6)所示。

(2)最大航程約束。AUV自身攜帶的能源決定了其最遠航行距離，故在路徑規劃時必須將其加入約束條件。AUV航程約束如式(7)所示，式中Lmax為AUV所攜帶能量能航行的最大路程；Lg為算法規劃路徑長度;δg為實際航行路程與算法規劃路程的偏差，該偏差取決于AUV路徑跟蹤算法。

3 改進蟻群算法

3.1 基于Dijkstra算法的初始信息素分配

在蟻群算法中，螞蟻通常根據路徑上信息素濃度的差異來尋找最優路徑，在信息素的引導下選擇一條從起始點到目標點的完整路徑。由于傳統蟻群算法初始信息素濃度是均勻分配的，這樣算法初期搜索具有較強的盲目性進而導致收斂速度慢，容易使算法陷入局部最優，影響算法的尋優能力[8]。

為了避免因信息素導致算法陷入局部最優，減少錯誤的啟發信息對螞蟻的誤導，提高算法路徑規劃能力，本文在Dijkstra算法[15-17]所規劃出的最短路徑基礎上改進初始信息素的分配。

由于水下自主航行機器人所消耗的能量在很大程度上取決于所需跟蹤路徑的長度，使用Dijkstra算法能規劃出最短路徑。如圖3所示，將在最短路徑上的點的信息素設置為τ0，最短路徑兩邊的點的信息素設置為τ=τ0-λn，其中λ為信息素遞減系數，n為路徑點距離最短路徑點的柵格數，即最短路徑周圍的點的信息素濃度隨著與最短路徑點距離增大逐漸減少。

Figure 3 Initial pheromone allocation for improved ant colony algorithm

3.2 啟發式函數改進

啟發式函數對于算法快速、高效地規劃出一條安全合理的路徑起著重要的作用，是路徑規劃算法中的重要組成部分。構造函數時所考慮的各項因素綜合體現了算法構建路徑的優化目標。本文路徑規劃算法的優化目標是安全、能耗最低、路徑盡可能最短的等深航行路線。根據這一目標設計的環境抽象模型中任意點(x,y,z)的啟發式函數H(x,y,z)如式(10)所示：

H(x,y,z)=

en·[E(x,y,z)]ζ·[S(x,y,z)]ξ+uc

(10)

其中，ζ和ξ為相關影響因子；考慮到機器人路徑中的安全因素，在構造三維環境模型時，將柵格分為安全區域K和禁止航行區域N，安全區域系數en的取值如式(11)所示：

(11)

E(x,y,z)為路徑點(x,y,z)所需要消耗的能量，由式(4)可知，選擇下一個路徑點所需的能量與選擇該路徑點后的路徑長度和AUV在該點處的速度相關，AUV在實際的運動過程中會根據路徑的轉彎半徑?(x,y,z)調節速度，轉彎處轉彎半徑越大，相應運動速度會加快，故下一個路徑點處所消耗的能量如式(8)所示：

E(x,y,z)=K1/(L1+L2)+K2*?(x,y,z)

(12)

其中,L1表示下一步將要選擇的點與上一個路徑點的距離;L2表示下一步將要選擇的點與規劃路徑目標點的距離，若下一步選擇的點處于上一個點與目標點所連線段上，則L1+L2的值達到最小，該點的啟發信息會較大；K1和K2分別為路程影響因子和轉彎半徑影響因子。

考慮規劃路徑平滑度因素S(x,y,z)，使得規劃出來的航線盡可能地平滑以利于控制器求解跟蹤。S(x,y,z)表示考慮環境障礙時可行點(x,y,z)的平滑程度，其計算如式(13)所示:

(13)

其中，yg為下一個路徑點的坐標值,K3為路徑平滑系數。

海洋往往存在海流，海流隨深度的不同而有所變化，在同一深度，一般表現為水平方向上的均勻流。海流的流速和流向均會對AUV的能量消耗產生巨大的影響，海流模型如圖4所示。順海流方向航行時AUV可以節省部分能耗，頂流時AUV需要額外耗費能量以保持航速，側流時不但需要消耗能量以保持航速，而且海流還會使AUV偏離原來的航向，加大了控制難度。設海底存在漸變的速度為Vc，流向為δ的無旋海流，uc和vc分別為海流投影到水下機器人橫向和縱向的速度，其計算分別如式(14)和式(15)所示：

(14)

(15)

Figure 4 AUV current model

3.3 基于線性回歸的信息素更新

信息素是螞蟻尋路的重要依據，指導螞蟻向更優的路徑前進。本文通過改進信息素更新方式優化算法收斂速度與求解質量。

3.3.1 基于線性回歸更新全局信息素

由于算法具有隨機性，每一次迭代的m只螞蟻選擇的路徑點都有可能距離最優路徑點較遠，甚至有些螞蟻在搜索路徑的過程中遇到死鎖，無法找到一條可行的路徑，因此合理的路徑更新方式是螞蟻能夠尋找到一條可達路徑的保證。本文采用線性回歸模型[18,19](如式(16)所示)將本輪迭代最具有代表性的路徑點的信息更新一次，強化下一輪螞蟻搜索路徑的方向性，從而提高算法的收斂性能，改善解的質量。

(16)

(17)

(18)

(19)

以上各式中，(xi,yi)為第i只螞蟻所選擇路徑點的坐標，hω(x)為線性回歸模型，ω為線性回歸模型中的權重參數向量，x表示路徑點的橫坐標向量，J(ω)為采用梯度下降法求解線性回歸模型的目標函數，η′為學習率。

3.3.2 自適應調節信息素揮發因子

蟻群算法中的人工螞蟻具有記憶功能，隨著時間的推移，以前留下的信息素會逐漸消失，信息素揮發因子ρi的大小直接影響到蟻群算法的全局搜索能力及其收斂速度。若求解問題的規模比較大時，會使得那些從未被搜索到的路徑上的信息素減少到接近于零，降低了算法的全局搜索能力。本文采用前后2次迭代后線性回歸路徑的長度來自動調節信息素揮發因子，當回歸路徑長度差較大時，增強信息素的啟發作用，適當調小ρi，當回歸路徑長度差較小時，為提升螞蟻尋找新路徑的能力，增強全局搜索能力，適當增大ρi，具體調節如式(20)所示：

(20)

其中，ΔL為前后2次線性回歸路徑的長度差;σ為路徑長度系數，表征路徑長度對信息素的影響程度，為防止ρi過大和過小影響算法收斂速度和求解質量，將其限制在[ρmin,ρmax]之間，以此來優化蟻群算法。

3.4 路徑平滑

在柵格地圖中規劃出來的路徑是根據規劃出的路徑點坐標序列在柵格的中心依序連接而成的折線段，在線段的連接處存在尖峰，容易損壞AUV上的零部件，不利于AUV進行路徑的跟蹤，為了解決該問題，本文采用Bezier曲線對尖峰進行平滑處理[20]。

3.5 求解流程

改進蟻群算法求解流程如下所示：

(1)首先進行參數初始化，主要包括2類參數，環境信息參數和蟻群算法本身的參數，這2類參數對仿真的結果有重要的影響。環境信息參數包括各個柵格點上的初始信息素、路徑的起止點和障礙物信息的表達等；與算法相關的參數主要包括算法迭代次數、同時尋找路徑的螞蟻只數、信息素影響因子、信息素常量和啟發函數因子等。

(2)開始進行迭代，一群螞蟻同步或異步地在問題的相鄰狀態之間轉移，利用關聯在每個狀態中的信息素和啟發信息，采用狀態轉移規則選擇移動的方向，逐步構造出問題的可行解；每只螞蟻在構造可行解時，可以局部地構造信息素；每一代的所有螞蟻都完成了解的構造后，依據獲得的解對信息素進行全局更新。

(3)同時，完成一次迭代后，根據優化準則在當代中選擇一條最優路徑，保留該路徑直到下一次迭代產生更優的路徑。

(4)該迭代過程持續到完成規定的迭代次數或者連續多代迭代結果一致為止。滿足停止尋優條件后，表明已經得到一條滿足能耗最低的路徑，再通過平滑路徑得到一條適合AUV跟蹤的平滑路徑。

4 AUV路徑規劃仿真實驗

4.1 實驗參數設置

蟻群算法作為一種啟發式算法，性能優化在很大程度上取決于參數的設置，參數是影響蟻群算法求解效率和全局收斂性能的關鍵因素之一。由于蟻群算法參數空間的龐大性以及各參數之間的關聯性，如何確定一組優質參數組合使得算法求解性能最佳是一個復雜的優化問題，本文通過大量的實驗和經驗，選取了如表1所示的參數。

Table 1 Parameter setting of improved ant colony algorithm

4.2 規劃路徑分析

本文根據建立的三維環境模型和目標函數(式(5))，利用傳統蟻群算法、改進蟻群算法和遺傳算法分別對 AUV 基于能耗最優進行等深路徑規劃仿真實驗，最后利用本文所改進的蟻群算法對 AUV 定深航行進行了路徑規劃仿真實驗。

本文將AUV航行高度設為300 m，使用傳統蟻群算法、遺傳算法和改進蟻群算法分別進行路徑規劃仿真實驗，各算法分別進行80次，其中將改進蟻群算法中所需的參數設置成表1所示，其統計結果如表2所示。圖5～圖10分別為使用傳統蟻群算法、遺傳算法和改進蟻群算法所規劃出的路徑，圖11～圖13是3種算法目標函數的收斂趨勢圖。

仿真實驗統計結果表明，采用改進蟻群算法規劃出的路徑其消耗的能量較傳統蟻群算法和遺傳算法的低，其長度較傳統蟻群算法和遺傳算法的短。改進蟻群算法具有比遺傳算法和傳統蟻群算法更強的求解能力，得到最優解的概率更大，能夠規劃出航程較短、能耗最小的全局路徑,滿足AUV等深度、低能耗航行的要求。

圖5和圖6分別給出了基于傳統蟻群算法所規劃出的路徑的三維立體視圖和俯視圖。該路徑并非全局最優路徑，其原因在于蟻群算法是一種隨機搜索算法，每只螞蟻要搜索的解空間規模較大，為了加快算法構造解的速度，在算法中增加了啟發信息所占的比重，即增加了算法的確定性，犧牲了算法的全局尋優能力。

Figure 5 3D view of path planning using traditional ant colony algorithm

Figure 6 Vertical view of path planning using traditional ant colony algorithm

Table 2 Statistical results of simulation experiment

算法在迭代的初期，信息素均勻分配導致其正反饋作用不強，路徑啟發信息與當前所在路徑點到下一個路徑點的距離有關，在啟發信息的引導下螞蟻更傾向于選擇使得整條路徑長度更短的路徑點，啟發信息的方向性較弱，故規劃出的路徑轉折點較多，路徑較長，AUV消耗的能量較多。

與傳統蟻群算法相比，遺傳算法從串集開始搜索，覆蓋面廣，利于全局擇優，路徑搜索不易陷入局部最優，由圖7和圖8可知道，由遺傳算法規劃出的路徑還是不夠平滑，AUV難以跟蹤該路徑。同時，由于遺傳算法的局部搜索能力較差，導致未經改進的遺傳算法比較費時，在進化后期搜索效率較低，求解時間較長。

Figure 7 3D view of path planning using genetic algorithm

Figure 8 Vertical view of path planning using genetic algorithm

改進蟻群算法所規劃的路徑如圖9和圖10所示，該路徑轉折點較少，使用Bezier曲線改善路徑的平滑性后，便于AUV跟蹤該路徑，AUV依靠慣性就能很好地跟蹤該路徑，從而減少了能量的消耗，保護了AUV的機械結構和電子元器件。

Figure 9 3D view of path planning using improved ant colony algorithm

Figure 10 Vertical view of path planning using improved ant colony algorithm

本文通過對啟發函數、信息素初始化及更新方式的改進，從而保證了規劃出的路徑平滑且能耗較低，使該算法更適合運用到水下機器人的路徑規劃上。

4.3 能耗分析

從3組算法所規劃的路徑中分別隨機地選擇一次實驗結果，其收斂過程如圖11～圖13所示。從路徑尋優的過程中不難發現，在尋求能耗最優的路徑時，路徑的總長度并非持續縮短，這就意味著能耗與路徑長度在某些情況下會存在矛盾，影響AUV能耗的因素除路徑長度外還有AUV轉彎幅度和水流等因素。

Figure 11 Convergence process of traditional ant colony algorithm

Figure 12 Convergence process of genetic algorithm

Figure 13 Convergence process of improved ant colony algorithm

遺傳算法同傳統蟻群算法相比，規劃出的路徑雖然在能量消耗和路徑長度方面有所改善，但需要更長的時間去優化路徑，路徑規劃實時性不高。改進蟻群算法的求解能力和求解速度相比傳統蟻群算法和遺傳算法都有了很大的提升，較傳統蟻群算法求解時間縮短了10.74%，較遺傳算法求解時間縮短了28.05%。

同傳統蟻群算法規劃出的路徑(如圖5和圖6所示)相比，在改進蟻群算法初始化前基于Dijkstra算法分配信息素，這些信息素集中分配在最短路徑附近，在算法的初期，為尋找路徑的螞蟻指明了方向，同時在每一輪迭代完成后，基于線性回歸模型進行全局信息素更新，線性回歸模型路徑上的路徑點更能代表本次迭代的最優路徑，加快了算法的收斂速度，提高了算法求解質量。最終規劃出的路徑(如圖9和圖10所示)，能量消耗比傳統蟻群算法降低了20.44%，路徑長度減少了11.51%。

基于傳統蟻群算法、遺傳算法和改進蟻群算法3種路徑規劃算法的能耗分析見表3。

Table 3 Energy consumption analysis of path planning

由表3可知改進蟻群算法求解效率較高，所規劃出的機器人路徑符合能量消耗最少的優化目標，改進蟻群算法用于水下機器人路徑規劃具有較強的工程意義。

5 結束語

本文基于蟻群算法對AUV路徑規劃問題進行了研究，詳細設計了三維空間環境建模以及蟻群算法在該模型中的搜索模式，結合能耗最低的優化目標改進了蟻群算法的啟發函數、信息素初始分配方式及更新方式，在規劃出的路徑點上采用貝塞爾曲線改善了路徑的平滑性，便于AUV跟蹤該路徑，最后給出了改進蟻群算法的具體流程。實驗結果表明，改進蟻群算法在一定的約束條件下，基于能耗最優所規劃出的路徑較傳統蟻群算法能耗降低了20.44%，較遺傳算法能耗降低了15.20%，適合用于水下三維環境中AUV的路徑規劃。