王 浩 董早鵬 劉 洋 張海勝 齊詩(shī)杰

(武漢理工大學(xué)船海與能源動(dòng)力工程學(xué)院 武漢 430063)

0 引 言

水面無(wú)人艇(unmanned surface vehicle，USV)軌跡跟蹤的定義是給定一條與時(shí)間相關(guān)的曲線(xiàn)，通過(guò)設(shè)計(jì)控制器算法使USV設(shè)定時(shí)間內(nèi)跟蹤到該軌跡的特定位置上.

準(zhǔn)無(wú)限時(shí)域模型預(yù)測(cè)控制(model predictive control，MPC)因?yàn)槟芴幚鞵ID較難解決的多變量約束優(yōu)化控制問(wèn)題[1]，逐漸被采用在船舶運(yùn)動(dòng)控制領(lǐng)域.文獻(xiàn)[2]提出了一種帶有干擾觀測(cè)器、基于非線(xiàn)性模型預(yù)測(cè)控制方法的欠驅(qū)動(dòng)船舶路徑跟蹤控制器設(shè)計(jì)方法；文獻(xiàn)[3]提出了一種基于模型預(yù)測(cè)控制的直線(xiàn)航跡跟蹤的設(shè)計(jì)方法；文獻(xiàn)[4]提出了一種基于LMI(linear matrix inequalities)的非線(xiàn)性模型預(yù)測(cè)控制方法，實(shí)現(xiàn)了對(duì)USV的鎮(zhèn)定控制；文獻(xiàn)[5]提出了基于線(xiàn)性模型預(yù)測(cè)控制的USV軌跡跟蹤控制方法，但沒(méi)有提出相對(duì)應(yīng)的理論證明所設(shè)計(jì)控制器的穩(wěn)定性.對(duì)于MPC的穩(wěn)定性分析問(wèn)題，文獻(xiàn)[6]指出保證MPC的穩(wěn)定性最直接的辦法是采取無(wú)限時(shí)域；文獻(xiàn)[7]提出了可以證明連續(xù)時(shí)間上準(zhǔn)無(wú)限時(shí)域的模型預(yù)測(cè)控制的方法，解決了無(wú)限時(shí)域無(wú)法直接實(shí)現(xiàn)的問(wèn)題；文獻(xiàn)[8]提出加入終端懲罰函數(shù)，可以證明離散時(shí)間準(zhǔn)無(wú)限時(shí)域的模型預(yù)測(cè)控制的穩(wěn)定性方法,但是未曾在USV軌跡跟蹤上有所應(yīng)用.

根據(jù)以上研究得知：雖然MPC在USV的運(yùn)動(dòng)控制中應(yīng)用很多，但缺少相對(duì)應(yīng)的可以使控制器穩(wěn)定的方法.文中將離散時(shí)間準(zhǔn)無(wú)限時(shí)域模型預(yù)測(cè)控制理論運(yùn)用到USV軌跡跟蹤中，并證明了所設(shè)計(jì)的控制器的穩(wěn)定性.通過(guò)仿真驗(yàn)證了所設(shè)計(jì)控制器收斂性較傳統(tǒng)線(xiàn)性MPC控制器更強(qiáng)，跟蹤過(guò)程中誤差更小.

1 控制器設(shè)計(jì)

1.1 系統(tǒng)模型

USV屬于多輸入多輸出系統(tǒng)，所以采用狀態(tài)空間模型進(jìn)行研究，選取Perez 等[9]提出的船舶狀態(tài)空間模型作為USV的動(dòng)力學(xué)模型.

只考慮船舶前進(jìn)、橫漂和艏搖三個(gè)自由度，驅(qū)動(dòng)力只有縱向推力和轉(zhuǎn)艏力矩，USV軌跡跟蹤示意圖見(jiàn)圖1.

xnonyn-大地坐標(biāo)系；xbobyb-USV的隨體坐標(biāo)系；s-目標(biāo)軌跡；s1-軌跡的起始點(diǎn)

USV的水平面三自由度運(yùn)動(dòng)模型為

(1)

式中：x,y,φ為位置和姿態(tài)角信息；u,v,r為速度信息；τu為USV縱向推力；τr為USV轉(zhuǎn)艏力矩，由于文中考慮的是欠驅(qū)動(dòng)船舶，所以無(wú)側(cè)向推力；m11,m22,m33為含附加質(zhì)量系數(shù)在內(nèi)的慣性系數(shù)；d11,d22,d33為阻尼系數(shù).

1.2 模型的線(xiàn)性化及離散化

