欠驅(qū)動(dòng)AUV水平面動(dòng)力定位控制方法研究

唐文政，胡 測(cè)，梁 鏡

（中國船舶重工集團(tuán)有限公司第七一〇研究所，湖北 宜昌 443003）

0 引言

隨著AUV應(yīng)用領(lǐng)域逐漸拓展，各種任務(wù)場(chǎng)景對(duì)其運(yùn)動(dòng)控制提出了更高要求。在執(zhí)行抵近探測(cè)、懸停作業(yè)以及定點(diǎn)待命等任務(wù)時(shí)，往往需要 AUV具備自主靠近目標(biāo)點(diǎn)并保持位置穩(wěn)定的動(dòng)力定位能力。國內(nèi)外針對(duì)動(dòng)力定位技術(shù)已開展一系列研究，并在水面船舶領(lǐng)域已得到較好解決[1-2]。出于對(duì)成本、質(zhì)量以及推進(jìn)效率等方面的考慮，AUV的運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)往往被配置為欠驅(qū)動(dòng)式，但是僅通過較少輸入實(shí)現(xiàn)六自由度運(yùn)動(dòng)控制，在獲得諸多優(yōu)勢(shì)的同時(shí)也增加了動(dòng)力定位控制器的設(shè)計(jì)難度[3]。

對(duì)于AUV動(dòng)力定位問題，文獻(xiàn)[4]設(shè)計(jì)了滑模控制器并引入波浪擾動(dòng)預(yù)測(cè)，實(shí)現(xiàn)軌跡跟蹤控制與定點(diǎn)控制，并通過仿真進(jìn)行驗(yàn)證；文獻(xiàn)[5]考慮了未知的海流干擾，提出了一種基于反演技術(shù)的自適應(yīng)變結(jié)構(gòu)控制方法；文獻(xiàn)[6]將環(huán)境最優(yōu)動(dòng)力定位控制（WOPC，Weather Optimal Positioning Control）的概念應(yīng)用到全驅(qū)動(dòng)自主水下機(jī)器人，基于神經(jīng)網(wǎng)絡(luò)自適應(yīng)反演法設(shè)計(jì)了動(dòng)力定位控制器；文獻(xiàn)[7]提出了一種基于 Lyapunov 的模型預(yù)測(cè)控制方法，結(jié)合推力的優(yōu)化策略進(jìn)行動(dòng)力定位控制；文獻(xiàn)[8]針對(duì)全驅(qū)動(dòng) AUV近水面運(yùn)動(dòng)中自身慣量小且環(huán)境擾動(dòng)敏感的問題，提出了一種動(dòng)力定位控制方法。

以上控制方法大多基于復(fù)雜的非線性控制方法，且有些僅針對(duì)全驅(qū)動(dòng)配置，難以有效應(yīng)用到欠驅(qū)動(dòng)AUV的工程實(shí)踐中。因此，本文以自研AUV平臺(tái)為研究對(duì)象，考慮其欠驅(qū)動(dòng)特性與任務(wù)內(nèi)容，基于線性控制提出一種動(dòng)力定位控制方法，以實(shí)現(xiàn)自主抵近目標(biāo)點(diǎn)并在其附近小范圍區(qū)域保持水平位置穩(wěn)定的控制效果，并結(jié)合試驗(yàn)數(shù)據(jù)證明該控制方法的有效性。

1 AUV平臺(tái)運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)配置

作為本文研究對(duì)象的AUV平臺(tái)，其運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)包括2個(gè)垂直推進(jìn)器、2個(gè)水平推進(jìn)器以及1組水平舵。其中：垂直推進(jìn)器位于載體重心附近沿中軸線左右分布，提供垂向推力進(jìn)行低速運(yùn)動(dòng)下深度控制；水平推進(jìn)器位于載體中后段沿中軸線左右分布，提供縱向推力與轉(zhuǎn)艏力矩進(jìn)行縱向速度、縱向位置以及航向的控制，其中轉(zhuǎn)艏力矩通過左右推進(jìn)器轉(zhuǎn)速差產(chǎn)生；1組水平舵位于水平推進(jìn)器后，提供俯仰力矩進(jìn)行高速運(yùn)動(dòng)下深度/俯仰控制。

