基于擴張狀態(tài)觀測器的雙槳推進無人艇抗干擾目標(biāo)跟蹤控制

吳文濤，彭周華，王丹，劉陸，姜繼洲，任帥

大連海事大學(xué) 船舶電氣工程學(xué)院，遼寧 大連 116026

0 引 言

近年來，隨著無人駕駛技術(shù)的興起和發(fā)展，無人艇(unmanned surface vehicle, USV)作為一種小型化、智能化、多用途無人海洋運載平臺，獲得了學(xué)者們的廣泛關(guān)注。從運動場景來看，無人艇可用于軌跡跟蹤、路徑跟蹤和目標(biāo)跟蹤，而目標(biāo)跟蹤技術(shù)在軍事和民用領(lǐng)域具有重要的應(yīng)用價值[1]。

無人艇的運動控制面臨著非線性、模型不確定性、欠驅(qū)動和強外部擾動等研究難點，給無人艇有效可靠的目標(biāo)跟蹤控制帶來了挑戰(zhàn)。目前，研究人員已提出眾多方法用于無人艇控制，如滑?？刂芠2]、魯棒控制[3]、模糊控制[4]、參數(shù)自適應(yīng)控制[5]、神經(jīng)網(wǎng)絡(luò)控制[6]、擾動觀測器[7]、擴張狀態(tài)觀測器[8]等。傳統(tǒng)的無人艇運動是通過調(diào)節(jié)螺旋槳和舵機來實現(xiàn)姿態(tài)控制，該方法較適用于大型船舶。因舵機需要頻繁調(diào)整舵角來控制船舶的航向姿態(tài)，故難以滿足對靈敏性要求較高的小型無人艇。相較之下，雙槳推進無人艇由于是通過2 個螺旋槳推力相同或不同來進行速度或航向的控制，能在很大程度上提高無人艇的靈敏性和機動能力，可使無人艇更好地適應(yīng)工作場景的需要[9]。

本文將研究含模型不確定性與未知海洋環(huán)境擾動的雙槳推進欠驅(qū)動無人艇抗干擾目標(biāo)跟蹤控制問題。首先，建立雙槳推進欠驅(qū)動無人艇的運動數(shù)學(xué)模型，包括運動學(xué)方程和動力學(xué)方程。在運動學(xué)層級，提出基于平行接近制導(dǎo)(constant bearing, CB)的目標(biāo)跟蹤制導(dǎo)律；在動力學(xué)層級，設(shè)計基于擴張狀態(tài)觀測器(extended state observer,ESO)的縱蕩速度控制律和艏搖角速度控制律，以消除模型不確定性與未知海洋環(huán)境擾動問題，實現(xiàn)無人艇的抗干擾目標(biāo)跟蹤控制。最后，通過輸入狀態(tài)穩(wěn)定性定理和級聯(lián)定理，分析所提出的基于ESO 的縱蕩速度和艏搖角速度控制器的穩(wěn)定性，并用實驗證明采用CB 制導(dǎo)的抗干擾目標(biāo)跟蹤控制方法的有效性。

1 無人艇數(shù)學(xué)模型

在地球坐標(biāo)系XE-YE和艇體坐標(biāo)系XB-YB下，雙槳推進的欠驅(qū)動無人艇運動的數(shù)學(xué)模型[10]如圖1 所示。

圖1 無人艇平面運動示意圖Fig. 1 Schematic diagram of plane motion of USV

圖中：u,v,r分別為無人艇在艇體坐標(biāo)系XBYB下的縱蕩速度、橫蕩速度和艏搖角速度；x,y,ψ分別為無人艇在地球坐標(biāo)系下的XE坐標(biāo)、YE坐標(biāo)和偏航角；f1和f2分別為左、右螺旋槳產(chǎn)生的推力；B為左、右螺旋槳之間的橫向軸距。

無人艇的運動學(xué)模型可由一個三自由度非線性數(shù)學(xué)模型描述[11]：

式中： υ=[u,v,r]T，為無人艇的速度狀態(tài)向量；η=[x,y,ψ]T， 為無人艇的位置狀態(tài)向量；R(ψ)為無人艇在地球坐標(biāo)系和艇體坐標(biāo)系下的矩陣。

在動力學(xué)建模中，無人艇在水面航行時的水動力阻尼項通常采用線性形式，即無人艇所受阻力與速度成線性關(guān)系。因此，無人艇的動力學(xué)方程可由如下方程描述[11]：式中：M為無人艇的慣性質(zhì)量矩陣；C為向心力和科氏力系數(shù)矩陣；D為水動力阻尼矩陣； τ為無人艇的推力及其力矩向量； τd為推力和力矩擾動項向量。

