張家聞，房浩霖，李家旺

(寧波大學 海運學院，浙江 寧波 315211)

0 引言

海洋資源合理開發(fā)與國家經濟發(fā)展關聯(lián)密切，釋放我國海洋產業(yè)發(fā)展動力和潛力的呼聲持續(xù)高漲。自主水下航行器(AUV)作為海洋勘探的前沿載體，其系統(tǒng)平臺的智能化已發(fā)展到新的高度。路徑規(guī)劃的技術水平可以表征AUV智能化程度。其定義是實時作業(yè)的AUV根據負載探測模塊(側掃聲吶、前視避碰聲吶等)及導航定位系統(tǒng)反饋的海洋環(huán)境信息，搜索出避繞水下障礙、連接始末點的全局路徑。

現(xiàn)階段，國內外AUV路徑規(guī)劃與自主避障技術研究持續(xù)發(fā)展。目前，AUV路徑規(guī)劃方法可歸納為如下三類：環(huán)境建模法(可視圖法、維諾圖法等)、智能化仿生學搜索法(粒子群優(yōu)化算法、蟻群算法等)以及傳統(tǒng)路徑搜索法(A算法、人工勢場法)。劉貴杰等基于水動力學建立AUV在海洋環(huán)境中移動時的受力模型，以最優(yōu)能耗計算模型為評價因子改進傳統(tǒng)蟻群算法，在兼顧收斂速度的同時保障了AUV自主規(guī)劃避障路徑。姚鵬等在區(qū)域洋流恒定的條件下，基于傳統(tǒng)人工勢場法提出一種導航向量場法，引入修正矩陣對初始導航向量場進行修正，量化海洋環(huán)境中障礙物對向量場的影響同時得到修正導航向量場，結合AUV運動速度模型解決避碰動靜態(tài)障礙物的問題。張楠楠等在傳統(tǒng)蟻群算法的基礎上加以改進，將并聯(lián)路徑能耗與路徑光滑度作為指標設計評價函數(shù)，對種群適應度進行評價。但算法運行時疏于考慮路徑曲率連續(xù)問題，導致路徑跟蹤存在困難。Raheem等在算法規(guī)劃行駛路徑的基礎上，篩選路徑控制節(jié)點結合樣條曲線理論擬合路徑，在保留路徑形狀特性的前提下得到曲率連續(xù)的路徑曲線。然而工作環(huán)境停留在二維范疇，未將三維環(huán)境的復雜性與航行器內外約束因素聯(lián)合分析。

本文選擇障礙物有限的海洋淺水區(qū)域進行研究，考慮AUV實時作業(yè)的特殊性，并忽略海流對AUV的影響，基于欠驅動AUV約束條件進行分析。然而，傳統(tǒng)智能化仿生學算法與欠驅動AUV路徑規(guī)劃存在適配度低、可行性差的技術問題。為有效解決算法適配問題，本文基于兩個方面創(chuàng)新改進。針對運動約束，確定規(guī)劃路徑方法中需要引入的影響因子，從逼近程度、崎嶇程度及垂直面安全域3個范疇改進傳統(tǒng)蟻群算法，提高尋徑搜索能力，同時避免局部路徑最優(yōu)及規(guī)劃失效。針對終端約束，本文提出速度回轉球相切方法重規(guī)劃路徑末端區(qū)域，在保留原路徑軌跡信息的同時，限定末端曲線軌跡以滿足終端速度矢量約束。聯(lián)合B樣條曲線理論對路徑點擬合處理，得到曲率連續(xù)的全局路徑。最后將算法在模擬海域環(huán)境中仿真，證明了算法有效性與魯棒性。

1 問題描述

欠驅動AUV在進行空間路徑規(guī)劃時，由于航行器自身運動能力及海底環(huán)境結構的特殊性，致使在設計尋徑算法之前，應先分析約束因素及空間環(huán)境建模。

1.1 運動約束

由于驅動裝置的舵輪和推進器直接控制欠驅動AUV的運動位置和姿態(tài)，欠驅動AUV在橫移和潛浮兩個自由度的運動沒有直接驅動力，導致AUV的位置控制困難，因此路徑規(guī)劃時需要考慮AUV運動及操作特性，增強路徑的可跟蹤性。

全局路徑軌跡生成受到AUV最小回轉半徑與潛浮角的限制，約束條件如下：

(1)

式中：()為路徑點的回轉半徑，為路徑節(jié)點；()為潛浮角變化函數(shù)；為相應物理量求得的最小回轉半徑；為最大潛浮角。

1.2 終端約束

由于航行器抵達終端目標點時觸發(fā)回收指令或銜接次級工作任務，AUV的姿態(tài)角度也是規(guī)劃終端目標點時考慮的因素。本文假設欠驅動AUV以特定的速度矢量進入終端目標點，引出終端速度約束，約束條件表達式如下：

