一種無人艇路徑跟蹤控制器的設(shè)計與驗證

文元橋， 楊 吉， 周 陽， 高欣國， 肖長詩

(1.武漢理工大學(xué) 航運學(xué)院， 武漢 430063； 2.內(nèi)河航運技術(shù)湖北省重點實驗室， 武漢 430063;3.國家水運安全工程技術(shù)研究中心， 武漢 430063)

無人艇(Unmanned Surface Vessel，USV)作為一種能自主航行和智能作業(yè)的無人駕駛小型船舶，隨著傳感器技術(shù)的進步和現(xiàn)代控制理論的發(fā)展，逐步從仿真模擬研究轉(zhuǎn)入橋梁檢測[1]、海洋測繪[2]、水質(zhì)監(jiān)測[3]和水上搜救[4]等工程應(yīng)用研究中。

無人艇路徑跟蹤控制是開展無人艇編隊控制和協(xié)同控制等研究的基礎(chǔ)。路徑跟蹤控制器一般具有2種結(jié)構(gòu)：

1) 將控制器分為外環(huán)制導(dǎo)和內(nèi)環(huán)控制2個環(huán)節(jié)。

2) 將制導(dǎo)和控制集成在一起，這種結(jié)構(gòu)的控制器通?；谏窠?jīng)網(wǎng)絡(luò)[5-6]等方法實現(xiàn)。

當(dāng)前路徑跟蹤控制器大多采用內(nèi)外環(huán)分離的結(jié)構(gòu)。無人艇路徑跟蹤控制器的內(nèi)環(huán)控制方法主要有滑?？刂芠7]、雙極模糊控制[8]、反步法[9]和動態(tài)面控制[10]等，這些算法大多對數(shù)學(xué)模型的精度和傳感器的性能要求較高，導(dǎo)致在工程上較難實現(xiàn)。無人艇路徑跟蹤控制器的外環(huán)制導(dǎo)律主要是常規(guī)的Line-Of-Sight (LOS)制導(dǎo)算法[11]和針對外界環(huán)境干擾的Integral Line-Of-Sight(ILOS)制導(dǎo)算法[12]，這2種算法只適于對直線參考路徑進行跟蹤[13]。實際應(yīng)用中的無人艇因航行和作業(yè)任務(wù)的需要，應(yīng)兼具對直線路徑和曲線路徑進行跟蹤的能力。

本文以武漢理工大學(xué)iNav-II型USV(見圖1)為研究對象，對其在實際應(yīng)用中遇到的路徑跟蹤控制問題進行研究。首先，在利用帶遺忘因子的遞推最小二乘法對無人艇的狀態(tài)空間型船舶線性數(shù)學(xué)模型進行在線參數(shù)辨識的基礎(chǔ)上，實現(xiàn)對USV操縱響應(yīng)模型參數(shù)的在線估算。其次，基于內(nèi)?？刂坪蚅1制導(dǎo)算法設(shè)計并實現(xiàn)無人艇的路徑跟蹤控制器。最后，通過一系列實船試驗驗證路徑跟蹤控制器的性能。

1 iNav-II型USV

iNav-II型USV是純電動的玻璃鋼制無人艇，設(shè)計總長3.96 m，型寬1.55 m，吃水0.3～0.5 m。該USV的推進器是一臺吊艙推進器，其偏轉(zhuǎn)角度限制為-25°～25°。iNav-II型無人艇配置有毫米波雷達、風(fēng)速風(fēng)向儀、慣性測量與導(dǎo)航系統(tǒng)、差分全球定位系統(tǒng)(Differential Global Positioning System, DGPS)和船岸無線通信系統(tǒng)。

2 USV運動數(shù)學(xué)模型和參數(shù)估計

圖2為iNav-II型USV平面運動示意，其中：Ψ為USV的艏向角；δr為吊艙推進器的偏轉(zhuǎn)角度。USV的狀態(tài)空間型船舶線性數(shù)學(xué)模型可表示為

(1)

根據(jù)USV上DGPS輸出的經(jīng)緯度坐標數(shù)據(jù)可計算出USV在大地坐標系下正東方向和正北方向上的速度分量Ev和Nv。[14]結(jié)合陀螺羅經(jīng)輸出的艏向角Ψ，可通過式(2)計算得到在USV附體坐標系下船體bx軸上的速度u和by軸上的速度v。艏搖角速度r由USV上安裝的高精度慣性測量單元(IMU)測量得到。

