小型無人艇無模型自適應(yīng)路徑跟蹤控制*

肖長詩 周 杰 陶 威 朱 曼 文元橋2,3,

(武漢理工大學(xué)航運(yùn)學(xué)院1) 武漢 430063) (內(nèi)河航運(yùn)技術(shù)湖北省重點(diǎn)實(shí)驗(yàn)室2) 武漢 430063) (國家水運(yùn)安全工程技術(shù)研究中心3) 武漢 430063) (武漢理工大學(xué)智能交通系統(tǒng)研究中心4) 武漢 430063)

0 引 言

無人艇(USV)路徑跟蹤控制通常分為兩種結(jié)構(gòu)：①分離控制方案，將控制器分為外環(huán)制導(dǎo)和內(nèi)環(huán)控制；②綜合控制方案，即將制導(dǎo)與控制綜合在一起[1-5].其中分離式控制方案優(yōu)點(diǎn)在于航跡和航向保持功能的相對分離，便于控制模式切換及軟硬件設(shè)計(jì)過程中地模塊化設(shè)計(jì)[6].

無人艇路徑跟蹤控制分離控制方案中，外環(huán)制導(dǎo)律主要有Line-Of-Sight(LOS)制導(dǎo)算法[7]、針對風(fēng)浪流干擾的ILOS制導(dǎo)算法[8]以及從短程戰(zhàn)術(shù)導(dǎo)彈末端制導(dǎo)律中的比例導(dǎo)引法的基礎(chǔ)上演變而來的L1制導(dǎo)算法[9]等.然而常規(guī)的LOS和ILOS制導(dǎo)算法通常適用于對直線路徑的跟蹤[10].L1制導(dǎo)算法雖然能夠通過控制無人艇艏搖角速度實(shí)現(xiàn)對曲線路徑的跟蹤，但是角速度數(shù)據(jù)容易受到風(fēng)浪干擾而導(dǎo)致控制效果不佳.在實(shí)際應(yīng)用中，無人艇由于航行及任務(wù)需要，應(yīng)該魯棒性強(qiáng)、且能對直線路徑和曲線路徑的跟蹤能力.

在內(nèi)環(huán)控制中，通常為航向控制與航速控制，其中航向航速控制方法主要有內(nèi)?？刂芠11]、模型參考自適應(yīng)控制[12]、滑?？刂芠13]、動(dòng)態(tài)面控制[14]等基于動(dòng)力學(xué)模型的控制方法，以及模糊神經(jīng)網(wǎng)絡(luò)PID[15]、改進(jìn)細(xì)菌覓食算法等基于數(shù)據(jù)的控制方法.由于無人艇是一類復(fù)雜的控制系統(tǒng)，在不同的工況及風(fēng)浪流干擾下，無人艇吃水、浸濕面積等參數(shù)變化很大，艇體的運(yùn)動(dòng)數(shù)學(xué)模型參數(shù)也隨之變化，使得無人艇變現(xiàn)出很強(qiáng)的非線性和不確定性，大部分基于動(dòng)力學(xué)模型的控制方法由于模型不夠精確而表現(xiàn)不佳、自適應(yīng)性較差.對于小型無人艇，雖然其準(zhǔn)確的動(dòng)力學(xué)模型難以構(gòu)建，但輸入輸出數(shù)據(jù)卻比較容易獲得.因此,本文將采用數(shù)據(jù)驅(qū)動(dòng)控制方法實(shí)現(xiàn)無人艇的航向與航速控制.

