一種基于多目標(biāo)約束下的UUV 航跡規(guī)劃算法

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

(浙江海洋大學(xué) 船舶與海運(yùn)學(xué)院，浙江 舟山 316022)

0 引 言

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

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

1 問題描述及模型的構(gòu)建

1.1 問題描述

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

1）UUV 的航跡長(zhǎng)度盡可能的短；

2）經(jīng)過校正點(diǎn)的次數(shù)盡可能的少；

3）UUV 在轉(zhuǎn)彎時(shí)受到自身機(jī)動(dòng)性能的限制，無法突然改變前進(jìn)方向，其轉(zhuǎn)向角應(yīng)在一定的限制范圍內(nèi)，考慮到航跡的光滑度，要求轉(zhuǎn)向角盡可能的小，UUV 允許的最大轉(zhuǎn)向角為 ψmax。

1.2 UUV 模型構(gòu)建

UUV 自起點(diǎn)A經(jīng) 過n個(gè)校正點(diǎn)航行至終點(diǎn)B，經(jīng)過校正點(diǎn)時(shí)進(jìn)行誤差校正，建立以起點(diǎn)A為圓心，r為半徑的球形可航行區(qū)域，球形區(qū)域內(nèi)的誤差校正點(diǎn)為待選校正點(diǎn)。由誤差校正和轉(zhuǎn)向角度約束可知，求待選節(jié)點(diǎn)P0時(shí) ，P點(diǎn)距離A點(diǎn)距離最近、轉(zhuǎn)向角最小的點(diǎn)會(huì)被優(yōu)先選中，經(jīng)過校正點(diǎn)校正后該方向的誤差校正為0，另一方向誤差保持不變，尋找到第二個(gè)校正點(diǎn)P2時(shí) ，以P2為 圓心r為半徑劃分球形可航行區(qū)域，在可航行區(qū)域內(nèi)選擇下一個(gè)校正點(diǎn)，重復(fù)選取過程直至終點(diǎn)B。選取過程如圖1 所示。

圖 1 校正點(diǎn)選取示意圖Fig. 1 Correction point selection schematic

1.3 數(shù)學(xué)模型構(gòu)建

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

表 1 符號(hào)變量說明Tab. 1 Symbolic variable description

1）目標(biāo)函數(shù)

要使航跡長(zhǎng)度盡可能短，即目標(biāo)函數(shù)為：

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

2）校正條件約束

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

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

當(dāng)μ=1 時(shí)，進(jìn)行垂直誤差校正，α1和 α2為滿足垂直誤差校正的垂直和水平誤差最大值，校正條件為：

校正后：

當(dāng) μ=0 時(shí)，進(jìn)行水平誤差校正，β1和 β2為滿足水平誤差校正的垂直和水平誤差最大值，校正條件為：

校正后：

到達(dá)終點(diǎn)時(shí)垂直和水平誤差均滿足以下條件：

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

3）轉(zhuǎn)向角條件約束

圖 2 轉(zhuǎn)向角約束示意圖Fig. 2 Diagram of steering angle constraint

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

1.4 算法步驟及流程

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

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

步驟2求當(dāng)前點(diǎn)到待選點(diǎn)偏向夾角 ψi，在可行域內(nèi)選取 ψi最小的點(diǎn)，進(jìn)行水平或垂直校正，作為下次迭代的起點(diǎn)。

步驟3從當(dāng)前點(diǎn)出發(fā)到終點(diǎn)B 的航跡距離是否滿足條件3，如果滿足，則輸出最終航跡，停止迭代，如若不能到達(dá)B 點(diǎn)，迭代次數(shù)加 1，返回步驟 1。

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

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

2 仿真實(shí)驗(yàn)

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

表 2 部分校正點(diǎn)坐標(biāo)Tab. 2 Correction point coordinates

基于表2 數(shù)據(jù)，針對(duì)上述2 種約束條件，調(diào)整半徑10000～20000 m，利用 Matlab 數(shù)值模擬仿真，可以得出不同調(diào)整半徑r對(duì)應(yīng)的校正點(diǎn)次數(shù)和航跡長(zhǎng)度。由圖4 可知，基于距離約束時(shí)，調(diào)整半徑小于11000 m得不到航跡規(guī)劃結(jié)果，當(dāng)調(diào)整半徑r=14000 m 時(shí)航跡長(zhǎng)度最短，調(diào)整半徑r=18000 m 時(shí)校正點(diǎn)次數(shù)最少。基于距離和轉(zhuǎn)向角約束情況下，當(dāng)r=14000 m 時(shí)校正次數(shù)為8，航跡長(zhǎng)度為106350 m，此后再增加調(diào)整半徑的值，路徑長(zhǎng)度不再減少，校正次數(shù)不變，比基于單一距離約束下的算法提前達(dá)到最優(yōu)。