該平臺(tái)缺少側(cè)向推進(jìn)器提供側(cè)移作用力；在低速狀態(tài)下，其水平舵效率大幅降低，垂直推進(jìn)器采用左右分布形式，基本不具備俯仰作用力矩；而高速狀態(tài)下，其垂直推進(jìn)器效率大幅降低，基本不具備垂向作用力。根據(jù)以上運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)配置的分析，可判斷該AUV平臺(tái)屬于欠驅(qū)動(dòng)系統(tǒng)。

2 動(dòng)力定位控制

在完成抵近探測(cè)、懸停作業(yè)以及定點(diǎn)待命等任務(wù)內(nèi)容的過程中，往往需要AUV平臺(tái)能夠沿著預(yù)設(shè)路徑自主抵近目標(biāo)點(diǎn)，并最終在該點(diǎn)附近小范圍區(qū)域保持位置穩(wěn)定。考慮上述任務(wù)場(chǎng)景需求，設(shè)計(jì)動(dòng)力定位控制分為路徑跟蹤與區(qū)域鎮(zhèn)定2個(gè)階段。

以任務(wù)目標(biāo)點(diǎn)為圓心，在指定半徑范圍內(nèi)設(shè)置為抵近區(qū)域，該區(qū)域邊界作為路徑跟蹤與區(qū)域鎮(zhèn)定2個(gè)控制階段的分界線。其內(nèi)部進(jìn)一步設(shè)置較小半徑范圍內(nèi)為定位區(qū)域，AUV最終將在該區(qū)域內(nèi)保持位置實(shí)現(xiàn)懸停，如圖1所示。按照時(shí)間順序，啟動(dòng)任務(wù)后 AUV首先進(jìn)入路徑跟蹤階段，沿預(yù)設(shè)路徑向目標(biāo)點(diǎn)快速靠近；進(jìn)入抵近區(qū)域后 AUV切換到區(qū)域鎮(zhèn)定階段，降低轉(zhuǎn)速進(jìn)一步抵近目標(biāo)點(diǎn)，進(jìn)入定位區(qū)域后保持水平位置穩(wěn)定，若檢測(cè)受到干擾偏離定位區(qū)域則自主返回，從而實(shí)現(xiàn)從起始點(diǎn)出發(fā)沿預(yù)設(shè)路徑抵達(dá)目標(biāo)點(diǎn)并保持位置的動(dòng)力定位控制。

圖1 動(dòng)力定位控制示意圖Fig. 1 Schematic diagram of dynamic positioning control

2.1 路徑跟蹤階段

在路徑跟蹤階段，控制器根據(jù)預(yù)設(shè)路徑與實(shí)時(shí)導(dǎo)航信息，計(jì)算水平推進(jìn)器轉(zhuǎn)速指令值，發(fā)送至執(zhí)行機(jī)構(gòu)產(chǎn)生縱向推力與轉(zhuǎn)艏力矩，控制AUV沿著預(yù)設(shè)路徑進(jìn)入抵近區(qū)域。預(yù)設(shè)路徑通常為一系列路徑點(diǎn)構(gòu)成的分段直線，因此具體方法采用路徑點(diǎn)跟蹤控制。路徑起始點(diǎn)與載體當(dāng)前點(diǎn)的水平面位置坐標(biāo)在WGS-84坐標(biāo)系下，轉(zhuǎn)換到ENU坐標(biāo)系為

式中：（lo n, lat）為轉(zhuǎn)換點(diǎn)經(jīng)緯度坐標(biāo)；（lo n0, lat0）為基準(zhǔn)點(diǎn)經(jīng)緯度坐標(biāo)；R為地球基準(zhǔn)橢球體長(zhǎng)半徑。