由于本文旨在追蹤任意一條直接給點(diǎn)的軌跡，參考文獻(xiàn)[10]中近似化方法，假設(shè)參考系統(tǒng)已經(jīng)跟蹤了給定的軌跡，得到了軌跡中每個(gè)時(shí)刻的狀態(tài)量和控制量，通過(guò)對(duì)參考系統(tǒng)與當(dāng)前實(shí)際系統(tǒng)的偏差處理，設(shè)計(jì)模型預(yù)測(cè)控制器以達(dá)到軌跡跟蹤的目的.其中，參考系統(tǒng)的系統(tǒng)方程為

(2)

式中：χR=[xR,yR,φR,uR,vR,rR]T為參考系統(tǒng)的狀態(tài)量；τR=[τuR,τrR]T為參考系統(tǒng)的輸入控制量.

實(shí)際系統(tǒng)在任意點(diǎn)(χR,uR)進(jìn)行泰勒展開(kāi)，保留一階項(xiàng)可得：

(3)

式中：χ為USV實(shí)際系統(tǒng)的狀態(tài)量；τ為實(shí)際系統(tǒng)的輸入控制量.結(jié)合式(2)～(3)可得：

(4)

結(jié)合式(1)，可得A,B矩陣為

(5)

考慮到模型預(yù)測(cè)控制器需要離散式的模型，對(duì)式(5)采取歐拉法離散，可得：

(7)

式中：dt為時(shí)間步長(zhǎng)；I為單位矩陣；k為任意時(shí)刻.

1.3 設(shè)計(jì)目標(biāo)函數(shù)

(8)

為使目標(biāo)函數(shù)便于數(shù)值求解，以Δτ(k+i)為參量，設(shè)計(jì)：

(9)

結(jié)合式(7)，可得新的系統(tǒng)表達(dá)式：

(10)

Ak+i=Ak,Bk+i=Bk,i=1,2,…,NP

(11)

參考文獻(xiàn)[8]，求式(8)中的終端懲罰函數(shù).

選取一個(gè)參數(shù)κ使其滿(mǎn)足條件：

κ>1

(12)

求解離散形式的Lyapunov方程：

(13)

可得唯一確定的正定矩陣P，且存在α∈(0，∞)，使平衡點(diǎn)的某一鄰域：

(14)

(15)

結(jié)合式(8)、式(15)，可將目標(biāo)函數(shù)改寫(xiě)為

(16)

則系統(tǒng)的模型預(yù)測(cè)表達(dá)式可寫(xiě)為

X(k)=Ωξ(k)+ΓΔU

(17)

將式(17)代入式(16)并忽略掉與ΔU無(wú)關(guān)的項(xiàng)可得：

(18)

1.4 約束條件設(shè)置

USV在實(shí)際工況中受執(zhí)行機(jī)構(gòu)如螺旋槳、舵等機(jī)械結(jié)構(gòu)的限制，映射到控制算法中就是需要對(duì)USV的輸入力τu、力矩τr以及輸入增量Δτu、Δτr進(jìn)行限制.同時(shí)考慮到USV操縱性能，需要對(duì)其輸出量進(jìn)行約束，本文研究的是軌跡跟蹤，需要同時(shí)考慮對(duì)6個(gè)狀態(tài)量的要求，所以將6個(gè)狀態(tài)量均看作輸出量，進(jìn)行約束限制，參考文獻(xiàn)[5]，具體約束形式如下.

Δτmin≤Δτ(k+i)≤Δτmaxi=0,1,2,…,NC-1

(19)

τmin≤τ(k+i)≤τmaxi=0,1,2,…,NC-1

(20)

(21)

結(jié)合式(18)，可以把控制問(wèn)題轉(zhuǎn)化為下式的優(yōu)化問(wèn)題求解：

s.t. ΔUmin≤ΔU(k)≤ΔUmax

Umin≤C·ΔU(k)+U(k)≤ΔUmax

ζmin≤ζ(k)≤ζmax

(22)

式中：ΔUmin=INC?Δτmin,ΔUmax=INC?Δτmax

Umin=INC?τmin,Umax=INC?τmax

ζmin=INC?χmin,ζmax=INC?χmax

式中：INC為行數(shù)為NC的單位列向量；INu為維數(shù)為控制輸入維度的單位矩陣；?為克羅內(nèi)克積.

2 穩(wěn)定性證明

參考文獻(xiàn)[8]，分兩步證明本文設(shè)計(jì)的控制器穩(wěn)定性問(wèn)題，首先證明終端懲罰函數(shù)可以替代無(wú)限預(yù)測(cè)時(shí)域.

(23)

(24)

(25)

當(dāng)κ>1且α→0時(shí)，下式成立

(26)

(27)

對(duì)式(27)從k→∞時(shí)刻進(jìn)行求和，則有

(28)

(29)

第二步證明系統(tǒng)穩(wěn)定性.

