基于改進(jìn)的投餌船模型預(yù)測(cè)軌跡跟蹤控制

張麗珍,顧素郡,吳 迪

（上海海洋大學(xué)工程學(xué)院，上海 201306）

0 引言

軌跡跟蹤控制是自主巡航投餌船運(yùn)動(dòng)控制的一項(xiàng)重要研究?jī)?nèi)容，它是指投餌船從起始位置沿著給定的參考軌跡航行到目的地[1,2]。投餌船具有時(shí)滯的特點(diǎn)，并且投餌船的運(yùn)動(dòng)學(xué)模型會(huì)隨著航行狀態(tài)的不同而變化，海上環(huán)境因素的干擾也會(huì)導(dǎo)致跟蹤發(fā)生偏離。另外，投餌船在軌跡跟蹤的過(guò)程中，系統(tǒng)獨(dú)立控制變量的個(gè)數(shù)比其本身的自由度個(gè)數(shù)少。以上這些特點(diǎn)使得投餌船軌跡跟蹤控制成為一個(gè)非線性、欠驅(qū)動(dòng)、受多種約束限制的控制問(wèn)題[3]。對(duì)于投餌船這樣存在多種約束限制的系統(tǒng)，約束會(huì)破壞系統(tǒng)獲取理想的控制性能，導(dǎo)致跟蹤發(fā)生不穩(wěn)定的現(xiàn)象。而對(duì)執(zhí)行機(jī)構(gòu)采用較大的控制動(dòng)作能夠控制投餌船保持較高程度的跟隨性以及穩(wěn)定性。但是，投餌船執(zhí)行動(dòng)作不能過(guò)大是投餌船系統(tǒng)對(duì)控制增量的時(shí)域約束。所以投餌船系統(tǒng)對(duì)控制量的時(shí)域約束與對(duì)控制性能的要求產(chǎn)生矛盾，這讓投餌船軌跡跟蹤問(wèn)題更加困難[4]。

模型預(yù)測(cè)控制（Model Predictive Control,MPC）是一種基于狀態(tài)空間方程的反饋控制，該控制算法利于解決投餌船這種非線性動(dòng)力學(xué)問(wèn)題。系統(tǒng)通過(guò)對(duì)每一當(dāng)前時(shí)刻的優(yōu)化，來(lái)充分利用允許的控制動(dòng)作，從而提高控制的性能，達(dá)到最優(yōu)控制。同時(shí)，在預(yù)測(cè)系統(tǒng)未來(lái)的輸出中，能夠及時(shí)糾正因時(shí)變約束引起的跟蹤不穩(wěn)定現(xiàn)象[5,6]。是處理時(shí)域約束系統(tǒng)控制的最優(yōu)方法，具有較強(qiáng)的魯棒性，對(duì)解決受約束自主巡航投餌船軌跡跟蹤控制問(wèn)題具有獨(dú)特的優(yōu)勢(shì)。

文獻(xiàn)[7]在模型預(yù)測(cè)控制器中考慮了環(huán)境干擾因素，實(shí)現(xiàn)了無(wú)人船軌跡和航向的穩(wěn)定跟蹤。但是沒(méi)有進(jìn)行實(shí)時(shí)控制，處理約束條件的能力低，使得無(wú)人船跟蹤收斂效果比較差。文獻(xiàn)[8]采用廣義預(yù)測(cè)控制的算法，根據(jù)所建立的大型船舶模型提出了約束條件，設(shè)計(jì)了約束軌跡跟蹤控制器，解決了直線軌跡跟蹤不穩(wěn)定的問(wèn)題，但是沒(méi)有考慮曲線以及其它航跡的跟蹤問(wèn)題。文獻(xiàn)[9]針對(duì)無(wú)人艇非線性系統(tǒng)，構(gòu)造了穩(wěn)定性控制律，實(shí)現(xiàn)了較精準(zhǔn)的軌跡跟蹤。但是，該控制算法是建立在船舶存在橫搖運(yùn)動(dòng)的基礎(chǔ)上，要求船舶在參考軌跡的鄰域內(nèi)航行，這就導(dǎo)致了船舶不能實(shí)現(xiàn)全面完整的軌跡跟蹤。

