一種基于多目標約束下的UUV 航跡規劃算法

2023-05-14 09:58:44韓喜紅丁天明鄭海林

艦船科學技術 2023年8期

韓喜紅，丁天明，鄭海林，劉虎，鞠拓

(浙江海洋大學船舶與海運學院，浙江舟山 316022)

0 引言

無人水下航行器（unmanned underwater vehicle，UUV）憑借操縱靈活、可控性好、航行時間長等優勢，被廣泛應用于執行各種任務，如海洋環境數據采集、海洋資源勘探、通信中繼、反潛警戒、水下偵察與監視、海洋工程和海底電纜檢測等[1–3]。具有智能化、程度高和成本低的優點，可以代替人類執行水下危險任務。

UUV 因自身結構的限制，其大多采用“人在回路中”的控制方式[4]，但是UUV 在水下航行中易受到風、浪和流等外界因素的影響，特別是在惡劣的海洋環境下，通信環境受到干擾，通信可靠性降低[5]，造成通信中斷。因此在復雜海洋環境下工作的UUV 必須要求具備一定的自主航行控制能力和自主校正能力，以適應航行過程中復雜變化的海洋環境[6]。另一方面精確的導航與定位技術是UUV 完成水下航行任務的關鍵因素，目前，UUV 的導航定位主要采用慣性導航系統[7]，其在航行的過程中會產生位置誤差，定位系統無法對自身進行準確定位，誤差會隨著時間累積，當誤差積累到一定值時，將導致航行任務失敗[8]。航跡規劃技術是實現UUV 自主航行并完成水下任務的關鍵，其主要思想是在特定的環境中，規劃出一條滿足約束條件、目標最優的航跡。隨著控制技術的發展，多約束條件航跡快速規劃越來越重要，國內外學者對航跡規劃問題進行了大量研究，并取得了一定成果。其中常用的方法有Dijkstra算法[9]、人工勢場法[10]、模擬退火算法[11]、A*算法[12]等，本文主要從航跡規劃、轉向角、校正定位3 個方面建立模型，提出一種基于最小轉向角搜索算法，并對算法的有效性對比討論。

1 問題描述及模型的構建

1.1 問題描述

UUV 在水下執行任務時，可能會遇到惡劣的海洋環境，其通信受到影響，無法定位自身位置，定位誤差累積增加，將會造成航行任務失敗。航行區域中存在水平和垂直校正點，即用于校正定位誤差的安全位置，且UUV 航跡轉向點都在所給的校正點上，每次校正只能選取一個方向，需要在復雜海洋環境條件下找出一條滿足以下條件的最優航跡：

1）UUV 的航跡長度盡可能的短；

2）經過校正點的次數盡可能的少；

3）UUV 在轉彎時受到自身機動性能的限制，無法突然改變前進方向，其轉向角應在一定的限制范圍內，考慮到航跡的光滑度，要求轉向角盡可能的小，UUV 允許的最大轉向角為 ψmax。

1.2 UUV 模型構建

UUV 自起點A經過n個校正點航行至終點B，經過校正點時進行誤差校正，建立以起點A為圓心，r為半徑的球形可航行區域，球形區域內的誤差校正點為待選校正點。由誤差校正和轉向角度約束可知，求待選節點P0時，P點距離A點距離最近、轉向角最小的點會被優先選中，經過校正點校正后該方向的誤差校正為0，另一方向誤差保持不變，尋找到第二個校正點P2時，以P2為圓心r為半徑劃分球形可航行區域，在可航行區域內選擇下一個校正點，重復選取過程直至終點B。選取過程如圖1 所示。

圖 1 校正點選取示意圖Fig. 1 Correction point selection schematic

1.3 數學模型構建