(2)

將得到的USV運動狀態(tài)參數(shù)代入到式(1)中，并在遞推最小二乘法[15]的基礎(chǔ)上引入值為0.98的遺忘因子，可實現(xiàn)對USV的狀態(tài)空間型船舶線性數(shù)學(xué)模型參數(shù)的在線辨識。

將式(1)解耦艏搖和橫移運動并忽略橫移項的影響，經(jīng)過拉式變換可得一階線性Nomoto模型為

(3)

在USV做定常運動、舵角較小的情況下，該模型可近似描述USV的操縱響應(yīng)特性。采用該模型可大大簡化角速度控制器的設(shè)計。式(3)中USV的操縱性指數(shù)K和T可在對式(1)進行在線參數(shù)辨識的基礎(chǔ)上通過式(4)計算得到。[16]

(4)

3 USV路徑跟蹤控制器設(shè)計

3.1 路徑跟蹤控制器結(jié)構(gòu)

圖3為路徑跟蹤控制器結(jié)構(gòu)。USV路徑跟蹤控制器采用制導(dǎo)環(huán)節(jié)與控制環(huán)節(jié)分離的結(jié)構(gòu)，由內(nèi)環(huán)基于內(nèi)?？刂频腢SV艏搖角速度控制器和外環(huán)L1制導(dǎo)律2部分組成。L1制導(dǎo)律根據(jù)參考路徑信息和USV的運動狀態(tài)數(shù)據(jù)輸出無人艇跟蹤參考路徑所需的參考艏搖角速度指令rcmd。制導(dǎo)環(huán)節(jié)輸出的參考艏搖角速度指令作為輸入信號進入控制環(huán)節(jié)中，而控制環(huán)節(jié)輸出的吊艙推進器偏轉(zhuǎn)角度指令δr-cmd將由吊艙推進器的轉(zhuǎn)向結(jié)構(gòu)予以執(zhí)行。

3.2 基于內(nèi)?？刂频腢SV艏搖角速度控制器

內(nèi)?？刂?Internal Model Control, IMC)是在Smith預(yù)估控制的基礎(chǔ)上發(fā)展起來的，具有控制結(jié)構(gòu)簡單、對模型誤差具有較強的魯棒性、調(diào)節(jié)參數(shù)少和調(diào)參方向明確等特點。[17]

(5)

(6)

(7)

(8)

為便于在實船試驗中實現(xiàn)式(8)，進一步將式(8)改寫為經(jīng)典的PID控制結(jié)構(gòu)，則式(8)可等效為一個PI控制器，即

(9)

(10)

(11)

由式(9)可知，基于內(nèi)?？刂频腢SV艏搖角速度控制器中含有USV操縱性指數(shù)K、T和增益參數(shù)β等3個參數(shù)?？刂破髦械腢SV操縱性指數(shù)K和T由上述在線參數(shù)估計得到。因此，基于內(nèi)?？刂频腢SV艏搖角速度控制器在實際應(yīng)用中還需通過現(xiàn)場調(diào)試來確定影響控制器響應(yīng)速度和魯棒性的增益參數(shù)β。

3.3 L1制導(dǎo)律

PARK等[19]在短程戰(zhàn)術(shù)導(dǎo)彈末端制導(dǎo)律中的比例導(dǎo)引法的基礎(chǔ)上提出L1制導(dǎo)算法，并通過理論分析和開展基于L1制導(dǎo)算法的固定翼無人機飛行試驗來證明L1制導(dǎo)算法的穩(wěn)定性。L1制導(dǎo)算法既適用于直線路徑制導(dǎo)，也適用于曲線路徑制導(dǎo)，且其因在曲線路徑制導(dǎo)方面較好的性能表現(xiàn)而被較多固定翼無人機所采用。因此，采用L1制導(dǎo)算法作為iNav-II型USV路徑跟蹤的制導(dǎo)律。

L1制導(dǎo)算法見圖6，其中：u為艏向速度；L1為USV到參考點的直線距離；η為艏向與USV和參考點連線的夾角。L1為定值，結(jié)合當(dāng)前船位和參考路徑信息可確定參考路徑上參考點的坐標。將USV回到參考路徑的運動過程近似看成圓周運動，由式(12)可計算出USV從當(dāng)前船位跟蹤參考路徑所需的橫向加速度指令acmd。結(jié)合式(13)，可根據(jù)橫向加速度acmd進一步求得USV進行路徑跟蹤所需的參考艏搖角速度rcmd，從而實現(xiàn)USV路徑跟蹤過程中的制導(dǎo)環(huán)節(jié)。