數(shù)據(jù)驅(qū)動(dòng)控制是指控制器設(shè)計(jì)不顯含受控過程的數(shù)學(xué)模型信息，僅利用受控系統(tǒng)的在線或離線I/O數(shù)據(jù)以及經(jīng)過數(shù)據(jù)處理而得到的知識來設(shè)計(jì)控制器，并在一定假設(shè)下有收斂性、穩(wěn)定性保障和魯棒性結(jié)論的控制理論與方法.目前常用數(shù)據(jù)驅(qū)動(dòng)的方法有PID控制、無模型自適應(yīng)控制(model free adaptive control，MFAC)、迭代反饋整定控制等.其中無模型自適應(yīng)控制因具有完備的理論分析而受到了很多學(xué)者的關(guān)注.文獻(xiàn)[16]針對四旋翼飛行器的非線性、易受干擾等特點(diǎn),設(shè)計(jì)一種比例-無模型自適應(yīng)串級姿態(tài)控制器，利用Matlab驗(yàn)證了該控制器對于四旋翼姿態(tài)控制的穩(wěn)定性和抗干擾性.文獻(xiàn)[17]設(shè)計(jì)的基于非圓車削的無模型自適應(yīng)迭代學(xué)習(xí)控制方案具有使直線電機(jī)具備自我學(xué)習(xí),改進(jìn)其位置誤差的能力.

文中以武漢理工大學(xué)iNav-IV型無人艇為研究對象，基于緊格式動(dòng)態(tài)線性化的無模型自適應(yīng)控制方法(CFDL-MFAC)，探討小型雙推進(jìn)無人艇的航向航速控制及路徑跟蹤問題.首先分析了無人艇動(dòng)力學(xué)特性，證明了動(dòng)態(tài)線性化模型與傳統(tǒng)動(dòng)力學(xué)模型等價(jià)并采用無模型自適應(yīng)控制理論，設(shè)計(jì)了無人艇航向航速協(xié)同控制器.設(shè)計(jì)了一種改進(jìn)LOS制導(dǎo)算法，通過對路徑參數(shù)化實(shí)現(xiàn)了無人艇對直線及曲線路徑的跟蹤，最后通過實(shí)船實(shí)驗(yàn)驗(yàn)證了路徑跟蹤控制器的性能.

1 無人艇動(dòng)力學(xué)模型及動(dòng)態(tài)線性化

只考慮無人艇平面運(yùn)動(dòng)，其動(dòng)力學(xué)模型可簡化為

(1)

式中：M為質(zhì)量矩陣；C為科氏力矩陣；D為阻力矩陣；τ=[Tx,Ty,Tz]為控制輸入；τw為非線性外界擾動(dòng)；為η=[u,v,r]T無人艇的速度向量.

定義無人艇姿態(tài)跟蹤誤差向量e，形式為

e=ηd-η

(2)

式中：ηd為期望姿態(tài)向量.對式(2)求一階導(dǎo)后，代入式(1)中，得到姿態(tài)誤差開環(huán)動(dòng)態(tài)方程：

M-1(C+D)ηd-M-1τw

(3)

(4)

為方便計(jì)算，定義輔助函數(shù)F(k)為如下形式.

(5)

(6)

對于式(6)所示的系統(tǒng)，滿足下面的兩個(gè)條件：

1) 除有限時(shí)刻點(diǎn)外，y(k+1)關(guān)于y(k)，τ(k)的偏導(dǎo)數(shù)連續(xù).

2) 除有限時(shí)刻點(diǎn)外，系統(tǒng)滿足廣義Lipschitz條件，即對任意k1≠k2，k1,k2>0和τ1≠τ2有：|y(k1+1)-y(k2+1)|≤b|τ(k1)-τ(k2)|.

為方便后續(xù)無人艇相關(guān)控制器的設(shè)計(jì)，記Δy(k+1)=y(k+1)-y(k)為相鄰兩時(shí)刻的輸出變化，Δτ(k)=τ(k)-τ(k-1)為相鄰兩時(shí)刻的輸入變化.并提出如下定理：

定理1對滿足條件1)和條件2)的非線性系統(tǒng)，當(dāng)‖ΔH(k)‖=‖[Δy(k),Δτ(k)]T‖≠0時(shí)，一定存在一個(gè)時(shí)變參數(shù)矩陣φ(k)，使系統(tǒng)可以轉(zhuǎn)化為如下的數(shù)據(jù)模型.

