復雜擾動下水下機器人的軌跡精確跟蹤控制

陳浩華，趙紅，王寧*,2，郭晨，魯挺，王寧

1 大連海事大學 船舶電氣工程學院，遼寧 大連 116026

2 哈爾濱工程大學 船舶工程學院，黑龍江 哈爾濱 150001

0 引 言

水下機器人是一種可以按照預設指令或者操作者的意愿完成某項任務的智能水下航行器。其廣泛用于民用和軍事領域，如海上油氣的勘探和開發，海洋環境的觀測，魚雷的排查等[1-3]。水下機器人的高精度軌跡跟蹤控制是完成上述任務的前提，因而受到學者們的廣泛關注[4]。然而，水下機器人是一個多自由度耦合、多變量的非線性系統，由于復雜的水下環境對其產生的干擾影響，如海浪、洋流和海洋生物的碰撞等，使得水下機器人的軌跡跟蹤控制器的設計成為一個難點。控制器應該具有處理非線性和抗干擾的能力[5]。

滑模控制對系統中未建模部分不敏感，具備一定的抗干擾能力，因此被廣泛用于水下機器人軌跡跟蹤控制領域[6]。針對水下機器人的三維軌跡跟蹤控制的問題，Qiao 等[7]提出了一種雙閉環無抖振自適應滑模控制方法，解決了UUV 的三維目標軌跡跟蹤控制問題。其設計了外環位置控制器和內環速度控制器，用飽和函數代替符號函數來克服外環的抖振。此外，設計了一個連續自適應項來代替傳統滑模的不連續切換函數，并利用Lyapunov 穩定性理論證明了雙環控制系統的全局漸近穩定。Rezazadegan 等[8]針對水下機器人三維軌跡跟蹤控制的問題，提出了一種基于Lyapunov 直接法的自適應反步控制器。結果表明實際軌跡可以漸進收斂于期望軌跡，但是不能實現有限時間穩定。Guerrero 等[9]提出了一種自適應高階滑模控制方法，在保留魯棒控制的優點下，忽略擾動上界對系統的影響，并且將該算法應用到Leonard ROV 深度軌跡跟蹤控制和偏航控制中，實驗結果表明該控制策略可以有效抑制外界擾動和水動力學參數的不確定性。Qiao 等[10]針對時變外界干擾對水下機器人三維軌跡跟蹤控制問題，提出了一種自適應非奇異積分終端滑模控制策略。消除傳統滑模控制中的奇異問題，保證速度跟蹤誤差在有限時間內局部收斂到零，位置跟蹤誤差局部指數收斂到零。孫巧梅等[11]采用了反步滑模的控制策略，在外界干擾下，該控制策略得到了連續且平滑的控制輸入，并且有效抑制了外界擾動。魏斯行等[12]在反步滑模控制的基礎上，引入了一種生物啟發神經動力學模型來解決平滑輸出速度跳變的問題。仿真實驗表明，對于纜控式水下機器人能夠有效解決反步滑模造成的速度跳變。張偉等[13]引入了徑向基函數神經網絡和反步的方法來解決控制器運動參數在拐點處跳躍引起的跟蹤沖擊問題。同時，避免了單次反步控制中的奇異值問題。利用Lyapunov 穩定性理論分析了整個閉環控制系統的穩定性，但并不能實現對期望軌跡的精確跟蹤。嚴浙平等[14]提出了一種雙閉環終端滑模控制策略，將位置和姿態負反饋作為外環控制，并引入虛擬速度作為內環控制器的期望目標，采用終端滑模控制方法降低了抖振，使跟蹤誤差在有限時間內收斂，仿真實驗表明該控制策略可以實現對空間軌跡的精確跟蹤。但是設計過程較為復雜，計算量較大，不能廣泛應用于水下機器人的軌跡跟蹤控制。面對復雜的外界擾動，為進一步提高跟蹤精度，Wang等[15-16]嘗試使用觀測器技術提高無人船的跟蹤精度，及時補償復雜的未知擾動，對姿態實現精準調節。

本文將針對水下機器人三維軌跡精確跟蹤控制的問題，采用有限時間擾動觀測器對外界復雜擾動進行觀測，并結合非奇異終端滑模控制設計基于有限時間擾動觀測器的非奇異終端滑模控制器（FDO-NTSMC）。其中所設計的FDO 可以精確觀測外界時變干擾，降低擾動對系統的影響，提高系統的抗干擾能力，同時采用冪次趨近律降低NTSMC 產生的抖振。此外，將利用Lyapunov 函數及相關引理證明基于本文策略的水下機器人軌跡跟蹤系統可以在有限時間內收斂。最后，通過仿真實驗證明，在水下存在時變擾動及階躍疊加擾動等復雜環境擾動時，本文FDO-NTSMC 策略不僅可以實現對水下機器人期望軌跡的精確跟蹤，且比反步滑模控制策略（SMC）具有更好的控制性能。