()=

(2)

式中：()為AUV到達目標點時的速度矢量，為路徑終端目標點；為特定速度矢量。

1.3 空間海域建模

為合理處理數(shù)據信息集以確保路徑規(guī)劃可行性，需要對已知海域進行環(huán)境建模。運用柵格法以定精度對已知海域環(huán)境空間劃分，將地質障礙物信息數(shù)字化抽象模型，以便于計算機操作，得到等間距柵格地圖，如圖1所示，其中為工作空間的原點，軸、軸、軸分別為橫向、縱向及垂直方向。

圖1 空間區(qū)域柵格劃分Fig.1 Grid division of space region

將面按照給定精度分隔出×個柵格，與分別代表軸、軸方向的劃分精度值。各柵格的頂點代表AUV作業(yè)時能夠抵達的空間路徑點。將面的柵格點沿著軸方向延伸并貫穿整個工作空間，得到×個長方體?；诤５椎刭|形態(tài)的空間坐標值，各長方體沿軸方向能夠實現(xiàn)任意精度下垂直方向無碰撞的路徑搜索。基于海域環(huán)境模型與路徑生成約束因素，設計算法完成全局路徑規(guī)劃。

2 路徑規(guī)劃算法

傳統(tǒng)蟻群算法的并行正反饋特性賦能算法具有尋優(yōu)能力強、魯棒性高的優(yōu)勢，但面對大體量問題時，算法迭代時間長、精度低，容易陷入局部最優(yōu)。因此，改進算法模型及參數(shù)是適配欠驅動AUV路徑規(guī)劃的關鍵。

2.1 算法函數(shù)設計

211 安全域設計

由于障礙物的位置可能位于柵格相鄰節(jié)點之間，如圖2所示法平面的障礙物界面位置。

圖2 Oyz截面的障礙物局部輪廓Fig.2 Local contour of obstacle in Oyz

按照定步長規(guī)劃的全局路徑將會造成垂直方面避碰冗量過大、能耗增加，甚至潛浮角超過約束極限，導致路徑無法有效跟蹤。為解決上述問題，本文提出垂直方向的變步長路徑搜索規(guī)則，如(3)式所示：

=(,)+Δ

(3)

式中：為垂直方向的步長；(,)為障礙物高度值；(,)為備選路徑節(jié)點在面的坐標值；Δ=12代表安全域值，為AUV艇體長度。垂直面的變步長設定提高了尋徑時的避碰精度，使規(guī)劃出的路徑具有高平緩度、低偏離性的特性，工程中具有增加航行器使用壽命、節(jié)約成本的經濟價值，有效滿足AUV勘探信息的作業(yè)目的。

212 啟發(fā)函數(shù)改進

傳統(tǒng)蟻群算法進行路徑規(guī)劃時，算法迭代過程中的啟發(fā)值與采樣點的距離有關。啟發(fā)函數(shù)影響因素過少，將導致狀態(tài)選擇條件局限，造成路徑陷入局部最優(yōu)甚至尋徑失效，因此本文的啟發(fā)函數(shù)設計如下：

(4)

式中：(,,)為算法啟發(fā)函數(shù)；、、分別為距離影響因子、逼近度影響因子、崎嶇度影響因子；、、為相應影響因子的權重系數(shù)。

距離影響因子表示當前點與備選點之間距離對轉移概率的影響，

(5)

式中：‖,‖為點到點向量,的二范數(shù)。越大，螞蟻越傾向于選擇距離短的目標點。相比于二維情況，規(guī)劃三維路徑時螞蟻可選擇的目標點更多，情況更復雜，僅考慮距離影響因子將導致搜索路徑偏離終點。因此引入逼近度影響因子(,,)來限制路徑偏移，

(6)

式中：Δ=-表示終點的縱坐標與備選點的縱坐標的差值，體現(xiàn)了螞蟻由橫向順次選擇目標點時縱向上目標節(jié)點與終點的逼近程度，以保證規(guī)劃路徑的平滑性。

為減少AUV舵輪的轉動次數(shù)與幅值，構建崎嶇度影響因子(,,)以保證路徑的平滑性，具體形式如下：

(,,)=2-05(〈(,),(-1,)〉+
〈(,),(-1,)〉)

(7)

式中：(,)與(,)分別表示路徑節(jié)點向量,在水平面與垂直面的投影向量；〈,〉表示向量間夾角的弧度值。

213 信息素更新

信息素修正啟發(fā)值來引導單個螞蟻在循環(huán)中尋找更優(yōu)路徑節(jié)點，傳統(tǒng)蟻群算法中信息素僅在單次循環(huán)后進行全局更新，一定程度上割裂了螞蟻之間的啟發(fā)交流，弱化了單個螞蟻的全局搜索能力。為解決尋徑缺陷，信息素的更新方式如下：

