基于復(fù)雜約束條件的欠驅(qū)動AUV三維路徑規(guī)劃

張家聞，房浩霖，李家旺

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

0 引言

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

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

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

1 問題描述

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

1.1 運動約束

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

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

(1)

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

1.2 終端約束

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

()=

(2)

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

1.3 空間海域建模

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

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

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

2 路徑規(guī)劃算法

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

2.1 算法函數(shù)設(shè)計

211 安全域設(shè)計

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

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

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

=(,)+Δ

(3)

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

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

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

(4)

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

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

(5)

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

(6)

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

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

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

(7)

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

213 信息素更新

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

(8)

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

(9)

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

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

3 路徑生成規(guī)則

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

3.1 終端速度限制

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

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

(10)

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

圖3 回轉(zhuǎn)半徑球相切軌跡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ù)根據(jù)自變量區(qū)間結(jié)合運算生成。路徑點集合的起始點與目標點分別為、，路徑曲線在空間中的軌跡需要經(jīng)過指定控制節(jié)點，以引導(dǎo)AUV按照預(yù)設(shè)速度矢量經(jīng)過目標終點。利用B樣條曲線的固有特性，采取三重點方法對與中間輔助點加以控制。

(12)

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

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

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

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

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

: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ī)劃，得到相應(yīng)幾何參數(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，最小回轉(zhuǎn)半徑=25 m，最大潛浮角=0.223 rad)對其進行仿真。首先導(dǎo)入某海域海底空間結(jié)構(gòu)，通過障礙物相對高度及輪廓峰值結(jié)合自然插值法獲得數(shù)值點集，柵格劃分AUV工作空間。初始化起始點和目標點，設(shè)計預(yù)置算法參數(shù)值如表1所示。

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

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

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

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

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

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

5 結(jié)論

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

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

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

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

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