本文在充分考慮投餌船非線性欠驅(qū)動(dòng)約束的特性下，分別研究了投餌船受時(shí)域約束的直線和兩種不同曲線的軌跡跟蹤性能。采用模型預(yù)測(cè)控制的方法處理時(shí)變控制輸入約束，并將線性時(shí)變模型預(yù)測(cè)系統(tǒng)輸出結(jié)合系統(tǒng)狀態(tài)量、控制量以及控制增量的時(shí)域約束，把軌跡跟蹤控制轉(zhuǎn)化為二次規(guī)劃目標(biāo)函數(shù)的求解。通過(guò)仿真證實(shí)了該控制器是提高直線，雙移線和蛇形軌跡跟蹤性能的最優(yōu)方案。

1 投餌船系統(tǒng)模型建立

1.1 運(yùn)動(dòng)學(xué)模型

投餌船三自由度運(yùn)動(dòng)學(xué)方程[10]為：

式中，位置狀態(tài)向量E=[x,y,ψ]分別是投餌船所在位置的縱坐標(biāo)，橫坐標(biāo)和航向角；速度狀態(tài)向量v=[u,v,r]分別表示投餌船沿縱坐標(biāo)方向的運(yùn)動(dòng)速度，沿橫坐標(biāo)方向的運(yùn)動(dòng)速度以及航向角速度。

1.2 動(dòng)力學(xué)模型

投餌船的動(dòng)力學(xué)方程為：

式中,mi（i=1,2,3）為投餌船包含附加質(zhì)量在內(nèi)的慣性質(zhì)量；di（i=1,2,3）為投餌船三個(gè)位置狀態(tài)下的水動(dòng)力系數(shù)；τ=[τ1τ2τ3]=[F1+F20 (F1-F2)·L]，F(xiàn)1和F2分別是左右推進(jìn)器的推力,L表示推進(jìn)器到投餌船中線的距離。

2 問(wèn)題制定

把上式(1)和式(2)轉(zhuǎn)換為非線性狀態(tài)空間方程的形式：

設(shè)定參考軌跡上的每一個(gè)點(diǎn)都能夠滿足上述運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程。用d代表參考量，則非線性狀態(tài)空間方程為：

將式(3)和式(4)相減，則系統(tǒng)輸出量和期望量的誤差量為：

為了讓投餌船系統(tǒng)良好地跟蹤參考軌跡，期望投餌船的控制輸入接近參考軌跡的虛擬輸入，即滿足。

3 線性時(shí)變模型預(yù)測(cè)集成控制器設(shè)計(jì)

模型預(yù)測(cè)控制的投餌船軌跡跟蹤控制器的原理圖如圖1。在整個(gè)控制的過(guò)程中，設(shè)定一條期望的參考軌跡，并建立與控制器的接口。投餌船系統(tǒng)通過(guò)求解改進(jìn)的目標(biāo)函數(shù)以及對(duì)控制量，控制增量以及輸出量的時(shí)域約束優(yōu)化，得到控制時(shí)域內(nèi)一系列控制輸出，系統(tǒng)將其最優(yōu)輸出值設(shè)定為投餌船控制系統(tǒng)當(dāng)前時(shí)刻的實(shí)際控制量。控制器則根據(jù)當(dāng)前時(shí)刻投餌船系統(tǒng)的獲取值和參考軌跡，預(yù)測(cè)未來(lái)一段時(shí)間的系統(tǒng)輸出。在下一個(gè)時(shí)刻，系統(tǒng)重復(fù)滾動(dòng)優(yōu)化。加上時(shí)域約束，確保了結(jié)果是在投餌船動(dòng)力學(xué)模型執(zhí)行范圍內(nèi)。同時(shí)，二次規(guī)劃最優(yōu)問(wèn)題的算法往往需要反復(fù)迭代的結(jié)果，加上時(shí)域約束，能夠減少控制系統(tǒng)對(duì)狀態(tài)空間方程的計(jì)算時(shí)間。

圖1 控制器原理圖

3.1 線性誤差模型