1 預備知識和數學模型

1.1 預備知識

定理1[17]：考慮非線性系統

式 中：x=[x1,···,xn]T為 系 統n維 的 狀 態 向 量；f(·)為 原點鄰域上的非線性系統，且f(0) =0。若存在函數V(x,t)滿足：

1）V(x,t)正 定，V˙(x,t)負定，則系統在原點處為漸進穩定；

2）V(x,t)正 定，V˙(x,t)半 負 定，除 原 點 之 外，V˙(x,t)不恒為零，則系統在原點處是漸進穩定；

3）V(x,t)正 定，V˙(x,t)半 負定，且當 ‖x‖→∞時，V(x,t)→∞，則系統在原點處是大范圍漸進穩定。

引理1[18]：定義一正標量函數V(x)，若滿足

式中， λ ＞0， 0 ＜θ ＜1。則系統式(1) 有限時間穩定，且其有限時間T滿足如下不等式

式中，V(x(t0)) 為V(x(t))的初始值。

引理2[19]：考慮以下系統

式 中： βi＞0(i=0,1,···,n)，L＞0，為 常 數；n為 系統階數。如果系統滿足上述條件，則其有限時間穩定。

1.2 數學模型

水下機器人的運動狀態需要依靠坐標系進行描述，如圖1 所示，分別以大地為坐標原點建立慣性坐標系E-ξηζ，以水下機器人的重心為坐標原點建立附體坐標系O-xoyozo。

圖1 坐標系Fig. 1 Coordinate system

根據Fossen 等[20]提出的潛水器動力學建模方法得到水下機器人六自由度數學模型。由于水下機器人在運動的過程中其橫滾角變化較小，因此可以忽略其對水下機器人的影響，得到水下機器人五自由度數學模型。

式中：慣性坐標系下， η=[x,y,z,θ,ψ]T，x為水下機器人縱向位移，y為橫向位移，z為垂向位移， θ為俯仰角， ψ為偏航角；附體坐標系下， υ=[u,v,w,q,r]T，u為縱向速度，v為橫向速度，w為垂向速度，q為俯仰 角 速 度，r為 偏 航 角 速 度； τ=[τ1,τ2,τ3,τ4,τ5]T，τ1為 縱向控制力， τ2為 橫向控制力， τ3為垂向控制力，τ4為俯仰角控制力矩，τ5為偏航角控制力矩；τδ=MJ-1(η)δ(t)為在對應方向上受到的外界干擾，其中，δ(t)=[δ1,δ2,δ3,δ4,δ5]T；M為質量與附加質量矩陣，且有M=MT＞0；C(υ)為科氏向心力矩陣，且有C(υ)=-CT(υ)；D(υ)為阻尼矩陣；g(η)為恢復力矩陣；J(η)為慣性坐標系與附體坐標系轉換矩陣，具體描述如下：

式中：m為水下機器人的質量；Iy，Iz為轉動慣量；Xu˙，Yv˙，Zw˙，Mq˙，Nr˙分 別 為 橫 向、縱 向、垂 向、俯 仰角和航向角5 個自由度的水動力導數；Xu，Yv，Zw，Mq，Nr分別為橫向、縱向、垂向、俯仰角和航向角5 個 自 由 度 的 一 階 阻 尼 系 數；Xu|u|，Yv|v|，Zw|w|，Mq|q|，Nr|r|分別為橫向、縱向、垂向、俯仰角和航向角5 個自由度的二階阻尼系數；W和B分別為水下機器人所受到的重力和浮力；zB為附體坐標系下浮心在z軸坐標，即浮心高度。

控制目標：依據水下機器人的數學模型式(5)建立期望軌跡下的數學模型，構造跟蹤誤差方程，并根據此方程設計控制器 τ。當存在外界多種復雜時變擾動時，基于該控制策略下的水下機器人軌跡跟蹤系統不僅可以在有限時間內實現對期望軌跡的精確跟蹤，而且可以得到一條較為光滑的控制輸入曲線。

2 FDO-NTSMC 控制器設計

2.1 問題描述