已知UUV 的起點和終點，要使航跡長度盡可能短，滿足約束條件下構建數學模型。本文設計一條經過若干校正點航跡長度最短的軌跡，使得在航經校正點處誤差不斷校正，從而能順利達到終點。A(x0,y0,z0)表示起點坐標，Pi(xi,yi,zi) 表示第i個校正點坐標i=1,2,···n，終點坐標表示為B(xn+1,yn+1,zn+1) ，μ表達校正點的類型。

表 1 符號變量說明Tab. 1 Symbolic variable description

1）目標函數

要使航跡長度盡可能短，即目標函數為：

式中：d(0,1)為起點A到第一個校正點P1的距離；d（n,n+1）為第n個校正點Pn到終點B的距離；d(i,i+1)為校正點Pi到校正點Pi+1的距離。

2）校正條件約束

第1 個約束條件是校正次數盡可能少：在起點A(x0,y0,z0) 時UUV 的垂直和水平誤差均為0，即r0=0，s0=0 。UUV 前進 1 m，誤差增加 δ個單位，航行器從第Pi?1個校正點到Pi校正點的距離為di?1,i，產生的積累誤差為 δdi?1,i，同時在到達校正點Pi時含有i?1個未校正的原始誤差。

因此，校正前該點的垂直誤差ri為積累誤差δdi?1,i與原始誤差ri?1之和，水平誤差si為積累誤差 δdi?1,i與原始誤差si?1之和，垂直誤差ri和水平誤差si可以表示為：

當μ=1 時，進行垂直誤差校正，α1和 α2為滿足垂直誤差校正的垂直和水平誤差最大值，校正條件為：

校正后：

當 μ=0 時，進行水平誤差校正，β1和 β2為滿足水平誤差校正的垂直和水平誤差最大值，校正條件為：

校正后：

到達終點時垂直和水平誤差均滿足以下條件：

由式（4）～式（9）可知校正誤差隨著航行距離增加，因此，可以把誤差校正約束轉化為待選點間距離約束。

3）轉向角條件約束

圖 2 轉向角約束示意圖Fig. 2 Diagram of steering angle constraint

考慮到UUV 的操縱性能，當轉向角大于90°時，操縱及為困難，且在海底存在暗流的復雜情況下，控制過彎的失誤率明顯增加，航行過程中還要減少航跡折返的情況，因此在航跡規劃時各校正點處轉向角越小的點應該優先考慮，在滿足校正條件和偏轉角約束下建立目標函數。

1.4 算法步驟及流程

步驟1以A 為起點，求待選點到A 點的距離d(A,Pi)。

條件1：判斷待選點是否定在以r為半徑的可行域內d(A,Pi)

步驟2求當前點到待選點偏向夾角 ψi，在可行域內選取 ψi最小的點，進行水平或垂直校正，作為下次迭代的起點。

步驟3從當前點出發到終點B 的航跡距離是否滿足條件3，如果滿足，則輸出最終航跡，停止迭代，如若不能到達B 點，迭代次數加 1，返回步驟 1。

具體算法流程如圖3 所示。

圖 3 航跡規劃算法流程Fig. 3 Route planning algorithm flow

2 仿真實驗

根據上述模型和算法，其參數分別設置為 α1=25 ，α2=15 ，β1= 20，β2= 25 ，θ = 30 ，δ = 0.001 ，起點坐標A(0,50000,5000)，終點坐標B(10000,59652.34,5022)，校正點數量共612 個，坐標參數如表2 所示。

表 2 部分校正點坐標Tab. 2 Correction point coordinates

基于表2 數據，針對上述2 種約束條件，調整半徑10000～20000 m，利用 Matlab 數值模擬仿真，可以得出不同調整半徑r對應的校正點次數和航跡長度。由圖4 可知，基于距離約束時，調整半徑小于11000 m得不到航跡規劃結果，當調整半徑r=14000 m 時航跡長度最短，調整半徑r=18000 m 時校正點次數最少。基于距離和轉向角約束情況下，當r=14000 m 時校正次數為8，航跡長度為106350 m，此后再增加調整半徑的值，路徑長度不再減少，校正次數不變，比基于單一距離約束下的算法提前達到最優。