將投餌船誤差模型進(jìn)行線性離散化處理[11]，則系統(tǒng)在每個(gè)κ采樣時(shí)刻的線性時(shí)變模型預(yù)測(cè)方程為：

基于增量形式的狀態(tài)空間預(yù)測(cè)模型，用控制增量ΔU（κ）代替控制量U（κ）作為輸入量，同時(shí)約束控制增量和控制量，從而避免投餌船受外界干擾而穩(wěn)定地跟蹤目標(biāo)軌跡[12,13]。新的增廣狀態(tài)空間方程為：

式中，各矩陣定義如下：

3.2 未來(lái)狀態(tài)預(yù)測(cè)

3.3 約束優(yōu)化

式中：INC為NC行列向量，?為克羅內(nèi)克積，U（t-1）為上一時(shí)刻投餌船實(shí)際的控制量。

由式(15)和式(16)，控制量約束可轉(zhuǎn)化為如下形式：

在每一控制時(shí)域周期內(nèi)計(jì)算式(13)，得到最優(yōu)解為ΔU*C（κ）。把最優(yōu)解的第一列作為系統(tǒng)實(shí)際控制增量，并把該增量和上一時(shí)刻的控制量之和作用于當(dāng)前時(shí)刻：

依據(jù)模型預(yù)測(cè)控制的滾動(dòng)優(yōu)化策略，下一時(shí)刻依據(jù)上一時(shí)刻的狀態(tài)信息重新預(yù)測(cè)下一時(shí)刻的輸出，反復(fù)求解，得出系統(tǒng)控制過(guò)程中的最優(yōu)控制輸入。

4 仿真分析

為了驗(yàn)證本文所設(shè)計(jì)控制器的跟蹤性，在MATLAB/Simulink環(huán)境下對(duì)投餌船進(jìn)行軌跡跟蹤的仿真。對(duì)比三種不同軌跡仿真工況下，投餌船軌跡跟蹤的實(shí)時(shí)穩(wěn)定性以及跟隨性。不同仿真工況結(jié)果如下：

表1 投餌船模型參數(shù)

投餌船初始位置和方向設(shè)置為:x（0）=0，y（0）=0，ψ=0，初始速度和角速度設(shè)置為：u=2m/s，v=0/s，r=0m/s。

模型預(yù)測(cè)控制器仿真參數(shù)以及時(shí)域約束條件設(shè)置為：Ts=0.02s，預(yù)測(cè)時(shí)域NY=15，控制時(shí)域NC=1，誤差權(quán)矩陣Q=[600 0；0 800]，權(quán)值矩陣R=10。-20°≤ψ≤40°，-15°≤δ≤30°。

工況一：直線軌跡跟蹤

