基于準無限時域MPC的無人艇軌跡跟蹤控制器

王 浩 董早鵬 劉 洋 張海勝 齊詩杰

(武漢理工大學船海與能源動力工程學院 武漢 430063)

0 引 言

水面無人艇(unmanned surface vehicle，USV)軌跡跟蹤的定義是給定一條與時間相關的曲線，通過設計控制器算法使USV設定時間內跟蹤到該軌跡的特定位置上.

準無限時域模型預測控制(model predictive control，MPC)因為能處理PID較難解決的多變量約束優化控制問題[1]，逐漸被采用在船舶運動控制領域.文獻[2]提出了一種帶有干擾觀測器、基于非線性模型預測控制方法的欠驅動船舶路徑跟蹤控制器設計方法；文獻[3]提出了一種基于模型預測控制的直線航跡跟蹤的設計方法；文獻[4]提出了一種基于LMI(linear matrix inequalities)的非線性模型預測控制方法，實現了對USV的鎮定控制；文獻[5]提出了基于線性模型預測控制的USV軌跡跟蹤控制方法，但沒有提出相對應的理論證明所設計控制器的穩定性.對于MPC的穩定性分析問題，文獻[6]指出保證MPC的穩定性最直接的辦法是采取無限時域；文獻[7]提出了可以證明連續時間上準無限時域的模型預測控制的方法，解決了無限時域無法直接實現的問題；文獻[8]提出加入終端懲罰函數，可以證明離散時間準無限時域的模型預測控制的穩定性方法,但是未曾在USV軌跡跟蹤上有所應用.

根據以上研究得知：雖然MPC在USV的運動控制中應用很多，但缺少相對應的可以使控制器穩定的方法.文中將離散時間準無限時域模型預測控制理論運用到USV軌跡跟蹤中，并證明了所設計的控制器的穩定性.通過仿真驗證了所設計控制器收斂性較傳統線性MPC控制器更強，跟蹤過程中誤差更小.

1 控制器設計

1.1 系統模型

USV屬于多輸入多輸出系統，所以采用狀態空間模型進行研究，選取Perez 等[9]提出的船舶狀態空間模型作為USV的動力學模型.

只考慮船舶前進、橫漂和艏搖三個自由度，驅動力只有縱向推力和轉艏力矩，USV軌跡跟蹤示意圖見圖1.

xnonyn-大地坐標系；xbobyb-USV的隨體坐標系；s-目標軌跡；s1-軌跡的起始點

USV的水平面三自由度運動模型為

(1)

式中：x,y,φ為位置和姿態角信息；u,v,r為速度信息；τu為USV縱向推力；τr為USV轉艏力矩，由于文中考慮的是欠驅動船舶，所以無側向推力；m11,m22,m33為含附加質量系數在內的慣性系數；d11,d22,d33為阻尼系數.

1.2 模型的線性化及離散化

由于本文旨在追蹤任意一條直接給點的軌跡，參考文獻[10]中近似化方法，假設參考系統已經跟蹤了給定的軌跡，得到了軌跡中每個時刻的狀態量和控制量，通過對參考系統與當前實際系統的偏差處理，設計模型預測控制器以達到軌跡跟蹤的目的.其中，參考系統的系統方程為

(2)

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

實際系統在任意點(χR,uR)進行泰勒展開，保留一階項可得：

(3)

式中：χ為USV實際系統的狀態量；τ為實際系統的輸入控制量.結合式(2)～(3)可得：

(4)

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

(5)

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

(7)

式中：dt為時間步長；I為單位矩陣；k為任意時刻.

1.3 設計目標函數

(8)

為使目標函數便于數值求解，以Δτ(k+i)為參量，設計：

(9)

結合式(7)，可得新的系統表達式：

(10)

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

(11)

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

選取一個參數κ使其滿足條件：

κ>1

(12)

求解離散形式的Lyapunov方程：

(13)

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

(14)

(15)

結合式(8)、式(15)，可將目標函數改寫為

(16)

則系統的模型預測表達式可寫為

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

(17)

將式(17)代入式(16)并忽略掉與ΔU無關的項可得：

(18)

1.4 約束條件設置

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

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

(19)

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

(20)

(21)

結合式(18)，可以把控制問題轉化為下式的優化問題求解：

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為行數為NC的單位列向量；INu為維數為控制輸入維度的單位矩陣；?為克羅內克積.

2 穩定性證明

參考文獻[8]，分兩步證明本文設計的控制器穩定性問題，首先證明終端懲罰函數可以替代無限預測時域.

(23)

(24)

(25)

當κ>1且α→0時，下式成立

(26)

(27)

對式(27)從k→∞時刻進行求和，則有

(28)

(29)

第二步證明系統穩定性.

結合式(4)可得，(0,0)點為系統的平衡點，且(0,0)表示任意一處USV實際系統跟蹤上參考軌跡點的工況，與USV實際系統根據模型預測控制的原理，結合式(8)，式(10)可以將k時刻目標函數的最小值即最優值記為

(30)

k+1時刻考慮可行解的目標函數值可寫為

(31)

考慮到最優解應該小于可行解，則有：

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

(32)

可證J*(k)是單調不增的，即系統在設計控制器作用下漸進穩定.

3 仿 真

3.1 仿真參數

為驗證算法可行性，參考文獻[11]中簡化后的模型，模型參數見表1.

表1 USV模型參數

3.2 仿真工況與參數

為證明文中所設計算法的普適性和優越性，分別進行直線與正弦曲線軌跡的仿真實驗，并與文獻[5]中的算法進行比較.具體工況與參數如下.

工況1跟蹤直線，跟蹤參考軌跡為

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

約束條件為

與文獻[5]中的基于線性模型預測USV軌跡跟蹤控制器進行結果比較，將優化問題式(22)進行仿真，得到的仿真結果見圖2～4.

圖2 跟蹤軌跡比較

由圖2可知:所設計控制器與線性模型預測控制器均可跟蹤給定軌跡，但在初始位置與目標軌跡偏差較大的情況下，加入終端懲罰函數即本文設計的準無限時域模型預測控制器相對于線性模型預測控制器在跟蹤直線軌跡時，誤差幅值更小，且可以更快的收斂于給定軌跡.

由圖4可知:所設計控制器輸入力和力矩收斂于定值，由圖3可知，USV狀態量在此控制器輸入下收斂于定值，符合所跟蹤直線軌跡的期望狀態量，驗證了所設計控制器的穩定性.

圖3 USV狀態量

圖4 USV控制量

工況2 跟蹤正弦曲線,選取參考軌跡為

圖5 跟蹤軌跡比較

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

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

圖6 USV狀態量

圖7 USV控制量

4 結 論

1) 將準無限時域模型預測控制理論拓展到USV軌跡跟蹤控制器設計中.設計了一種基于準無限時域的模型預測控制器，并證明了所設計的控制器是穩定的.

2) 通過在仿真平臺上進行直線軌跡以及正弦曲線軌跡的仿真驗證，證實了本文所設計控制器的穩定性和普適性.相較于線性模型預測控制器，本文所設計控制器可以更快的收斂到期望軌跡，且在跟蹤軌跡過程中，與期望軌跡的誤差更小.本文的不足之處在于未考慮擾動的影響，后續將作進一步研究.