基于改進的投餌船模型預測軌跡跟蹤控制

張麗珍,顧素郡,吳 迪

（上海海洋大學工程學院，上海 201306）

0 引言

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

模型預測控制（Model Predictive Control,MPC）是一種基于狀態空間方程的反饋控制，該控制算法利于解決投餌船這種非線性動力學問題。系統通過對每一當前時刻的優化，來充分利用允許的控制動作，從而提高控制的性能，達到最優控制。同時，在預測系統未來的輸出中，能夠及時糾正因時變約束引起的跟蹤不穩定現象[5,6]。是處理時域約束系統控制的最優方法，具有較強的魯棒性，對解決受約束自主巡航投餌船軌跡跟蹤控制問題具有獨特的優勢。

文獻[7]在模型預測控制器中考慮了環境干擾因素，實現了無人船軌跡和航向的穩定跟蹤。但是沒有進行實時控制，處理約束條件的能力低，使得無人船跟蹤收斂效果比較差。文獻[8]采用廣義預測控制的算法，根據所建立的大型船舶模型提出了約束條件，設計了約束軌跡跟蹤控制器，解決了直線軌跡跟蹤不穩定的問題，但是沒有考慮曲線以及其它航跡的跟蹤問題。文獻[9]針對無人艇非線性系統，構造了穩定性控制律，實現了較精準的軌跡跟蹤。但是，該控制算法是建立在船舶存在橫搖運動的基礎上，要求船舶在參考軌跡的鄰域內航行，這就導致了船舶不能實現全面完整的軌跡跟蹤。

本文在充分考慮投餌船非線性欠驅動約束的特性下，分別研究了投餌船受時域約束的直線和兩種不同曲線的軌跡跟蹤性能。采用模型預測控制的方法處理時變控制輸入約束，并將線性時變模型預測系統輸出結合系統狀態量、控制量以及控制增量的時域約束，把軌跡跟蹤控制轉化為二次規劃目標函數的求解。通過仿真證實了該控制器是提高直線，雙移線和蛇形軌跡跟蹤性能的最優方案。

1 投餌船系統模型建立

1.1 運動學模型

投餌船三自由度運動學方程[10]為：

式中，位置狀態向量E=[x,y,ψ]分別是投餌船所在位置的縱坐標，橫坐標和航向角；速度狀態向量v=[u,v,r]分別表示投餌船沿縱坐標方向的運動速度，沿橫坐標方向的運動速度以及航向角速度。

1.2 動力學模型

投餌船的動力學方程為：

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

2 問題制定

把上式(1)和式(2)轉換為非線性狀態空間方程的形式：

設定參考軌跡上的每一個點都能夠滿足上述運動學和動力學方程。用d代表參考量，則非線性狀態空間方程為：

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

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

3 線性時變模型預測集成控制器設計

模型預測控制的投餌船軌跡跟蹤控制器的原理圖如圖1。在整個控制的過程中，設定一條期望的參考軌跡，并建立與控制器的接口。投餌船系統通過求解改進的目標函數以及對控制量，控制增量以及輸出量的時域約束優化，得到控制時域內一系列控制輸出，系統將其最優輸出值設定為投餌船控制系統當前時刻的實際控制量。控制器則根據當前時刻投餌船系統的獲取值和參考軌跡，預測未來一段時間的系統輸出。在下一個時刻，系統重復滾動優化。加上時域約束，確保了結果是在投餌船動力學模型執行范圍內。同時，二次規劃最優問題的算法往往需要反復迭代的結果，加上時域約束，能夠減少控制系統對狀態空間方程的計算時間。

圖1 控制器原理圖

3.1 線性誤差模型

將投餌船誤差模型進行線性離散化處理[11]，則系統在每個κ采樣時刻的線性時變模型預測方程為：

基于增量形式的狀態空間預測模型，用控制增量ΔU（κ）代替控制量U（κ）作為輸入量，同時約束控制增量和控制量，從而避免投餌船受外界干擾而穩定地跟蹤目標軌跡[12,13]。新的增廣狀態空間方程為：

式中，各矩陣定義如下：

3.2 未來狀態預測

3.3 約束優化

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

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

在每一控制時域周期內計算式(13)，得到最優解為ΔU*C（κ）。把最優解的第一列作為系統實際控制增量，并把該增量和上一時刻的控制量之和作用于當前時刻：

依據模型預測控制的滾動優化策略，下一時刻依據上一時刻的狀態信息重新預測下一時刻的輸出，反復求解，得出系統控制過程中的最優控制輸入。

4 仿真分析

為了驗證本文所設計控制器的跟蹤性，在MATLAB/Simulink環境下對投餌船進行軌跡跟蹤的仿真。對比三種不同軌跡仿真工況下，投餌船軌跡跟蹤的實時穩定性以及跟隨性。不同仿真工況結果如下：

表1 投餌船模型參數

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

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

工況一：直線軌跡跟蹤

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

圖2 直線軌跡跟蹤仿真結果

工況二：雙移線軌跡跟蹤

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

圖3 雙移線軌跡跟蹤仿真結果

工況三：蛇形軌跡跟蹤

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

圖4 蛇形軌跡跟蹤仿真結果

5 結語

本文研究了帶有時域約束的自主巡航投餌船軌跡跟蹤問題，采用模型預測軌跡跟蹤控制器實現了投餌船在移動時域下對直線，雙移線和蛇形參考軌跡的跟蹤控制。在控制器的設計過程中，考慮了控制輸入受時變的約束影響，讓其能夠有效的處理系統時域約束以及非線性動力學問題。仿真結果表明，采用模型預測控制算法可以有效地解決帶時域約束的投餌船軌跡跟蹤控制問題，達到良好的跟隨性和穩定性。但仍然存在不足，對軌跡曲率實時變化下的跟蹤仍有一定的偏差，需在后續工作中進一步展開控制研究。