基于模型預測控制的移動回收站對接控制算法

齊貝貝，宋 敏，王林林，張亞維

（西安航空職業技術學院 航空維修工程學院，西安 710089）

如今人們發現越來越多的資源來源于海洋，因此探索海洋對于獲取能源和相關資源意義重大。自主水下機器人AUV 是一種智能化的海洋探測設備，目前已經成為探索海洋資源的重要工具之一。由于AUV 自身條件受限，不能長時間作業，需要及時進行能源補充、數據下載等任務，因此在自主水下機器人的控制過程中，回收控制是重要的關鍵技術之一[1]。目前在AUV 回收控制的研究中，絕大多數回收站都是靜止的，移動回收站的AUV 回收控制研究很少。在整個回收過程中，由于回收站受到海流的影響，AUV 與回收站都處于運動狀態。隨著AUV 與移動回收站相互接近，兩者也會產生相互干擾，AUV 不僅要克服各種復雜約束限制，而且要抵抗外界干擾。所以，移動回收站AUV 回收對接研究具有一定難度[2]。

關于自主水下機器人的水下回收對接問題，國內外的廣大學者和研究人員做了大量的研究。為了實現在未知的海流干擾下的回收對接，文獻[3]提出了一種基于魯棒模糊理論的水下航行器對接控制算法，并通過MATLAB 仿真驗證了該算法；針對REMUS-100 型AUV，文獻[4]設計了一種基于超短基線USBL 定位的回收對接系統，并且成功地完成了對接試驗。

關于水下機器人的自主回收對接，國內也有相關的研究成果。文獻[5]針對動基座水下航行器回收對接控制，采用了模型預測控制算法，同時滿足多種復雜的約束條件，通過仿真驗證了算法的有效性。為了滿足AUV 水下對接系統的需求，文獻[6]建立了AUV 的動力模型，將回收對接控制分為追蹤回歸和直線對接2 個階段，通過仿真研究，驗證了控制策略的可行性。文獻[7]考慮到水下運動母船與AUV 的自主回收控制問題，根據有限時間同步理論，設計了一種能夠在有限時間內被運動母艇回收的滑?？刂破鳎⒃诜抡嬷序炞C了該控制器的有效性。

本文在之前研究的基礎上，考慮了實際應用場景下，由海流造成的回收站移動的問題，提出了基于MPC 的AUV 移動回收站對接控制算法。針對安裝有應答器的移動回收站，采用基于MPC 的對接控制算法對AUV 進行運動控制，其中重點研究了狀態約束中的可見性約束和自身約束中的輸入約束。通過預測控制器的解算給出最優控制輸入，在滿足約束條件的同時，實現AUV 的回塢導引控制。最后，在回收站移動的情況下，通過仿真證明了所設計的對接控制算法是有效的。

1 AUV 建模

AUV 的6 自由度空間運動方程是一個較為復雜的非線性系統。在工程實踐中，為了研究AUV 的運動規律，常將6 自由度空間運動分解為垂直面運動與水平面運動兩部分，同時忽略它們之間的耦合，并假設橫滾運動在重力的恢復力矩作用下是穩定的。注意到本文研究的AUV 的回塢過程發生在海面上，因此本文主要研究欠驅動AUV 水平面運動的控制問題，下面給出欠驅動AUV 在水平面內的3 自由度運動模型。

式中：m11，m22和m33為包含附加質量的慣性系數；d11，d22和d33為阻尼系數；X，Y 和N 為控制力和力矩。

2 回收系統

回收對接系統如圖1所示，將USBL 的收發器安裝在AUV 的頭部，2 個應答器安裝在回收站的兩側。為了描述AUV 的運動，定義了對接坐標系和體坐標系?；厥照旧系? 個應答器的中點作為對接坐標系的原點，記為原點O，沿著x 軸的方向為回塢的方向，y 軸與x 軸垂直；AUV 的浮心作為體坐標系的原點，記為原點OB，AUV 的縱軸為xB軸，并指向運動的前方。

圖1 AUV 回收對接系統圖Fig.1 Diagram of AUV docking system

3 回塢導引模型預測控制設計

模型預測控制算法在設計時就有約束項，因此在處理帶有約束的控制問題時，較其他算法具有優越性。本文提出的基于模型預測控制的AUV 回收控制算法中，重點研究了約束優化問題，模型預測控制器解算出的最優控制輸入在滿足約束的同時，完成AUV 在水平面內的回收。

在欠驅動自主水下機器人AUV 的水平面運動方程中，x= [x yψ]T是AUV 的廣義坐標矢量，v＝N為系統的輸入量。

在欠驅動AUV 的水平面回收控制問題中，利用模型預測控制算法設計回收對接預測控制器。在時間間隔［t，t+TP］內，計算與系統狀態x（t）有關的代價函數。然后最小化代價函數從而得到最優的控制輸入，并且將最優的控制輸入作用于在比預測時域TP短的控制時域Tc上。

本文的代價函數選擇了最簡單的二次型，處理系統約束優化問題的代價函數為[9]