(8)

式中：為迭代次數(shù)；為當前迭代中的螞蟻編號；為全局信息素衰減系數(shù)，決定了每次循環(huán)后各路徑上信息素的衰減量，∈[0，1]；表示每代螞蟻未經過的路徑點上的信息素衰減量，其數(shù)值決定算法收斂速度，值越大，算法收斂越快，易陷入局部最優(yōu)解，反之強化螞蟻的搜索能力；Δ為最優(yōu)個體信息素釋放量，

(9)

為單個螞蟻信息素濃度值，min(())表示截止到第代循環(huán)，完成路徑搜索的螞蟻最小適應度函數(shù)值，適應度函數(shù)的性能指標為路徑長度與平滑度的結合，用以判斷單個螞蟻規(guī)劃路徑的優(yōu)劣，評價是否滿足潛浮角約束限制。

由于螞蟻在垂直面方向以變步長尋徑，本文將單位柵格體垂向棱邊上任意一點(上端點除外)的信息素設為等值，以滿足螞蟻垂向搜索特性。

3 路徑生成規(guī)則

經過蟻群算法迭代計算后搜索出一系列路徑點集合，連接路徑點(∈)得到直線段可行路徑。由于直線段路徑曲率不連續(xù)，跟蹤實現(xiàn)困難。因此，需要基于AUV運動及姿態(tài)約束對路徑控制點進行擬合優(yōu)化，使其具有更高合理性。

3.1 終端速度限制

為滿足AUV進入路徑終點的速度約束以銜接其他工作任務，需要對路徑點集合的末端區(qū)域(，)進行重規(guī)劃(為終點前一路徑點)。由于目標點速度矢量特定，周圍零障礙物空間有限，AUV回收進塢的直線銜接技術存在局限性，本文提出速度回轉球相切的方法，收縮AUV運動空間，以降低碰撞危險系數(shù)。

假設路徑點集末端兩點(，)的空間連線為AUV駛向目標點的速度矢量，=(0,||,0)為AUV經過終端目標點時的速度矢量方向。經過速度矢量的任意平面能夠確定兩個相切球，半徑為最小回轉半徑。兩個速度矢量確定4個切球，兩兩相切有4種位置情況。為保證AUV在兩相切球面上回轉連續(xù)性，兩球心應在同一平面內。

(10)

AUV以速度進入相切球曲線軌跡開始回轉時，速度與回轉球的切點為AUV行進的中間輔助點。兩切球球心、的連線為2倍回轉半徑，通過空間幾何運算，聯(lián)立球心半徑2的空間球方程解出中間輔助點=(,,)。經過環(huán)繞兩相切球行跡，按預置速度到達終端點，具體實現(xiàn)如圖3所示。

圖3 回轉半徑球相切軌跡Fig.3 Tangent trajectory of two balls at turning radius

3.2 B樣條曲線擬合規(guī)則

利用3次B樣條曲線方法擬合全局路徑，其曲線局部性滿足改變中間型值點僅變更局部曲線形狀，不影響整體擬合曲線軌跡，匹配AUV局部動態(tài)避障的要求，如圖4所示。

圖4 改變中間型值點的B樣條曲線擬合Fig.4 B-spline curve fitting with changed points

擬合路徑由有限分段曲線順次首尾拼接而成，限定擬合路徑的曲線形狀，確保路徑同時滿足曲率連續(xù)及約束因素。B樣條曲線的方程如下：

(11)

式中：()為B樣條曲線的分段形式；()表示B樣條曲線的基函數(shù)；,為第段曲線的第個控制點。

每段樣條曲線由4個控制點與基函數(shù)根據自變量區(qū)間結合運算生成。路徑點集合的起始點與目標點分別為、，路徑曲線在空間中的軌跡需要經過指定控制節(jié)點，以引導AUV按照預設速度矢量經過目標終點。利用B樣條曲線的固有特性，采取三重點方法對與中間輔助點加以控制。

(12)

綜上所述，路徑點集提供B樣條曲線擬合的型值點，銜接樣條曲線與切球軌跡以完成路徑規(guī)劃的所有步驟。

3.3 算法實現(xiàn)流程

結合改進蟻群算法的參數(shù)設計與B樣條曲線的擬合特性，路徑規(guī)劃算法實現(xiàn)整體實現(xiàn)流程圖如圖5所示。

圖5 算法具體流程Fig.5 Flow chart of algorithm

根據算法框架及思路，改進蟻群及B樣條擬合算法實現(xiàn)的偽代碼結構如下：

:environment_data

:optimal_path

Phase Ⅰ Initialization

1 # 初始化參數(shù)

