唐文政,胡 測(cè),梁 鏡
(中國船舶重工集團(tuán)有限公司第七一〇研究所,湖北 宜昌 443003)
隨著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ù)證明該控制方法的有效性。
作為本文研究對(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)。
在完成抵近探測(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
在路徑跟蹤階段,控制器根據(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)速值。
鑒于研究對(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)保持水平位置,其中位置控制偏差為
對(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
采用自研欠驅(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
本文以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ǔ)。