期望軌跡下的數學模型，描述如下：

式中：ηd=[xd,yd,zd,θd,ψd]T為期望位置和航向角；

υd=[ud,vd,wd,qd,rd]T為期望速度和角速度；τd=[τd1,τd2,τd3,τd4,τd5]T為期望軌跡對應的控制輸入。

為了便于說明控制器的設計過程，定義以下狀態變量：

式中， ω =[ω1,ω2,ω3,ω4,ω5]T為實際期望位置和航向角， ωd=[ωd1,ωd2,ωd3,ωd4,ωd5]T為實際速度和角速度，其中 ω1～ω5分別為水下機器人在慣性坐標系下的縱向、橫向、垂向速度與俯仰角及航向角角速度的實際值， ωd1～ ωd5為上述5 個變量對應的期望值。

結合狀態變量方程，將式(5)改寫為

其中，

相同的，式(11)可改寫為

其中，

結合式(13) 和式(14)，得到系統跟蹤誤差方程：

其中：

2.2 控制器設計

在外界存在多維度干擾的情況下，為使水下機器人實現對三維期望軌跡的精確跟蹤，基于跟蹤誤差方程(15)設計了FDO-NTSMC 控制器。

第1 步：針對外界多維度時變擾動，設計有限時間擾動觀測器進行補償。

假設1：式(15)中外界擾動 δ滿足

式中，Lδ＞0有界且為常數。

針對跟蹤誤差系統方程中的擾動，有限時間擾動觀測器設計如下：

其中，

式中：z0為 速度誤差的估計值，z1為外界擾動的觀測值，z2為外界擾動一階導數的估計值，其中zi∈R5x1(i=0,1,2) ；βi＞0(i=0,1,2)為 增 益 系 數，L=diag(l1,l2,l3,l4,l5)為有限時間擾動觀測器參數；sigθ(x)=|x|θsign(x)。

第2 步：設計非奇異終端滑模面s：

式 中：k＞0， 為 常 數；p，q為 正 奇 數， 且 有q/p∈(1,2)。

對式(19)求導，并結合式(15)，得

式中，diag(·)表示對角矩陣。為保證求導后維度正確，將ω改寫為diag(ω。

根據設計的滑模面式(20)，控制器可設計為

其中，等效控制項為

魯棒控制項為

式中：選取冪次趨近律s˙=κ|s|αsign(s)，可以保證在有限時間內到達滑動面，其中，κ=diag(κ1,κ2,κ3,κ4,κ5)，κi＞0(i=1,2,3,4,5) 為 常 對 角 矩 陣； α為 常數，且0＜α＜1； sign(s)=[sign(s1),sign(s2),sign(s3),sign(s4),sign(s5)]T，sign(·)表示符號函數，并且有以下性質：

2.3 系統穩定性分析

定理2：針對假設1 下的多維度時變復雜擾動，設計的有限時間擾動觀測器可以實現對擾動的觀測，補償擾動對系統的影響，提高魯棒性。

證明：

對于所設計的有限時間擾動觀測器式(17)，定義觀測誤差方程，即

對上式等號兩端求導， 結合式(17)和式(18)，得

式(26)還可以寫成

由引理2 可知，式(27)有限時間穩定，即所設計的有限時間擾動觀測器可以在有限時間內對擾動進行觀測。并且在有限時間內有

因此可以得到觀測誤差z1-δ ≡0。證畢。

定理3：考慮外界多維度時變干擾 δ影響下的水下機器人數學模型，在控制律 τ的作用下，系統可以在有限時間內驅動狀態變量到達滑動面s(t)=0，使位姿跟蹤誤差變量在有限時間內收斂于零，即實際位姿向量 η和速度向量 υ跟蹤上參考位姿向量 ηd和 期望速度向量 υd。

證明：

將式(21)代入式(20)，整理后得

結合定理1，上式可以寫為

定義Lyapunov 函數：

對其求導，并將式(30)代入，得

令

當ωei≠0(i=1,2,3,4,5)時 ， 由 于q/p-1 ＞0，k＞0， κ ＞0 且q，p為正奇數，可以得到Q為正定矩陣，即 λmin(Q)≥0。那么，式(32)可以改寫為：

根據定理1，可知該系統漸進穩定。下面進一步證明系統有限時間穩定。令

結合式(31)與式(34)～式(35)，整理后得

由于0 ＜α ＜1， 得1 /2 ＜(α+1)/2 ＜1，根據引理1可知，系統有限時間收斂。

當ωei≠0(i=1,2,3,4,5)時，根據式(21)～式（23)和式(15)，有

