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

張家聞，房浩霖，李家旺

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

0 引言

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

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

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

1 問(wèn)題描述

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

1.1 運(yùn)動(dòng)約束

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

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

(1)

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

1.2 終端約束

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

()=

(2)

式中：()為AUV到達(dá)目標(biāo)點(diǎn)時(shí)的速度矢量，為路徑終端目標(biāo)點(diǎn)；為特定速度矢量。

1.3 空間海域建模

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

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

將面按照給定精度分隔出×個(gè)柵格，與分別代表軸、軸方向的劃分精度值。各柵格的頂點(diǎn)代表AUV作業(yè)時(shí)能夠抵達(dá)的空間路徑點(diǎn)。將面的柵格點(diǎn)沿著軸方向延伸并貫穿整個(gè)工作空間，得到×個(gè)長(zhǎng)方體?；诤５椎刭|(zhì)形態(tài)的空間坐標(biāo)值，各長(zhǎng)方體沿軸方向能夠?qū)崿F(xiàn)任意精度下垂直方向無(wú)碰撞的路徑搜索?；诤Ｓ颦h(huán)境模型與路徑生成約束因素，設(shè)計(jì)算法完成全局路徑規(guī)劃。

2 路徑規(guī)劃算法

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

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

211 安全域設(shè)計(jì)

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

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

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

=(,)+Δ

(3)

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

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

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

(4)

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

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

(5)

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

(6)

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

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

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

(7)

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

213 信息素更新

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

(8)

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

(9)

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

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

3 路徑生成規(guī)則

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

3.1 終端速度限制

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

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

(10)

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

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

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

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

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

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

(11)

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

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

(12)

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

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

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

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

根據(jù)算法框架及思路，改進(jìn)蟻群及B樣條擬合算法實(shí)現(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 # 改進(jìn)蟻群算法尋找最優(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 #路徑控制點(diǎn)的曲線擬合

33 for== 1 :size(control_point)

34 fitting_path = curve_fit (parameters)

35 end

4 仿真

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

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

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

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

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

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

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

5 結(jié)論

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

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

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

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

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