針對(duì)多域協(xié)調(diào)的水下無(wú)人船路徑規(guī)劃及軌跡追蹤研究

趙永鵬

（貴州大學(xué)機(jī)械工程學(xué)院，貴州 貴陽(yáng)550025）

1 概述

近年來(lái)，隨著人類對(duì)海洋的勘探開(kāi)發(fā)日益頻繁，水下定位技術(shù)在鋪設(shè)海底光纜線路、獲取大規(guī)模海洋空間和環(huán)境數(shù)據(jù)、勘探開(kāi)發(fā)海洋資源等領(lǐng)域呈現(xiàn)多樣化。人們?cè)趯?shí)際應(yīng)用中對(duì)定位設(shè)備的性能、精度、成本和安全性都提出了更高的要求。水下無(wú)人船是一種攜帶傳感器的無(wú)纜平臺(tái)，能夠在靠近海床的高度進(jìn)行測(cè)量。將傳感器承載平臺(tái)靠近海底可以顯著提高采集數(shù)據(jù)的分辨率。

選擇合適的控制策略，充分了解運(yùn)動(dòng)關(guān)系，對(duì)潛艇制導(dǎo)導(dǎo)航系統(tǒng)的設(shè)計(jì)和研制具有重要意義。然而，與此相關(guān)的大多數(shù)流行研究都集中在一些簡(jiǎn)化的非線性運(yùn)動(dòng)關(guān)系的降階。典型地，潛艇的任務(wù)設(shè)計(jì)過(guò)程包括三個(gè)主要的挑戰(zhàn)組成部分: 方向舵尺寸的優(yōu)化(在設(shè)計(jì)狀態(tài)下決定);潛艇自動(dòng)駕駛儀的設(shè)計(jì)，以保證在物理允許的情況下平穩(wěn)操作，以減少液壓油的消耗，從而減少噪音的產(chǎn)生，并減少在復(fù)雜情況下(例如，沿海水域、導(dǎo)彈發(fā)射或侵略性采礦)過(guò)度運(yùn)動(dòng)所造成的脆弱性。

2 模型建立

2.1 定位模型

測(cè)距定位系統(tǒng)一般是由導(dǎo)航控制單元、發(fā)射換能器、水聽(tīng)器和應(yīng)答器四部分組成。在實(shí)際工作中，還需要無(wú)人船、GPS 系統(tǒng)、計(jì)算機(jī)等。發(fā)射換能器和水聽(tīng)器一般安裝在船底或船側(cè)，應(yīng)答器固定在水下節(jié)點(diǎn)。單點(diǎn)測(cè)距定位系統(tǒng)的工作原理：導(dǎo)航控制單元產(chǎn)生的詢問(wèn)電信號(hào)。發(fā)射換能器將電信號(hào)轉(zhuǎn)換為聲學(xué)信號(hào)，并將其發(fā)射到水中。當(dāng)應(yīng)答器接收到詢問(wèn)信號(hào)時(shí)，它將發(fā)出帶有時(shí)間戳的響應(yīng)信號(hào)。

圖1 四種不同的情況涉及頂部聲學(xué)定位系統(tǒng)

與前文所述一樣，USBL 系統(tǒng)根據(jù)聲學(xué)條件包含固有偏差，沒(méi)有充分考慮所有的偏差來(lái)源。通常，我們?cè)谶@種情況下的操作程度是不同的。

我們另外取矩陣R2∈SO(2)表示可能時(shí)變航向下在(水平)平面上的旋轉(zhuǎn)矩陣：

斜對(duì)稱矩陣S(λ)∈SS(3)為向量叉積算子，旋轉(zhuǎn)矩陣Rp(f(θ))對(duì)一個(gè)角度θ 關(guān)于主軸的導(dǎo)數(shù)等價(jià)于矩陣乘以一個(gè)斜對(duì)稱矩陣，其中E 是對(duì)應(yīng)于主軸的基向量。