圖 4 不同調(diào)整半徑對(duì)比圖Fig. 4 Comparison of different adjusting radius

2.1 基于航跡長(zhǎng)度約束數(shù)據(jù)分析

基于航跡長(zhǎng)度約束條件仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比，結(jié)果見表3。可知，當(dāng)調(diào)整半徑r小于10000 m 時(shí)，由于球形可航行區(qū)域內(nèi)的點(diǎn)減少，找不到滿足校正條件的待選點(diǎn)，無法得到仿真結(jié)果。當(dāng)調(diào)整半徑r=14000 m時(shí)，航跡長(zhǎng)度最短為128909 m，總轉(zhuǎn)向角度最小為523.46。當(dāng)調(diào)整半徑r=18000 m 時(shí)，校正點(diǎn)最少為11 次，與r=14000 m 相比，校正次數(shù)減少了2 次，但航跡長(zhǎng)度增加1206 m，為了直觀呈現(xiàn)路徑平滑度，做出二維航跡平面圖。

表 3 不同調(diào)整半徑對(duì)比結(jié)果Tab. 3 Comparison results of different adjustment radius

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

圖 5 不同調(diào)整半徑的航跡規(guī)劃平面圖Fig. 5 Plane diagram of route planning with different adjustment radius

2.2 基于轉(zhuǎn)向角約束數(shù)據(jù)分析

在航跡長(zhǎng)度約束的基礎(chǔ)上增加轉(zhuǎn)向角約束條件對(duì)比數(shù)據(jù)分析，其具體結(jié)果見表4。

由表4 可知，調(diào)整半徑r增大時(shí)，航跡長(zhǎng)度變短，校正點(diǎn)次數(shù)減少，總轉(zhuǎn)向角度變小。當(dāng)半徑r達(dá)到14000 m 時(shí)，再次增加調(diào)整半徑r的大小，其航跡長(zhǎng)度、校正點(diǎn)個(gè)數(shù)和總轉(zhuǎn)向角保持不變，說明算法達(dá)到最優(yōu)值，為了直觀呈現(xiàn)路徑平滑度，做出二維航跡平面圖。

表 4 不同調(diào)整半徑對(duì)比結(jié)果Tab. 4 Comparison results of different adjustment radius

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

圖 6 不同調(diào)整半徑的航跡規(guī)劃圖Fig. 6 Path planning with different adjustment radius

結(jié)果表明，在合理范圍調(diào)整半徑r，能夠有效縮短航跡長(zhǎng)度，減少校正次數(shù)和總轉(zhuǎn)向角度。如表5 所示，對(duì)比2 種方法，當(dāng)考慮轉(zhuǎn)向角約束條件時(shí)，校正次數(shù)為8 次，優(yōu)于單一路徑最短規(guī)劃方法的11 次，路徑長(zhǎng)為106350 m，相較于單一路徑最短規(guī)劃方法的130115 m，其規(guī)劃航跡長(zhǎng)度減少了23765 m，航跡平滑沒有折回、比鄰、平行的情況，滿足UUV 在水下復(fù)雜環(huán)境中的航跡規(guī)劃，圖7 為2 種約束條件下對(duì)應(yīng)的最優(yōu)三維航跡圖。

表 5 兩種方法對(duì)比Tab. 5 Comparison of two methods

圖 7 不同約束下最優(yōu)三維空間航跡圖Fig. 7 Optimal three-dimensional space trajectory under different constraints

3 結(jié) 語

本文圍繞UUV 在復(fù)雜海洋環(huán)境中航行時(shí)誤差校正問題，建立單目標(biāo)函數(shù)，根據(jù)約束條件的不同，在基于單一航跡長(zhǎng)度最短規(guī)劃基礎(chǔ)上，提出考慮轉(zhuǎn)向角約束的最小轉(zhuǎn)向角航跡規(guī)劃算法，通過對(duì)比可以發(fā)現(xiàn)，考慮轉(zhuǎn)向角約束條件時(shí)，航跡長(zhǎng)度明顯縮短，校正次數(shù)減少，路徑光滑，滿足航行要求。