在工程應(yīng)用中，實(shí)際平臺(tái)往往具有一定程度的左右結(jié)構(gòu)以及推力不對(duì)稱性，并且欠驅(qū)動(dòng)AUV無法在側(cè)流干擾時(shí)通過側(cè)向推力直接平衡干擾力，從而對(duì)路徑點(diǎn)跟蹤控制精度產(chǎn)生影響。因此，在常規(guī)的“視線法”制導(dǎo)方式基礎(chǔ)上，將路徑偏移量引入控制回路，如圖2所示。

圖2 路徑跟蹤控制示意圖Fig. 2 Schematic diagram of path tracking control

式中：nL與nR分別為左右推進(jìn)器轉(zhuǎn)速指令值；n0為該航段設(shè)置的基礎(chǔ)轉(zhuǎn)速值。

2.2 區(qū)域鎮(zhèn)定階段

鑒于研究對(duì)象的欠驅(qū)動(dòng)特性，區(qū)域鎮(zhèn)定階段需結(jié)合縱向位置控制與航向控制實(shí)現(xiàn)水平位置控制。根據(jù)式（8）的運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)動(dòng)力分配，水平推進(jìn)器轉(zhuǎn)速指令值由基礎(chǔ)轉(zhuǎn)速值與轉(zhuǎn)速增量值 2部分構(gòu)成，區(qū)域鎮(zhèn)定階段的基礎(chǔ)轉(zhuǎn)速值為

式中：keP為縱向位置控制系數(shù)；d為縱向位置偏差。

航向控制以視線角作為目標(biāo)航向角，可根據(jù)式（2）得出，再計(jì)算轉(zhuǎn)速增量值為

式中， kPφ2與 kDφ2為航向控制系數(shù)。

AUV抵達(dá)定位區(qū)域后進(jìn)入懸停狀態(tài)，根據(jù)導(dǎo)航信息監(jiān)視位置偏差，通過運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)保持水平位置，其中位置控制偏差為

3 實(shí)驗(yàn)結(jié)果分析

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

對(duì)于缺乏側(cè)向機(jī)動(dòng)能力的欠驅(qū)動(dòng)AUV平臺(tái)，區(qū)域鎮(zhèn)定階段的水平位置控制是整個(gè)動(dòng)力定位過程的關(guān)鍵，因此首先在MATLAB環(huán)境下進(jìn)行水平位置控制仿真實(shí)驗(yàn)。ENU坐標(biāo)系下，取目標(biāo)點(diǎn)為基準(zhǔn)點(diǎn)(0,0)，設(shè)置航行器初始航速與航向?yàn)?，初始位置為(-3 0,-3 0)，仿真時(shí)間為1 000 s，定位區(qū)域?yàn)橐阅繕?biāo)點(diǎn)為圓心的2 m半徑內(nèi)區(qū)域。

仿真試驗(yàn)效果圖如圖3-4所示，航行器啟動(dòng)后逐漸調(diào)整航向靠近目標(biāo)點(diǎn)，首次抵達(dá)后由于慣性出現(xiàn)小量超調(diào)，通過調(diào)整逐漸穩(wěn)定在定位區(qū)域內(nèi)，位置偏差小于2 m。

圖3 仿真實(shí)驗(yàn)航行軌跡圖Fig. 3 Trajectory graph of simulation

圖4 仿真實(shí)驗(yàn)位置偏差曲線圖Fig. 4 Position deviation graph of simulation test

3.2 現(xiàn)場(chǎng)實(shí)驗(yàn)

采用自研欠驅(qū)動(dòng)AUV平臺(tái)，通過湖上實(shí)驗(yàn)對(duì)整個(gè)動(dòng)力定位控制流程進(jìn)行驗(yàn)證。設(shè)置目標(biāo)點(diǎn)附近2 m半徑內(nèi)區(qū)域?yàn)槎ㄎ粎^(qū)域，在距離其150 m處選取起始點(diǎn)，預(yù)設(shè)航路包含長(zhǎng)度分別約60 m與100 m兩個(gè)直線航段。