式中：L（x，v）是代價函數；M（x）是終端代價函數；Q和R 分別為狀態代價權重矩陣和輸入代價權重矩陣；P 為終端狀態代價權重矩陣；xd= [xdydψd]T為AUV 期望的廣義坐標矢量。

王霞在知道到這件事情后，這次來皮山結親，專門看望了這個特殊家庭，并為孩子帶來一些學習用品和冬日御寒的衣服，為老人帶來一些禮品。王霞表示，她會一直關心這兩個孩子，一起面對困難，讓孩子健康成長……

然后求解系統約束優化問題：

同時滿足下列2 個約束條件：

式中：v∈K 為系統的輸入約束；x（ti）∈Ω 為系統的狀態約束。本文在下一節中詳細給出這2 種約束的處理。

在時刻ti，模型預測控制MPC 算法包括以下幾個步驟[10]，如圖2所示。

圖2 MPC 算法示意圖Fig.2 Schematic diagram of MPC algorithm

（1）測量ti時刻系統的坐標x（ti）。

（2）通過在線求解系統約束優化問題（3）從而得到開環的優化控制輸入［ti，ti+TP］。

（3）把控制時域Tc內的第一個輸入v*，即［ti，ti+Tc］中的第一個控制輸入施加給系統。

（4）在下一個采樣時間ti=ti+Tc，重復上述優化問題從而形成模型預測控制律。

4 約束系統

在移動回收站AUV 對接回收過程中，存在著多種復雜的約束。本文主要研究的約束問題是狀態約束中的可見性約束和自身約束中的輸入約束。

4.1 可見性約束

狀態約束中的可見性約束指的是USBL 對應答器可見性的約束[11]，即在AUV 回塢過程中，回收站要始終處在USBL 的工作扇面之內。系統的可見性約束如圖3所示，USBL 收發器的可視角為α，可視角是關于xB軸對稱的，回收站上應答器的位置范圍為［-yT，yT］，USBL 的工作區域為［ f2，f1］?；厥照旧蠎鹌鞯奈恢迷赨SBL 的工作區域內，即［-yT，yT］?［f2，f1］。將可視角α 平移到AUV 的浮心，與回收坐標系的y 軸交于A′和B′兩點，USBL可視范圍變為［f2′，f1′］。由直角三角形A′O′OB和B′O′OB得：

圖3 水平面內AUV 回塢可見性約束示意圖Fig.3 Schematic diagram of docking visibility constraints of AUV in horizontal plane

顯然，f2′≤f2，f1′≥f1。因此，f2′≤-yT，f1′≥yT，即：

綜上，可將可見性約束寫為如下3 個不等式：

4.2 輸入約束

由于自主水下機器人AUV 本身硬件性能的原因，所以存在自身約束。本文中主要研究的自身約束是輸入力矩約束，可以表示為

因此，求解約束優化問題：

滿足約束條件：

目標函數為

式中：L（x，v），M（x）和z 的定義與式（2）中的定義是相同的。

5 仿真驗證

為了驗證提出的基于模型預測控制的移動回收站對接控制算法，本文采用REMUS AUV 模型參數進行仿真研究。仿真中，設AUV 起始點坐標為（-8，-10），初始航向角為45°，前向速度為1 m/s，回收站的初始位置為（-1，0），受到海流的干擾，以0.05 m/s 速度沿y 軸正向運動，期望的入塢方向為0°。正定矩陣Q∈R3×3，Q=diag（［1 300 500］），R=1?？刂戚斎隢∈［-100，100］，可視角α＝120°，即可視范圍為［-60，60］。在回塢過程中考慮了狀態約束中的可見性約束和自身約束中的輸入約束，求解代價函數得到最優的控制輸入。通過仿真得到AUV 預測控制回塢軌跡如圖4所示，從圖中可以看出在模型預測控制下，AUV 能夠以較光滑的軌跡、較短的時間航行到移動回收站。AUV 回收對接中航向角變化如圖5所示。AUV 回收對接中控制輸入變化如圖6所示，由圖可見，控制輸入不超過100 N·m 滿足輸入約束。仿真結果驗證了算法的有效性。

圖4 AUV 模型預測控制回塢軌跡Fig.4 AUV docking trajectory with MPC

圖5 AUV 模型預測控制回塢航向角變化圖Fig.5 Yaw angle of AUV docking with MPC

圖6 AUV 預測控制回塢控制輸入變化圖Fig.6 Control input of AUV docking with MPC

6 結語

本文針對自主水下機器人AUV 回收對接控制問題，提出基于模型預測控制的移動回收站對接控制算法?；厥照驹诤Ａ鞯淖饔孟掳l生移動，采用模型預測控制MPC 算法設計回收對接預測控制器，來完成回收對接。在MPC 算法中，重點研究了狀態約束中的可見性約束和自身約束中的輸入約束。最后，通過采用REMUS AUV 的模型參數進行仿真，仿真結果證明了自主水下機器人的移動回收站對接控制算法是有效的。