式中：d11,d33為 阻尼系數(shù)且d11,d33∈D；m11,m33為慣性質(zhì)量常數(shù)且m11,m33∈M；k為直流電機輸入電壓與產(chǎn)生的推力關(guān)系參數(shù)； τd1∈τ，為縱蕩速度方向擾動分量， τd3∈τ，為艏搖角速度方向擾動分量。由式(3)可知，僅通過調(diào)節(jié)控制電壓 σu或 σr就可控制無人艇的動作狀態(tài)，實現(xiàn)其縱蕩速度和艏搖角速度的解耦控制，簡化了雙槳推進無人艇動力學(xué)控制器設(shè)計，也解決了運動控制過程中回轉(zhuǎn)運動和推進運動耦合的問題。

2 控制器設(shè)計

2.1 運動學(xué)制導(dǎo)律設(shè)計

CB 制導(dǎo)的基本原理是通過將視距旋轉(zhuǎn)率降低到0，從而使跟蹤艇以恒定的方位感知并跟蹤目標(biāo)。目標(biāo)跟蹤是將跟蹤艇與目標(biāo)的視距降低到一個期望值并保持。

定義某時刻跟蹤艇和目標(biāo)的位置矢量分別為p(t)=[x(t),y(t)]T和pt(t)=[xt(t),yt(t)]T，則跟蹤艇和目標(biāo)的速度矢量分別為? (t)=dp(t)/dt?p˙(t) 和?t(t)=p˙t(t)。 令p?(t)=pt(t)?p(t)，為目標(biāo)和跟蹤艇的視距矢量，則CB 制導(dǎo)的控制目標(biāo)可以表述為

CB 制導(dǎo)律的速度分解關(guān)系如圖2 所示。

圖2 CB 制導(dǎo)律速度分解圖Fig. 2 The velocity assignment associated with CB guidance

根據(jù)圖2，得[13]

式中：umax為跟蹤者沿視距方向的最大靠近速度，umax＞0 ； δp?為 防 碰 撞 常 數(shù)， δp?＞0。因 此，基于CB原理的運動學(xué)制導(dǎo)律如下：

式中：ur為 無人艇的期望速度； ψr為無人艇的期望航向且滿足 ψr∈(?π,π]。由于動力學(xué)無法直接對航向進行控制，設(shè)計如下虛擬控制律：

式中：rr為 無人艇的期望角速度； ψe=ψr?ψ，為航向跟蹤誤差；Kr為增益常數(shù)。

2.2 縱蕩速度控制器設(shè)計及穩(wěn)定性分析

在不考慮電機特性的情況下，雙槳推進無人艇縱蕩速度的響應(yīng)模型為

式中：fu為縱蕩速度方向的不確定項；bu為控制增益。為ESO 子系統(tǒng)的設(shè)計和穩(wěn)定性分析做如下假設(shè)。

受模型不確定性和未知環(huán)境擾動的影響，式(8)中的fu為未知項。為估計未知項，設(shè)計如下一階線性ESO：

式中， εu∈R， 是一個大于0 的常數(shù)；I1是一個三維對角單位矩陣。

根據(jù)式(8)，為抵消縱蕩速度方向擾動，設(shè)計如下縱蕩速度自抗擾控制律：

1） ESO 子系統(tǒng)的穩(wěn)定性分析。

ESO 子系統(tǒng)式(11)的穩(wěn)定性由引理1 給出。

引理1：在滿足假設(shè)1 的條件下，式(11)所示系統(tǒng) 狀 態(tài) 為E1， 系統(tǒng)輸入為f˙u的ESO 子 系 統(tǒng) 是 輸入狀態(tài)穩(wěn)定的。

證明：構(gòu)建如下李雅普諾夫方程[8]：

對方程(15)進行求導(dǎo)，得

由可得

2） 控制子系統(tǒng)的穩(wěn)定性分析。

控制子系統(tǒng)式(14)的穩(wěn)定性由引理2 給出。

證明：構(gòu)建如下李雅普諾夫方程：

對V2求導(dǎo)，得

將式(14)代入式(21)，可得

式中， 0 ＜θˉ2＜1。因此，控制子系統(tǒng)式(14)是輸入狀態(tài)穩(wěn)定的，且存在 KL 類函數(shù) α2和 K∞類 函數(shù) γ2使?jié)M足：