整個(gè)動(dòng)力定位過程持續(xù) 300 s，AUV航行軌跡與推進(jìn)器轉(zhuǎn)速如圖 5所示，啟動(dòng)任務(wù)后航行器首先進(jìn)入路徑跟蹤階段，沿預(yù)設(shè)航路靠近目標(biāo)點(diǎn)，在138 s進(jìn)入抵近區(qū)域后切換為區(qū)域鎮(zhèn)定階段，以較低轉(zhuǎn)速進(jìn)一步靠近目標(biāo)點(diǎn)，最終穩(wěn)定在定位區(qū)域。

圖5 動(dòng)力定位實(shí)驗(yàn)航行軌跡圖Fig. 5 Trajectory graph of dynamic positioning test

在路徑跟蹤階段中，AUV基礎(chǔ)轉(zhuǎn)速值設(shè)置為800 r/min，在41 s抵達(dá)中轉(zhuǎn)點(diǎn)的有效判定區(qū)域，切換至第2航段開始轉(zhuǎn)彎，進(jìn)入抵近區(qū)域后結(jié)束路徑點(diǎn)跟蹤控制，路徑偏移量在1 m內(nèi)，如圖6所示。

圖6 路徑跟蹤階段偏航距曲線圖Fig. 6 Drift distance graph in path tracking positioning test

隨后AUV進(jìn)入持續(xù)162 s的區(qū)域鎮(zhèn)定階段，逐漸低速抵近目標(biāo)點(diǎn)，10 s后首次進(jìn)入定位區(qū)域并繼續(xù)調(diào)整，保持水平位置偏差在 2 m內(nèi)。以目標(biāo)點(diǎn)為基準(zhǔn)點(diǎn)(0,0)，階段切換時(shí)刻為時(shí)間軸起點(diǎn)，該階段航行軌跡與位置偏差分別如圖7-8所示。

圖7 區(qū)域鎮(zhèn)定階段航行軌跡圖Fig. 7 Trajectory graph in zone stabilization

圖8 區(qū)域鎮(zhèn)定階段位置偏差曲線圖Fig. 8 Position deviation graph in zone stabilization

為驗(yàn)證區(qū)域鎮(zhèn)定階段AUV具備在干擾后的水平位置恢復(fù)能力，對(duì)航行器施加外部作用使位置偏離，該過程軌跡與推進(jìn)器轉(zhuǎn)速如圖9。在32 s時(shí)位置誤差達(dá)到6.1 m，此時(shí)解除干擾，航行器逐漸返回定位區(qū)域，由于慣性出現(xiàn)小幅度超調(diào)，經(jīng)過調(diào)整后位置偏差重新控制在2 m內(nèi)，如圖10所示。

圖9 外力干擾下航行軌跡圖Fig. 9 Trajectory graph under external force disturbance

圖10 外力干擾下位置偏差曲線圖Fig. 10 Position deviation graph under external force disturbance

4 結(jié)束語

本文以AUV為研究對(duì)象，從實(shí)際工程應(yīng)用角度出發(fā)，結(jié)合常規(guī)任務(wù)場(chǎng)景對(duì)動(dòng)力定位功能的需求，針對(duì)欠驅(qū)動(dòng)式運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)配置，基于線性控制提出一種水平面動(dòng)力定位控制方法。

根據(jù)作業(yè)流程，設(shè)置路徑跟蹤與區(qū)域鎮(zhèn)定2個(gè)控制階段；針對(duì)預(yù)設(shè)路徑的常用設(shè)置方式選擇路徑點(diǎn)跟蹤控制方法，在“視線法”控制回路中引入路徑偏移量，以降低加工誤差與環(huán)境干擾等帶來的路徑控制偏差；結(jié)合縱向位置控制與航向控制，進(jìn)行無側(cè)向推力的水平面位置控制。

最終通過仿真與現(xiàn)場(chǎng)試驗(yàn)，證明AUV能夠沿預(yù)設(shè)路徑自主抵近目標(biāo)點(diǎn)，并保持在該點(diǎn)附近2 m范圍內(nèi)區(qū)域，從而驗(yàn)證該動(dòng)力定位控制方法的可行性，為欠驅(qū)動(dòng)AUV順利完成抵近探測(cè)、懸停作業(yè)與定點(diǎn)待命等任務(wù)奠定基礎(chǔ)。