由上式可得，當si＞0 時， ω˙ei＜0 ， ωei快速減小；當si＜0 時， ω˙ei＞0 ， ωei快 速 增大。因此，當ωei=0時，在有限時間內有s(t)=0。 即跟蹤誤差 ηe和速度跟蹤誤差 ωe在有限時間內到達滑模面[21]。

根據上述證明可知，本文所設計的FDONTSMC 可以使水下機器人在有限時間實現三維軌跡的精確跟蹤，系統跟蹤誤差可以在有限時間內被鎮定到零。

3 仿真實驗研究

為了證明本文所設計控制器的優越性和有效性，借鑒文獻[22]中水下機器人的動力學模型和水動力參數，在相同的初始條件下，與反步滑模BSMC 和非奇異終端滑模NTSMC 進行對比仿真實驗。同時，為驗證基于本文方法的暫態和穩態精度，采用絕對誤差積分準則（integral absolute error，IAE）和時間乘絕對誤差積分準則（integral time absolute error，ITAE）指標進行衡量[23-24]。表達式為：

式中，e(·)表示位姿跟蹤誤差，采用絕對誤差積分準則的計算結果為fIAE，采用時間乘絕對誤差積分準則的計算結果為fITAE。

其中，產生期望軌跡的控制輸入為：τd=[50,30cos(0.1πt)2,20cos(0.1πt)2,-12cos(0.1πt)2，12cos(0.1πt)2]T。 選取水下機器人的初始位姿η(0)=[15,7.2,2.7,0,0]T，初始速度及角速度υ (0)=[0,0,0,0,0]T。假設外界多維度時變干擾為：δ=[0.3cos(0.2πt-π/3),0.4cos(0.4πt-π/4) ，0.6cos(0.6πt-π/6), 0.2cos(0.2πt-π/2), 0.3cos(0.4πt-π/3)]T，階躍擾動為：δ2=[5,5,5,5,5]T。FDO-NTSMC 控制器關鍵參數取q=7，p=5，k=2 ， α=0.7， κ=diag(3.6,3.6,3.6,3.6,3.6)； 有 限 時 間 擾 動 觀 測 器 參 數 取 β1=2.2，β2=2.6， β3=0.8，L=diag(30,30,30,30,30)。

1） 仿真實驗工況1：水下存在如上擾動 δ時，基于本文算法與NTSMC 和BSMC 算法分別進行了水下機器人三維軌跡跟蹤的仿真實驗，仿真結果如圖2～圖8 所示。

由圖2～圖4 可以看出，水下存在擾動 δ時，雖然NTSMC 和BSMC 控制策略可令水下機器人完成對期望軌跡的跟蹤，但卻無法實現精確跟蹤，即存在波動的跟蹤誤差，而FDO-NTSMC 控制策略可令軌跡跟蹤誤差為零。圖5 所示的控制輸入曲線表明，與BSMC 策略相比，FDO-NTSMC 具有更為平滑的控制輸入，更利于執行器工作，且其中的有限時間擾動觀測器可以對外界復雜擾動實現精確觀測（圖8），故可以及時補償外界擾動對系統的干擾，提高系統的抗干擾能力。由圖6～圖7所示的速度與速度誤差曲線可見，本文設計的控制器相較于NTSMC 更有優勢。

圖2 工況1 的跟蹤軌跡Fig. 2 The tracking trajectory for condition 1

圖3 工況1 的位姿狀態曲線Fig. 3 Position states for condition 1

圖4 工況1 的位姿跟蹤誤差曲線Fig. 4 Position errors for condition 1

圖5 工況1 的控制輸入曲線Fig. 5 Control input for condition 1

圖6 工況1 的速度狀態曲線Fig. 6 Speed states for condition 1

圖7 工況1 的速度跟蹤誤差曲線Fig. 7 Velocity errors for condition 1

圖8 工況1 的水下擾動及其觀測曲線Fig. 8 Disturbance and observed values for condition 1

2） 仿真實驗工況2：水下環境中除了存在如上擾動δ，于30～40 s 處再疊加一階躍擾動。在相同的初始條件下，得到FDO-NTSMC，NTSMC 和BSMC 仿真實驗結果，如圖9～圖15 所示。

圖9 工況2 的跟蹤軌跡Fig. 9 The tracking trajectory for condition 2

圖10 工況2 的位姿狀態曲線Fig. 10 Position states for condition 2