(12)

L1=2Rsinη

(13)

4 實船試驗

在武漢市東湖水域進行實船試驗，試驗水域開闊且無遮擋，試驗船速為2.0 m/s，試驗場景見圖7。實船試驗中內(nèi)?？刂频脑鲆鎱?shù)β設(shè)為4.25，L1制導(dǎo)律中的L1設(shè)為15 m。試驗過程中在線估計出來的iNav-II型USV的操縱性指數(shù)K約為0.75，T約為1.2。

為驗證所設(shè)計的基于內(nèi)?？刂频腢SV艏搖角速度控制器的實際控制效果，分別進行參考艏搖角速度為5 (°)/s和-12 (°)/s的USV艏搖角速度控制試驗。艏搖角速度控制試驗結(jié)果分別見圖8和圖9，試驗結(jié)果表明，基于內(nèi)?？刂平Y(jié)構(gòu)的USV艏搖角速度控制器能有效控制USV的艏搖角速度，且在艏搖角速度保持階段的誤差較小。

進行直線路徑跟蹤試驗(見圖10)。直線路徑跟蹤試驗的參考路徑約長410 m。直線路徑跟蹤的航跡誤差見圖11，試驗結(jié)果表明所設(shè)計的路徑跟蹤控制器能較好地實現(xiàn)iNav-II型USV對直線路徑的跟蹤，且航跡誤差控制在±1 m以內(nèi)。

USV實際航行過程中因避碰或作業(yè)任務(wù)的需求，需具有跟蹤連續(xù)曲線路徑進行機動航行的能力。因此，設(shè)計一組參考路徑由4個半徑為80 m的圓弧組成的連續(xù)曲線路徑跟蹤試驗(見圖12)。試驗結(jié)果表明，所設(shè)計的路徑跟蹤控制器能實現(xiàn)iNav-II型USV對連續(xù)曲線路徑的跟蹤，且航跡誤差可控制在±5 m以內(nèi)。

圖14為圓形路徑跟蹤的軌跡，假設(shè)iNav-II型USV執(zhí)行對圖中所示的氣脹式救生筏的警戒護航任務(wù)，警戒區(qū)域是以救生筏為圓心的半徑50 m的圓形區(qū)域，USV對這一圓形參考路徑進行路徑跟蹤。圓形參考路徑跟蹤結(jié)果表明，USV進行圓形路徑跟蹤的航跡誤差在±5 m以內(nèi)，能實現(xiàn)無人艇對救生筏的警戒護航(見圖15)。

USV在實際執(zhí)行任務(wù)時的參考路徑通常由路徑規(guī)劃算法運算生成，且參考路徑多由直徑路徑和曲線路徑混合組成。因此，設(shè)計一組基于Dubins路徑[20]的復(fù)雜路徑跟蹤試驗(見圖16)。參考路徑包含9段直線路徑和11段曲線路徑，總長約2 370 m。

復(fù)雜路徑跟蹤試驗結(jié)果表明，iNav-II型USV跟蹤復(fù)雜路徑的航跡誤差在±7 m以內(nèi)，在直線路徑和曲線路徑銜接處的航跡誤差較大(見圖17)。

5 結(jié)束語

針對iNav-II型USV的路徑跟蹤問題，設(shè)計并實現(xiàn)一種基于內(nèi)模控制和L1制導(dǎo)算法的USV路徑跟蹤控制器。通過系統(tǒng)辨識的方法確定基于內(nèi)?？刂频腢SV艏搖角速度控制器中USV操縱性指數(shù)的值。在開闊水域的實船試驗證明結(jié)合內(nèi)?？刂坪蚅1制導(dǎo)算法的路徑跟蹤控制器能較好地實現(xiàn)iNav-II型USV對直線路徑和曲線路徑的跟蹤。考慮到本文采用的一階線性操縱響應(yīng)模型不能較好地適用于USV機動航行、舵角比較大的情況，下一步將研究采用非線性模型來設(shè)計USV控制器，以提高控制器的適應(yīng)能力和精度。