圖 4 不同調整半徑對比圖Fig. 4 Comparison of different adjusting radius

2.1 基于航跡長度約束數據分析

基于航跡長度約束條件仿真實驗數據對比，結果見表3。可知，當調整半徑r小于10000 m 時，由于球形可航行區域內的點減少，找不到滿足校正條件的待選點，無法得到仿真結果。當調整半徑r=14000 m時，航跡長度最短為128909 m，總轉向角度最小為523.46。當調整半徑r=18000 m 時，校正點最少為11 次，與r=14000 m 相比，校正次數減少了2 次，但航跡長度增加1206 m，為了直觀呈現路徑平滑度，做出二維航跡平面圖。

表 3 不同調整半徑對比結果Tab. 3 Comparison results of different adjustment radius

如圖5 所示，在只考慮航跡長度約束的算法下，航跡路徑上的校正點會出現從起點到終點方向的折回、相鄰、平行的現象。當r=12000 m 時，航跡平面圖出現6 次折回和 1 次相鄰的現象，校正點次數多達18 次，航跡長度為143331 m，第5 個校正點偏向角最大為121.80°；當r=13000 m 時，航跡平面圖出現3 次折回、1 次平行和1 次相鄰的現象，校正點次數為16 次，航跡長度為144041 m，第13 個校正點偏向角最小為4.52°；當r=14000 m 時，航跡平面圖出現1 次折回和1 次相鄰現象；當r=18000 m 時，航跡平面圖出現2 次折回現象，航跡最靠近AB 直線方向，該算法達到最優，此后再增加調整半徑r的值，航跡長度和校正次數不在減少。

圖 5 不同調整半徑的航跡規劃平面圖Fig. 5 Plane diagram of route planning with different adjustment radius

2.2 基于轉向角約束數據分析

在航跡長度約束的基礎上增加轉向角約束條件對比數據分析，其具體結果見表4。

由表4 可知，調整半徑r增大時，航跡長度變短，校正點次數減少，總轉向角度變小。當半徑r達到14000 m 時，再次增加調整半徑r的大小，其航跡長度、校正點個數和總轉向角保持不變，說明算法達到最優值，為了直觀呈現路徑平滑度，做出二維航跡平面圖。

表 4 不同調整半徑對比結果Tab. 4 Comparison results of different adjustment radius

如圖6 所示，當r=11000 m 時，校正點次數多達13次，所得的航跡長度為115848 m，總轉向角度為349.24°，第10 個校正點轉向角最大，最大值為64.34°；當r=12000 m 時，校正點次數為10 次，所得的航跡長度為111306 m, 總轉向角度為202.87°，與r=11000 m 相比，校正點減少3 次，總轉向角減少146.37°，當r=13000 m時，經過9 個校正點，所得的航跡長度為111926 m，與r=12000 m 時的航跡對比，雖然校正次數減少1 次，但航跡距離增加620 m，當r=14000 m 時，經過的校正點8 個，所得的航跡長度為106350 m，第7 個校正點轉向角最小，最小值為5.22°，總轉向角度變小為120.66°，UUV 航行軌跡為A-521-64-80-170-278-369-214-397-B，航跡方向最接近直線AB，得到最優解。

圖 6 不同調整半徑的航跡規劃圖Fig. 6 Path planning with different adjustment radius

結果表明，在合理范圍調整半徑r，能夠有效縮短航跡長度，減少校正次數和總轉向角度。如表5 所示，對比2 種方法，當考慮轉向角約束條件時，校正次數為8 次，優于單一路徑最短規劃方法的11 次，路徑長為106350 m，相較于單一路徑最短規劃方法的130115 m，其規劃航跡長度減少了23765 m，航跡平滑沒有折回、比鄰、平行的情況，滿足UUV 在水下復雜環境中的航跡規劃，圖7 為2 種約束條件下對應的最優三維航跡圖。