3） ESO 子系統(tǒng)與控制子系統(tǒng)級聯(lián)穩(wěn)定性分析。

將ESO 子系統(tǒng)式(11)與控制子系統(tǒng)式(14)看作一個級聯(lián)系統(tǒng)：

由 Σu1和 Σu2組成的基于ESO 的前縱蕩速度控制器級聯(lián)系統(tǒng)穩(wěn)定性由定理1 給出。

定理1：考慮無人艇縱蕩速度式(8)，在滿足假設(shè)1 的條件下，縱蕩速度ESO 式(9)和縱蕩速度控制律式(13)使 Σu1和 Σu2組成的級聯(lián)系統(tǒng)是輸入狀態(tài)穩(wěn)定的。

2.3 艏搖角速度控制器設(shè)計及穩(wěn)定性分析

在不考慮電機特性的情況下，雙槳推進無人艇艏搖角速度的響應(yīng)模型為：

式中：fr為 艏搖角速度方向的不確定項；br為控制增益。為ESO 子系統(tǒng)的設(shè)計和穩(wěn)定性分析做出如下假設(shè)。

受模型不確定性和未知環(huán)境擾動的影響，式(27)中的fr為未知項。為估計該未知項，設(shè)計如下一階線性ESO：

令E3=[r?,f?r]T，將式(29)改寫成矩陣形式：

式中， εr∈R，是一個大于0 的常數(shù)。

根據(jù)式(27)，為抵消艏搖角速度方向擾動，設(shè)計如下艏搖角速度自抗擾控制律：

至此，整個閉環(huán)系統(tǒng)控制器設(shè)計完畢，控制器結(jié)構(gòu)如圖3 所示。

圖3 控制器結(jié)構(gòu)Fig. 3 Controller structure

1） ESO 子系統(tǒng)的穩(wěn)定性分析。

ESO 子系統(tǒng)式(30)的穩(wěn)定性由引理3 給出。

引理3：在滿足假設(shè)2 的條件下，式(30)所示系統(tǒng) 狀 態(tài)為E3， 系統(tǒng) 輸 入 為f˙r的ESO 子系 統(tǒng) 是輸入狀態(tài)穩(wěn)定的。

證明：構(gòu)建如下李雅普諾夫方程：

對V3求導(dǎo)，聯(lián)立式(30)，可得

由

可得

2） 控制子系統(tǒng)的穩(wěn)定性分析。

控制子系統(tǒng)式(33)的穩(wěn)定性由引理4 給出。

證明：構(gòu)建如下李雅普諾夫函數(shù)：

對V4求導(dǎo)，聯(lián)立式(33)，可得

3） ESO 子系統(tǒng)與控制子系統(tǒng)級聯(lián)穩(wěn)定性分析。

將ESO 子系統(tǒng)式(30)與控制子系統(tǒng)式(33)看作一個級聯(lián)系統(tǒng)：

由 Σr1和 Σr2組成的基 于ESO 的艏搖 角速度控制器級聯(lián)系統(tǒng)穩(wěn)定性由定理2 給出。

定理2：考慮無人艇艏搖角速度式(27)，在滿足假設(shè)1 的條件下，艏搖角速度ESO 式(28)和艏搖角速度控制律 式(32)使 Σr1和 Σr2組成的級聯(lián)系統(tǒng)是輸入狀態(tài)穩(wěn)定的。

3 試驗結(jié)果

為了驗證本文所提的基于ESO 的平行接近制導(dǎo)抗干擾目標(biāo)跟蹤控制方法的有效性，采用圖4所示的2 艘CSICST-DH01 號無人艇進行試驗。該無人艇裝配有全球定位模塊、電子羅盤、運動處理單元、飛思卡爾單片機、無刷直流電機、電子調(diào)速器、無線通信模塊和電源等設(shè)備。

圖4 目標(biāo)跟蹤無人艇試驗平臺Fig. 4 Experimental platform for target tracking of USV

本次試驗的控制器參數(shù)為：umax=0.7 m/s，δp?=4，βu1=1， βu2=0.25 ，βr1=1， βr2=0.25，ξu=2，ξr=0.5，bu=1，br=1。為避免跟蹤艇和目標(biāo)艇發(fā)生碰撞，根據(jù)坐標(biāo)平移原理，使跟蹤艇跟蹤目標(biāo)艇附近的虛擬目標(biāo)點，虛擬目標(biāo)點在XE-YE中的位置如下[14]：