通過圖9 可以看出，30～40 s 處疊加階躍擾動后，NTSMC 與BSMC 控制策略均無法實現對參考軌跡的跟蹤，而本文提出的FDO-NTSMC 仍能實現對參考軌跡的精確跟蹤，這一點通過圖11 位姿跟蹤誤差結果也得到了驗證。通過圖15 可以看出，在疊加階躍擾動后，本文所設計的有限時間擾動觀測器依然可以對疊加后的擾動實現精確的觀測，以及時補償階躍擾動對系統的影響，故提高了軌跡跟蹤系統的魯棒性。

圖11 工況2 的位姿跟蹤誤差曲線Fig. 11 Position errors for condition 2

圖12 工況2 的控制輸入曲線Fig. 12 Control input for condition 2

圖13 工況2 的速度狀態曲線Fig. 13 Speed states for condition 2

圖14 工況2 的速度跟蹤誤差曲線Fig. 14 Velocity errors for condition 2

圖15 工況2 的水下擾動及其觀測曲線Fig. 15 Disturbance and observed values for condition 2

3） 仿真實驗工況3：在仿真實驗工況1 下控制輸入曲線的結果中，由于初始時刻水下機器人軌跡跟蹤誤差較大且存在多維度時變干擾，故而需要較大的動力，導致控制輸入曲線存在較大的階躍突變。但這在實際的工程應用中不存在，執行器需要在安全范圍內工作，因此對3 種方法下的控制輸入進行合理限幅。得到以下實驗結果，如圖16～圖22 所示。

圖16 工況3 的跟蹤軌跡Fig. 16 The tracking trajectory for condition 3

圖17 工況3 的位姿狀態曲線Fig. 17 Position states for condition 3

圖18 工況3 的位姿跟蹤誤差曲線Fig. 18 Position errors for condition 3

圖19 工況3 的控制輸入曲線Fig. 19 Control input for condition 3

圖20 工況3 的速度狀態曲線Fig. 20 Speed states for condition 3

圖21 工況3 的速度跟蹤誤差曲線Fig. 21 Velocity errors for condition 3

圖22 工況3 的水下擾動及其觀測曲線Fig. 22 Disturbance and observed values for condition 3

通過圖19 可以看出在控制器限幅后，BSMC的控制輸入曲線出現了較強的抖振，而本文所設計的控制方法與限幅前相同，為較為光滑的控制輸入曲線。以上為基于3 種控制方法的水下機器人三維軌跡跟蹤控制研究的定性分析。為了更加直觀地分析本文控制方法的優勢。以下采用絕對誤差積分準則IAE 和時間乘絕對誤差積分準則ITAE 指標對上述3 種工況的控制性能進行暫態和穩態精度的定量分析。數據結果如表1～表3所示。

表1 三種方法的性能比較Table 1 Performance comparison of three methods

表2 階躍擾動下3 種方法性能比較Table 2 Performance comparison of three methods under step perturbation

表3 執行器限幅下3 種方法性能比較Table 3 Performance comparison of three methods under the condition of limiting amplitude of control input

通過對比3 種工況下的IAE 指標，BSMC 與FDO-NTSMC 均小于NTSMC，且本文提出的基于FDO-NTSMC 的控制方法略小于BSMC，具有較高的暫態精度。對比3 種工況下的ITAE 指標，結果表明FDO-NTSMC 的ITAE 性能指標遠小于BSMC 和NTSMC，故而基于FDO-NTSMC 的控制方法可令水下機器人對期望三維參考軌跡實現高精度跟蹤，即通過量化的方式進一步驗證了本文控制策略的優越性，基于FDO-NTSMC 的控制方法具有較高的穩態精度[25]。

4 結 論

本文針對復雜多維度時變擾動下水下機器人三維軌跡精確跟蹤問題，提出了基于有限時間擾動觀測器的非奇異終端滑模FDO-NTSMC 控制策略。通過設計有限時間擾動觀測器解決水下復雜時變擾動對水下機器人軌跡跟蹤系統的干擾問題，并完成了理論驗證。同時，為了降低非奇異終端滑模NTSMC 所產生的抖振，采用冪次趨近律代替以往文獻[26]中使用的等速趨近律，保證了跟蹤誤差可以在有限時間內被鎮定到零。最后，在相同的仿真實驗條件下，與非奇異終端滑模NTSMC 和反步滑模BSMC 控制策略進行了對比實驗研究，并使用IAE 和ITAE 性能指標對跟蹤精度進行量化，仿真結果驗證了本文所提策略的有效性與優越性。