考慮二維空間中的定位方法，基于測(cè)量水下節(jié)點(diǎn)接收信標(biāo)節(jié)點(diǎn)查詢信號(hào)到達(dá)時(shí)間的傾斜距離定位方法。必須至少有三個(gè)信標(biāo)節(jié)點(diǎn)。利用TOA 計(jì)算水下節(jié)點(diǎn)和不同信標(biāo)節(jié)點(diǎn)的傾斜距離信息。水下節(jié)點(diǎn)的實(shí)際位置在環(huán)內(nèi)，測(cè)量?jī)A斜誤差為環(huán)寬。提出了一種基于測(cè)量?jī)A斜誤差的區(qū)域交叉口定位方法。無(wú)人信標(biāo)船按照規(guī)劃路徑在水上航行，軌跡如圖2 所示。為無(wú)人信標(biāo)船的聲學(xué)測(cè)量?jī)A斜位置。“為水下節(jié)點(diǎn)的位置。”圖2 中的環(huán)是一次傾斜測(cè)量后水下節(jié)點(diǎn)可能存在的區(qū)域通過(guò)交環(huán)逐漸減小水下節(jié)點(diǎn)的定位范圍。利用至少三個(gè)圓的交點(diǎn)區(qū)域的中心作為水下節(jié)點(diǎn)的定位位置。

圖2 無(wú)人信標(biāo)船測(cè)量航跡無(wú)人信標(biāo)船測(cè)量航跡

2.2 準(zhǔn)確定位模型

本工作中制定的跟蹤目標(biāo)的規(guī)劃范圍是在相對(duì)較長(zhǎng)的時(shí)間范圍內(nèi)執(zhí)行的。由于這個(gè)原因，短期動(dòng)態(tài)變得不那么重要了。因此，采用一個(gè)簡(jiǎn)單的欠驅(qū)動(dòng)船舶運(yùn)動(dòng)學(xué)模型來(lái)表示物理約束。取車輛狀態(tài)分別為北向位置、東向位置和航向輸入要計(jì)算的是前進(jìn)速度(浪涌)和偏航率。假定船舶由于水流具有可忽略的側(cè)向滑移和橫向運(yùn)動(dòng)。由于其深度，水下環(huán)境是一個(gè)三維空間。假設(shè)水下節(jié)點(diǎn)的深度信息H 為，則水下節(jié)點(diǎn)與信標(biāo)船在三維空間的傾斜距離L 由直達(dá)聲波飛行時(shí)間來(lái)測(cè)量將三維定位轉(zhuǎn)化為二維定位。無(wú)人信標(biāo)與水下節(jié)點(diǎn)的水平測(cè)量距離R 存在λ 乘以真實(shí)值為標(biāo)準(zhǔn)差的測(cè)量誤差。聲速誤差是測(cè)量誤差的主要來(lái)源，是一種隨機(jī)誤差。

設(shè)無(wú)人信標(biāo)船的坐標(biāo)為(x,y)。設(shè)水下節(jié)點(diǎn)的坐標(biāo)為(n,e)。水下節(jié)點(diǎn)位于半徑為的圓上R(R-U(1-λ) R,(1+λ)R))，以無(wú)人信標(biāo)船為中心。水下節(jié)點(diǎn)坐標(biāo)計(jì)算公式:

3 軌跡規(guī)劃

本文提出了一個(gè)軌跡規(guī)劃的最優(yōu)控制問(wèn)題。OCP 通過(guò)一個(gè)直接的多重射擊方法被實(shí)現(xiàn)為一個(gè)NLP。直接法將控制問(wèn)題離散化，在優(yōu)化前將控制問(wèn)題分解為射擊區(qū)間，并分別求解每個(gè)子問(wèn)題。

圖3 水下100 米時(shí)的穩(wěn)態(tài)標(biāo)準(zhǔn)偏差

多重射擊方法將控制輸入和狀態(tài)都視為決策變量。這導(dǎo)致問(wèn)題具有更大的規(guī)模和更多的自由度，但較少的非線性依賴于控制輸入。這提高了非線性問(wèn)題的收斂性和數(shù)值穩(wěn)定性。由于狀態(tài)被作為決策變量，也可以提供一個(gè)初步的猜測(cè)，以加快收斂。采用移動(dòng)視界方法，在有限的時(shí)間范圍內(nèi)解決問(wèn)題。

步驟1:模擬發(fā)送和接收聲音脈沖的過(guò)程。信標(biāo)船在第n 點(diǎn)的GPS 位置。

步驟2:用到達(dá)時(shí)間法測(cè)量距離。使用N T 反向估計(jì)節(jié)點(diǎn)與信標(biāo)船之間的水平距離。