[1] ELLENRIEDER K D V. Development of a USV-Based Bridge Inspection System[C]//Oceans. IEEE, 2016:1-10.

[2] IDRIS M H B M, SAHALAN M I, ABIDIN Z B Z, et al. Design and Development of an Autonomous Surface Vessel for Inland Water Depth Monitoring[C]//International Conference on Computer and Communication Engineering. IEEE, 2017:177-182.

[3] SIYANG S, KERDCHAROEN T. Development of Unmanned Surface Vehicle for Smart Water Quality Inspector[C]//International Conference on Electrical Engineering/ electronics, Computer, Telecommunications and Information Technology. IEEE, 2016:1-5.

[4] MATOS A, MARTINS A, DIAS A, et al. Multiple Robot Operations for Maritime Search and Rescue in EuRathlon 2015 Competition[C]//Oceans. IEEE, 2016:1-7.

[5] 董早鵬, 劉濤, 萬磊,等. 基于Takagi-Sugeno模糊神經(jīng)網(wǎng)絡(luò)的欠驅(qū)動無人艇直線航跡跟蹤控制[J]. 儀器儀表學(xué)報, 2015, 36(4):863-870.

[6] MU D D, WANG G F, FAN Y S. Design of Adaptive Neural Tracking Controller for Pod Propulsion Unmanned Vessel Subject to Unknown Dynamics[J]. Journal of Electrical Engineering & Technology, 2017, 12(6):2365-2377.

[7] SONNENBURG C R, Woolsey C A. Modeling, Identification, and Control of an Unmanned Surface Vehicle[J]. Journal of Field Robotics, 2013, 30(3):371-398.

[8] 燕聃聃, 文元橋, 肖長詩,等. 吊艙推進的小型水面無人船航跡控制系統(tǒng)設(shè)計[J]. 船海工程, 2017, 46(4):210-214.

[9] FAN Y, SUN Y, WANG G. On Model Parameter Identification and Trajectory Tracking Control for USV Based on Backstepping[C]//Control Conference, 2017 36th Chinese. IEEE, 2017:4757-4761.

[10] SHIN J, DONG J K, LEE Y I. Adaptive Path-Following Control for an Unmanned Surface Vessel Using an Identified Dynamic Model[J]. IEEE/ASME Transactions on Mechatronics, 2017, 22(3):1143-1153.

[11] FOSSEN T I, BREIVIK M, SKJETNE R. Line-Of-Sight Path Following of Underactuated Marine Craft[J]. IFAC Proceedings Volumes, 2003, 36(21): 211-216.

[12] CAHARIJA W, PETTERSEN K Y, BIBULI M, et al. Integral Line-Of-Sight Guidance and Control of Underactuated Marine Vehicles: Theory, Simulations and Experiments[J]. IEEE Transpactions on Control Systems Technology, 2014:147-148.

[13] 張國慶. 超惡劣海況下船舶運動簡捷魯棒自適應(yīng)控制[D].大連:大連海事大學(xué),2015.

[14] KARNEY C F F. Algorithms for Geodesics[J]. Journal of Geodesy, 2013, 87(1):43-55.

[15] ZHU M, HAHN A, WEN Y, et al. Parameter Identification of Ship Maneuvering Models Using Recursive Least Square Method Based on Support Vector Machines[J]. International Journal on Marine Navigation and Safety of Sea Transportation, 2017, 11(1):23-29.

[16] 張顯庫. 船舶運動簡捷魯棒控制[M]. 北京：科學(xué)出版社, 2012:17-23.

[17] 鄭恩讓, 阮士濤, 王黎. 基于失配模型的內(nèi)?？刂破鞯脑O(shè)計[J]. 控制工程, 2016, 23(4x):532-537.

[18] SAARI H, DJEMAI M. Ship Motion Control Using Multi-Controller Structure[J]. Ocean Engineering, 2012, 55(4):184-190.

[19] PARK S, DEYST J, HOW J. A New Nonlinear Guidance Logic for Trajectory Tracking[C]//Proceedings of the AIAA Guidance. Navigation and Control Conference, 2004:2004-4900.

[20] 劉樂柱, 肖長詩, 文元橋. 基于Dubins路徑的無人艇運動規(guī)劃算法[J]. 計算機應(yīng)用, 2017, 37(7):2114-2117.