2 load environment_data

3 popnumber ← population of ant

4 pher_ant ← initial pheromone；

5 iter_max ← max iteration

6 delta_x ← step length indirection

7 delta_y ← step length indirection

Phase Ⅱ Path planning based Advanced ACO

8 # 改進蟻群算法尋找最優(yōu)路徑

9 for== 1:iter_max

10 function search_path(parameters)；

11 function cacufit(parameters);

12 function update(parameters);

13 if== 1

14 [path，pher_ant]=search_path(paramters);

15 fitness=cacu_fit()；

16 [bestfitness，best_index]=min(fitness)；

17 bestpath=path(bestindex,:)；

18 BestFitness=[BestFitness；bestfitness]；

19 else

20 repeat

21 end

22 end

23 # 信息素更新

24 for== 1:size(path)

25 pher_ant=update(pher_ant)

26 end

Phase Ⅲ Path re-planning and trajectory fitting

27 #路徑末端區(qū)域重規(guī)劃，得到相應幾何參數(shù)

28 function re_plan(parameters)；

29 function curve_fit(parameters)；

30 [，，，sphere_core1，sphere_core2，tangency，

31 control_point] = re_plan(parameters)

32 #路徑控制點的曲線擬合

33 for== 1 :size(control_point)

34 fitting_path = curve_fit (parameters)

35 end

4 仿真

為驗證本文算法的有效性，選取某型AUV(艇體長度=12 m，最小回轉半徑=25 m，最大潛浮角=0.223 rad)對其進行仿真。首先導入某海域海底空間結構，通過障礙物相對高度及輪廓峰值結合自然插值法獲得數(shù)值點集，柵格劃分AUV工作空間。初始化起始點和目標點，設計預置算法參數(shù)值如表1所示。

根據表1中的參數(shù)編輯算法運行仿真，仿真結果與文獻[6]算法及傳統(tǒng)蟻群算法運行結果進行對比，如圖6(a)所示，在3種算法均有效規(guī)劃路徑的前提下，可以看出本文算法成功地規(guī)劃出了一條光滑的曲線路徑，連接起始點與目標點，規(guī)避了曲率不連續(xù)、轉角過大的問題。

表1 路徑規(guī)劃算法參數(shù)取值Tab.1 Parameter values in path planning algorithm

圖6 路徑規(guī)劃對比圖Fig.6 Comparison chart of path planning

對比路徑局部圖6(b)可知：本文算法規(guī)劃出的路徑在精確經過目標點的同時保證了AUV終端速度()==(0，1，0)，符合終端約束條件；文獻[6]算法速度矢量與的偏差值為0611 rad。同時對比潛浮角可知，本文算法規(guī)劃出的路徑的潛浮角始終保持在=0.223 rad的極限范圍內(見圖6(c))，確保了實際工作中AUV路徑跟蹤的可實現(xiàn)性。圖6(d)展示了本文算法迭代過程中適應度函數(shù)的收斂情況，在算法迭代計算初期，適應度值下降速率快，具體數(shù)值由適應度函數(shù)結合算法參數(shù)值計算得出，中期不斷替換最優(yōu)種群個體參數(shù)，更新適應度值，在第26次迭代中找到了全局最小適應度值，完成收斂。

表2為3種算法在路徑距離及轉角的數(shù)據對比。由表2可見，本文算法所規(guī)劃的路徑總距離為27.43 km、總轉角值為18.76 rad，相比文獻[6]算法與傳統(tǒng)蟻群算法，在算法性能方面均有提升。

表2 數(shù)據對比Tab.2 Results comparison

5 結論

本文針對AUV實時工作的約束條件，綜合分析路徑規(guī)劃要求。結合路徑跟蹤可實現(xiàn)性與終端限制，基于改進蟻群算法與B樣條曲線理論完成全局路徑規(guī)劃。根據仿真結果得出如下主要結論：

1)本文算法延續(xù)了傳統(tǒng)蟻群算法的特性，提高了收斂速度與全局搜索能力，規(guī)避了局部最優(yōu)，體現(xiàn)良好的通用性。

2)在保證有效避障的基礎上，本文所規(guī)劃的路徑曲線符合欠驅動AUV運動學特性，避免了實時情況下AUV無法完成路徑跟蹤的問題。

3)本文算法成功地將路徑曲線末端規(guī)劃為滿足AUV動力驅動條件的軌跡，有效完成了終端點的速度矢量約束目標，能夠平滑銜接AUV各級任務。

4)由于海底環(huán)境復雜，影響因素繁多，本文算法規(guī)劃過程中為簡化模型忽略了常規(guī)海流擾動，今后在深入研究路徑跟蹤控制時需要進一步考慮海流對控制器設計的影響并驗證其穩(wěn)定性。