步驟3:畫(huà)一個(gè)新的定位環(huán)，即加入極限方程。

步驟4:估計(jì)響應(yīng)節(jié)點(diǎn)的方位，求解方程，判斷定位區(qū)域是否縮小到需要的尺度。如果它沒(méi)有達(dá)到要求的規(guī)模，繼續(xù)下一步。

步驟5：修正航向角度。

令矩陣Pk, j 為拍攝間隔k 開(kāi)始時(shí)的協(xié)方差，其中j∈{1，…，nt}為目標(biāo)。考慮兩種公式。第一個(gè)公式假設(shè)所有車輛在整個(gè)射擊視界以相同的速度排列。在這種情況下，超光速率將根據(jù)被跟蹤車輛的數(shù)量而降低。第二種模式只能在每一個(gè)射擊視界內(nèi)使用一輛車。引入二元決策變量，將公式轉(zhuǎn)化為混合整數(shù)非線性規(guī)劃。二元決策變量bk,j∈{0,1}用于選擇在每個(gè)拍攝間隔范圍內(nèi)的車輛。

4 分析討論

通過(guò)一系列的仿真驗(yàn)證了目前所提出的方法。INS 的不確定性被認(rèn)為介于低端和高端系統(tǒng)之間。這樣做是為了在規(guī)劃范圍內(nèi)切實(shí)增加目標(biāo)的不確定性。通過(guò)在直線軌跡上測(cè)量水下航行器的位置而不將測(cè)量結(jié)果發(fā)送到INS，可以獲得合理的不確定性設(shè)置。NLP 參數(shù)的調(diào)整是通過(guò)首先將喘振和偏航速率約束設(shè)置為適合中型飛機(jī)的合理值并調(diào)整喘振懲罰表1 來(lái)完成的。以產(chǎn)生合理的軌跡。然后將其余參數(shù)調(diào)整為更傾向于平滑轉(zhuǎn)彎度變化的軌跡。運(yùn)動(dòng)學(xué)約束應(yīng)反映基本動(dòng)力系統(tǒng)的可實(shí)現(xiàn)性能。所有的模擬都使用移動(dòng)水平的方法進(jìn)行，其中軌跡在每次迭代中向前移動(dòng)一步。求解器是基于前一個(gè)迭代的解決方案啟動(dòng)的。第一次迭代使用的解是最小化到目標(biāo)的水平距離。

5 結(jié)論

本文提出了一種自主水面航行器的軌跡規(guī)劃方法。使用無(wú)人信標(biāo)船作為信標(biāo)節(jié)點(diǎn)降低了信標(biāo)節(jié)點(diǎn)的部署和回收成本。針對(duì)聲速誤差引起的測(cè)量誤差，提出了一種區(qū)域相交定位求解方法。通過(guò)優(yōu)化信標(biāo)船的航跡，快速減小了交點(diǎn)區(qū)域的面積。該方法通過(guò)測(cè)量信標(biāo)船與水下節(jié)點(diǎn)之間的距離，而不是測(cè)量水下節(jié)點(diǎn)的方向，從而實(shí)現(xiàn)遠(yuǎn)距離定位。軌跡最小化了一個(gè)或多個(gè)水下航行器的水平不確定性。假定水面車輛裝備有能夠測(cè)量到車輛的方向和距離的超短基線收發(fā)器。將目標(biāo)和約束表述為一個(gè)優(yōu)化問(wèn)題。解決這個(gè)問(wèn)題的前提是，在規(guī)劃范圍內(nèi)，每輛車的移動(dòng)都是已知的，例如，通過(guò)知道下一個(gè)路徑點(diǎn)。該解決方案適合實(shí)時(shí)重新規(guī)劃，以考慮中斷、計(jì)劃中不均衡的進(jìn)展或不可預(yù)見(jiàn)的中斷。由于方法的性質(zhì)，與直接配點(diǎn)法相關(guān)的代價(jià)與龐特里亞金最小原理相對(duì)應(yīng)的代價(jià)略有不同，因?yàn)榍罢呤且粋€(gè)數(shù)值過(guò)程，而后者是一個(gè)解析過(guò)程。在今后的研究中，應(yīng)考慮多環(huán)的路徑優(yōu)化方法。