結(jié)合式(4)可得，(0,0)點(diǎn)為系統(tǒng)的平衡點(diǎn)，且(0,0)表示任意一處USV實(shí)際系統(tǒng)跟蹤上參考軌跡點(diǎn)的工況，與USV實(shí)際系統(tǒng)根據(jù)模型預(yù)測(cè)控制的原理，結(jié)合式(8)，式(10)可以將k時(shí)刻目標(biāo)函數(shù)的最小值即最優(yōu)值記為

(30)

k+1時(shí)刻考慮可行解的目標(biāo)函數(shù)值可寫(xiě)為

(31)

考慮到最優(yōu)解應(yīng)該小于可行解，則有：

J*(k+1)≤J(k+1)≤

(32)

可證J*(k)是單調(diào)不增的，即系統(tǒng)在設(shè)計(jì)控制器作用下漸進(jìn)穩(wěn)定.

3 仿 真

3.1 仿真參數(shù)

為驗(yàn)證算法可行性，參考文獻(xiàn)[11]中簡(jiǎn)化后的模型，模型參數(shù)見(jiàn)表1.

表1 USV模型參數(shù)

3.2 仿真工況與參數(shù)

為證明文中所設(shè)計(jì)算法的普適性和優(yōu)越性，分別進(jìn)行直線(xiàn)與正弦曲線(xiàn)軌跡的仿真實(shí)驗(yàn)，并與文獻(xiàn)[5]中的算法進(jìn)行比較.具體工況與參數(shù)如下.

工況1跟蹤直線(xiàn)，跟蹤參考軌跡為

期望軌跡的速度量可以通過(guò)給定軌跡位置量微分得到，為

約束條件為

與文獻(xiàn)[5]中的基于線(xiàn)性模型預(yù)測(cè)USV軌跡跟蹤控制器進(jìn)行結(jié)果比較，將優(yōu)化問(wèn)題式(22)進(jìn)行仿真，得到的仿真結(jié)果見(jiàn)圖2～4.

圖2 跟蹤軌跡比較

由圖2可知:所設(shè)計(jì)控制器與線(xiàn)性模型預(yù)測(cè)控制器均可跟蹤給定軌跡，但在初始位置與目標(biāo)軌跡偏差較大的情況下，加入終端懲罰函數(shù)即本文設(shè)計(jì)的準(zhǔn)無(wú)限時(shí)域模型預(yù)測(cè)控制器相對(duì)于線(xiàn)性模型預(yù)測(cè)控制器在跟蹤直線(xiàn)軌跡時(shí)，誤差幅值更小，且可以更快的收斂于給定軌跡.

由圖4可知:所設(shè)計(jì)控制器輸入力和力矩收斂于定值，由圖3可知，USV狀態(tài)量在此控制器輸入下收斂于定值，符合所跟蹤直線(xiàn)軌跡的期望狀態(tài)量，驗(yàn)證了所設(shè)計(jì)控制器的穩(wěn)定性.

圖3 USV狀態(tài)量

圖4 USV控制量

工況2 跟蹤正弦曲線(xiàn),選取參考軌跡為

圖5 跟蹤軌跡比較

由圖5可知:本文所設(shè)計(jì)控制器與線(xiàn)性模型預(yù)測(cè)控制器均能跟蹤上期望軌跡，但在初始位置有偏差時(shí)，USV在本文所設(shè)計(jì)控制器控制作用下，可以更快地收斂于期望軌跡且誤差幅值要比線(xiàn)性模型預(yù)測(cè)控制器要小.在正弦曲線(xiàn)軌跡曲率最大處，文中所設(shè)計(jì)控制器跟蹤期望軌跡誤差更小.

由圖6可知，USV狀態(tài)量在此輸入下也呈現(xiàn)周期性變化，符合USV航行軌跡為正弦曲線(xiàn)時(shí)的狀態(tài)量變化趨勢(shì).由圖7可知，所設(shè)計(jì)控制器輸入力和力矩呈周期性變化.

圖6 USV狀態(tài)量

圖7 USV控制量

4 結(jié) 論

1) 將準(zhǔn)無(wú)限時(shí)域模型預(yù)測(cè)控制理論拓展到USV軌跡跟蹤控制器設(shè)計(jì)中.設(shè)計(jì)了一種基于準(zhǔn)無(wú)限時(shí)域的模型預(yù)測(cè)控制器，并證明了所設(shè)計(jì)的控制器是穩(wěn)定的.

2) 通過(guò)在仿真平臺(tái)上進(jìn)行直線(xiàn)軌跡以及正弦曲線(xiàn)軌跡的仿真驗(yàn)證，證實(shí)了本文所設(shè)計(jì)控制器的穩(wěn)定性和普適性.相較于線(xiàn)性模型預(yù)測(cè)控制器，本文所設(shè)計(jì)控制器可以更快的收斂到期望軌跡，且在跟蹤軌跡過(guò)程中，與期望軌跡的誤差更小.本文的不足之處在于未考慮擾動(dòng)的影響，后續(xù)將作進(jìn)一步研究.