Δy(k+1)=φ(k)ΔH(k)

(7)

證：

Δy(k+1)=y(k+1)-y(k)=

(8)

為方便計(jì)算，令

(9)

由條件1)和Cauchy微分中值定理：

Δy(k+1)=

(10)

對于每一個(gè)固定的時(shí)刻k，考慮以下含有φ(k)的方程：

ψ(k)=φ(k)ΔH(k)

(11)

對于每一個(gè)‖ΔH(k)‖≠0，式(11)至少存在一個(gè)解φ*(k).

定理得證.

2 無人艇路徑跟蹤控制器設(shè)計(jì)

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

圖1為無人艇路徑跟蹤控制器采用分離式控制方案，即外環(huán)制導(dǎo)和內(nèi)環(huán)控制的結(jié)構(gòu).制導(dǎo)環(huán)節(jié)根據(jù)與當(dāng)前航線的距離誤差計(jì)算出期望艏向角ψref并作為控制環(huán)節(jié)的輸入，控制環(huán)節(jié)根據(jù)期望的艏向角計(jì)算出期望的舵角，對于本文所使用的雙推進(jìn)無人艇，由于采用差速轉(zhuǎn)向，輸出為兩推進(jìn)器的油門Tl、Tr.

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

2.2 改進(jìn)LOS制導(dǎo)律

圖2 改進(jìn)LOS制導(dǎo)律

(12)

當(dāng)J(θ)取最小值的時(shí)候，e(θ)最小，利用梯度下降法，定義θ的迭代公式為

θ(k+1)=θ(k)-ηJ(θ)

(13)

式中：η為步長，控制θ朝梯度方向下降的速率.實(shí)驗(yàn)表明，該方法收斂到路徑上的局部最近點(diǎn)，從計(jì)算的角度看，收斂速度足夠快，可以在線執(zhí)行.

(14)

3 航向航速無模型自適應(yīng)控制器設(shè)計(jì)

航向航速控制器結(jié)構(gòu)見圖3，由參數(shù)矩陣φ(k)實(shí)時(shí)估計(jì)算法及航向航速控制算法組成.

圖3 航向航速CFDL-MFAC控制器結(jié)構(gòu)

考慮如下控制輸入準(zhǔn)則函數(shù)：

J(τ(k))=‖y*(k+1)-y(k+1)‖2+

λ‖τ(k)-τ(k-1)‖2

(15)

式中:λ為一個(gè)大于0的權(quán)重因子，用于懲罰控制如入量變化過大；y*(k+1)為期望輸出.將式(10)代入準(zhǔn)則函數(shù)(15)中，對其求關(guān)于τ(k)的偏導(dǎo)，并令其等于0，得到航向航速控制律如下：

τ(k-1)

(16)

式中：ρ∈(0,1]為步長因子，加入它的目的是為了使算法更具一般性.

‖y(k+1)-y(k)-φ(k)ΔH(k-1)‖2

(17)

對式(17)求關(guān)于φ(k)的極值，得到參數(shù)矩陣φ(k)的估計(jì)算法為

(18)

式中：η∈(0,1]為步長因子，目的是使算法更具靈活性和一般性.

穩(wěn)定性分析：

(19)

(20)

對式(20)右側(cè)第二項(xiàng)和第三項(xiàng)求取方得：

(21)

由于η∈(0,1)且μ>0，因此下式成立.

(22)

式(21)和式(22)表明存在0

(23)

將式(23)代入式(20)，得：

(24)

e(k+1)=

(25)

由于ρ∈(0,1)和λ>0，使得：

4 實(shí)船實(shí)驗(yàn)與結(jié)果分析

4.1 iNav-IV型無人艇系統(tǒng)架構(gòu)

iNav-IV為一艘純電雙推進(jìn)無人艇，見圖4.長1.7 m、寬0.75 m、吃水0.13 m.無人艇配備兩臺T200水下推進(jìn)器，每臺滿功率條件下可提供50 N推力；同時(shí)配置有慣性測量單元(IMU)、GPS和船岸無線通信網(wǎng)橋.