從圖2的仿真結(jié)果可以看出，本文設(shè)計(jì)的控制器基本實(shí)現(xiàn)了對(duì)直線軌跡較高精度的跟蹤。由圖（a）到圖（g）可知，投餌船的初始位置不在期望直線軌跡的初始位置上，但在5s內(nèi)，投餌船能夠快速地跟蹤上期望的直線軌跡，并且在隨后的運(yùn)動(dòng)中保持穩(wěn)定跟蹤。直線軌跡跟蹤的最大誤差值為投餌船初始位置與期望位置的距離，為3m。隨著投餌船不斷地向期望軌跡靠近，誤差值逐漸減小。在5s后，橫向位置跟蹤誤差基本收斂為零。航向角跟蹤在最初的5s內(nèi)，為了調(diào)整到期望軌跡的航向，變化較大，誤差范圍在40°以內(nèi)。5s后，航向跟蹤上的期望的軌跡航向，基本實(shí)現(xiàn)零偏差。前5s內(nèi)，航向角速度變化范圍大，最大為40°/s，但5s后基本能保持平緩穩(wěn)定。在投餌船進(jìn)行直線跟蹤過(guò)程中，前5s內(nèi)，因?yàn)檎{(diào)整航向，速度出現(xiàn)波動(dòng)，但5s后，速度能夠穩(wěn)定控制在2m/s內(nèi)。在15s的航行中，轉(zhuǎn)角始終控制在約束范圍內(nèi)。

圖2 直線軌跡跟蹤仿真結(jié)果

工況二：雙移線軌跡跟蹤

從圖3的仿真結(jié)果可以看出，本文設(shè)計(jì)的控制器對(duì)雙移線有良好的跟蹤性能。由圖3(h)到圖3(n)可知，在整個(gè)控制時(shí)域內(nèi)，投餌船基本實(shí)現(xiàn)了橫向位置良好的跟蹤效果，跟蹤誤差在0.05m范圍內(nèi)波動(dòng)?？赡苡捎趧?dòng)力學(xué)模型與控制器之間存在延遲模塊，用于計(jì)算系統(tǒng)實(shí)際航向角和采用的雙移線參考軌跡線性化方法所確定的航向角之間的偏差，造成了控制在時(shí)間上的滯后，導(dǎo)致航向控制在拐點(diǎn)處產(chǎn)生了6°左右的偏差值，但控制器能夠及時(shí)調(diào)整偏差，最終仍能保持向期望的雙移線航向前進(jìn)。航向角速度基本能夠平穩(wěn)的跟蹤上期望值，航行中有較小的波動(dòng)，但波動(dòng)范圍都在可控范圍內(nèi)，并且航向角速度最終實(shí)現(xiàn)了收斂。在整個(gè)雙移線航行中，前2s，因?yàn)樾枰D(zhuǎn)向，速度有微小的跳變，但隨后，縱向速度基本控制在2m/s。完成雙移線跟蹤需要25s，轉(zhuǎn)角始終控制在6°范圍內(nèi)。

圖3 雙移線軌跡跟蹤仿真結(jié)果

工況三：蛇形軌跡跟蹤

從圖4的仿真結(jié)果可以看出，本文設(shè)計(jì)的控制器提高了蛇形運(yùn)動(dòng)軌跡的跟蹤性能。由圖4(o)到圖4(u)可知，投餌船能夠穩(wěn)定地跟蹤期望的蛇形軌跡，在整個(gè)蛇形軌跡航行中，橫向位置誤差縮小在0.05m范圍內(nèi)。因?yàn)樯咝诬壽E的曲率在實(shí)時(shí)變化，航向控制的執(zhí)行存在時(shí)間滯后，導(dǎo)致在整個(gè)控制時(shí)域內(nèi)，航向始終存在誤差，最大誤差達(dá)5°左右，但在控制器可控范圍之內(nèi)，不會(huì)使投餌船出現(xiàn)嚴(yán)重偏離。航向角速度在每一時(shí)刻下，對(duì)蛇形軌跡的跟蹤都能夠保持平滑穩(wěn)定，基本沒(méi)有跳變現(xiàn)象，并且最終能夠有效收斂。在蛇形軌跡跟蹤過(guò)程中，速度在最初航行時(shí)出現(xiàn)較小的波動(dòng)，大約2s后，基本穩(wěn)定在2m/s。完成蛇形軌跡的跟蹤需要50s，航行中的轉(zhuǎn)角仍然控制在6°范圍內(nèi)。

圖4 蛇形軌跡跟蹤仿真結(jié)果

5 結(jié)語(yǔ)

本文研究了帶有時(shí)域約束的自主巡航投餌船軌跡跟蹤問(wèn)題，采用模型預(yù)測(cè)軌跡跟蹤控制器實(shí)現(xiàn)了投餌船在移動(dòng)時(shí)域下對(duì)直線，雙移線和蛇形參考軌跡的跟蹤控制。在控制器的設(shè)計(jì)過(guò)程中，考慮了控制輸入受時(shí)變的約束影響，讓其能夠有效的處理系統(tǒng)時(shí)域約束以及非線性動(dòng)力學(xué)問(wèn)題。仿真結(jié)果表明，采用模型預(yù)測(cè)控制算法可以有效地解決帶時(shí)域約束的投餌船軌跡跟蹤控制問(wèn)題，達(dá)到良好的跟隨性和穩(wěn)定性。但仍然存在不足，對(duì)軌跡曲率實(shí)時(shí)變化下的跟蹤仍有一定的偏差，需在后續(xù)工作中進(jìn)一步展開(kāi)控制研究。