圖5～圖7 展示了縱蕩速度控制試驗的結(jié)果。圖5 表明，對于給定的期望速度信號，基于ESO的估計值和無人艇的實際速度均能在較短的時間內(nèi)穩(wěn)定到期望值。圖6 顯示，在縱蕩速度控制律式(13)的作用下，在穩(wěn)態(tài)時，無人艇的速度跟蹤誤差穩(wěn)定在0 左右。圖7 展示了無人艇在暫態(tài)和穩(wěn)態(tài)時的擾動變化情況，在穩(wěn)態(tài)時，擾動基本為恒定值，這與實際情況相符，說明了縱蕩速度ESO式(9)的有效性。

圖8～圖10 展示了艏搖角速度控制試驗的結(jié)果。圖8 表明，對于給定的期望速度信號，基于ESO 的估計角速度和實際艏搖角速度均能在較短時間內(nèi)穩(wěn)定到期望值。圖9 顯示，在艏搖角速度控制律式(32)的作用下，在穩(wěn)態(tài)時，無人艇的角速度跟蹤誤差穩(wěn)定在0°左右。圖10 顯示無人艇在穩(wěn)態(tài)時擾動基本為恒定值，與實際情況相符，說明了艏搖角速度ESO 式(28)的有效性。

圖5 USV 縱蕩速度Fig. 5 Surge velocity of the USV

圖6 縱蕩速度跟蹤誤差Fig. 6 Tracking error of the surge velocity

圖7 縱蕩速度ESO 的擾動估計Fig. 7 Estimated disturbance based on the surge velocity ESO

圖8 USV 艏搖角速度Fig. 8 Yaw angular velocity of the USV

圖9 艏搖角速度跟蹤誤差Fig. 9 Tracking error of the yaw angular velocity

圖10 艏搖角速度ESO 的擾動估計Fig. 10 Estimated disturbance based on the yaw angular velocity ESO

圖11～圖14 展示了艏搖角速度控制試驗的結(jié)果。圖11 顯示了跟蹤艇（紅線）和目標(biāo)艇（藍線）的實際軌跡信息，從中可以看出，跟蹤艇已穩(wěn)定至式(45)設(shè)定的虛擬目標(biāo)點位置。圖12 展示了跟蹤艇與虛擬跟蹤點的跟蹤誤差，從中可以發(fā)現(xiàn)，在所提CB 制導(dǎo)律式(6)的作用下，無人艇的位置逐漸收斂至虛擬目標(biāo)點附近，跟蹤誤差穩(wěn)定在0 m 左右。由圖13 和14 可知，隨著跟蹤誤差的減小，跟蹤艇與虛擬目標(biāo)點的航向和速度漸趨一致。本次試驗湖泊中存在有水草并隨機分布于水中。在75 s 時經(jīng)過水草密集區(qū)，由于跟蹤艇推進器被水草纏繞，導(dǎo)致誤差增大，由制導(dǎo)律式(6)和控制律式(13)可知，縱蕩速度開始增加，縱蕩速度控制輸入 σu也增加，螺旋槳轉(zhuǎn)速增加，擺脫水草后繼續(xù)追趕目標(biāo)，使跟蹤狀態(tài)重新回到穩(wěn)態(tài)。

圖11 無人艇的目標(biāo)跟蹤軌跡Fig. 11 Target tracking trajectory of the USV

圖12 基于CB 制導(dǎo)的跟蹤誤差Fig. 12 Tracking error based the CB guidance law

圖13 跟蹤艇和目標(biāo)艇的速度Fig. 13 Actual speed of the target ship and follower ship

圖14 跟蹤艇和目標(biāo)艇的航向Fig. 14 Actual yaw of the target ship and follower ship

4 結(jié) 語

本文針對雙槳推進無人艇的目標(biāo)跟蹤問題，提出了基于ESO 的CB 制導(dǎo)抗干擾目標(biāo)跟蹤控制方法。首先，在XE-YE和XB-YB坐標(biāo)系下建立了雙槳推進無人艇運動模型。接著，在運動學(xué)層級提出了基于CB 制導(dǎo)的目標(biāo)跟蹤控制律，在動力學(xué)層級提出了基于ESO 的縱蕩速度控制律和艏搖角速度控制律，克服了無人艇模型的不確定性和由未知環(huán)境擾動帶來的問題。通過輸入狀態(tài)穩(wěn)定性定理和級聯(lián)定理，證明了所提控制器的穩(wěn)定性。最后，通過試驗證明了所提抗干擾目標(biāo)跟蹤控制方法的有效性。