圖4 iNav-IV型無人艇

實(shí)船實(shí)驗(yàn)在武漢理工大學(xué)高性能船舶實(shí)驗(yàn)中心實(shí)驗(yàn)水池進(jìn)行.實(shí)驗(yàn)場景見圖5.實(shí)驗(yàn)中控制器參數(shù)取值分別為μ=0.5,η=1,ρ=1,λ=8.

圖5 實(shí)驗(yàn)水池

4.2 航向航速協(xié)同控制

為驗(yàn)證航向航速無模型自適應(yīng)控制器效果，設(shè)計(jì)了一組縱向速度為1.0 m/s，轉(zhuǎn)向角為90°的大角度艏向控制實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果見圖6.實(shí)驗(yàn)結(jié)果表明，設(shè)計(jì)的航向航速無模型控制器能夠?qū)崿F(xiàn)對航向和航速協(xié)同控制.

圖6 航向航速協(xié)同控制

4.3 直線路徑跟蹤實(shí)驗(yàn)

iNav-IV型無人艇為一艘水文測繪用艇，其航行路徑通常由多段直線構(gòu)成.為驗(yàn)證設(shè)計(jì)的路徑跟蹤控制器效果，設(shè)計(jì)了圖7的多段直線路徑跟蹤實(shí)驗(yàn)，實(shí)驗(yàn)中無人艇轉(zhuǎn)向點(diǎn)半徑設(shè)為4 m.實(shí)驗(yàn)結(jié)果表明設(shè)計(jì)的無模型自適應(yīng)跟蹤控制方法能夠?qū)崿F(xiàn)對直線路徑的跟蹤，跟蹤誤差控制在±0.5 m以內(nèi).

圖7 多段直線路徑跟蹤

4.4 曲線路徑跟蹤實(shí)驗(yàn)

無人艇在作業(yè)時(shí)因任務(wù)需要或避碰時(shí)需要無人艇具備曲線路徑跟蹤能力.為此設(shè)計(jì)兩組曲線路徑跟蹤實(shí)驗(yàn)來檢驗(yàn)所設(shè)計(jì)的路徑跟蹤算法效果，見圖8.

圖8 曲線路徑跟蹤實(shí)驗(yàn)

圖8a)～b)為無人艇進(jìn)行圓形路徑跟蹤的實(shí)驗(yàn)結(jié)果.實(shí)驗(yàn)結(jié)果表明無人艇能夠較好地實(shí)現(xiàn)對圓形路徑的跟蹤，跟蹤誤差控制在±1 m以內(nèi).

無人艇參考路徑通常由路徑規(guī)劃算法生成，可能包含較為復(fù)雜的曲線.圖8c)～d)為無人艇進(jìn)行正弦路徑跟蹤的實(shí)驗(yàn)結(jié)果.實(shí)驗(yàn)結(jié)果表明，無人艇能夠較好地實(shí)現(xiàn)對正弦路徑的跟蹤，跟蹤誤差控制在±1 m以內(nèi).

5 結(jié) 束 語

針對在存在航行環(huán)境干擾的情況下以固定航速實(shí)現(xiàn)路徑跟蹤的問題，設(shè)計(jì)并實(shí)現(xiàn)了一種無模型自適應(yīng)路徑跟蹤控制器.根據(jù)無人艇動(dòng)力學(xué)模型及無模型控制理論，設(shè)計(jì)了航向航速無模型自適應(yīng)控制器；設(shè)計(jì)了一種改進(jìn)LOS制導(dǎo)算法，解決無人艇曲線路徑跟蹤問題；利用iNav-IV型USV在武漢理工大學(xué)實(shí)驗(yàn)船池的實(shí)船實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明，無模型自適應(yīng)路徑跟蹤控制器能夠較好地實(shí)現(xiàn)對直線與曲線路徑的跟蹤.