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

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

（西安航空職業(yè)技術(shù)學(xué)院 航空維修工程學(xué)院，西安 710089）

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

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

關(guān)于水下機(jī)器人的自主回收對接，國內(nèi)也有相關(guān)的研究成果。文獻(xiàn)[5]針對動基座水下航行器回收對接控制，采用了模型預(yù)測控制算法，同時滿足多種復(fù)雜的約束條件，通過仿真驗證了算法的有效性。為了滿足AUV 水下對接系統(tǒng)的需求，文獻(xiàn)[6]建立了AUV 的動力模型，將回收對接控制分為追蹤回歸和直線對接2 個階段，通過仿真研究，驗證了控制策略的可行性。文獻(xiàn)[7]考慮到水下運動母船與AUV 的自主回收控制問題，根據(jù)有限時間同步理論，設(shè)計了一種能夠在有限時間內(nèi)被運動母艇回收的滑模控制器，并在仿真中驗證了該控制器的有效性。

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

1 AUV 建模

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

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

2 回收系統(tǒng)

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

圖1 AUV 回收對接系統(tǒng)圖Fig.1 Diagram of AUV docking system

3 回塢導(dǎo)引模型預(yù)測控制設(shè)計

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

在欠驅(qū)動自主水下機(jī)器人AUV 的水平面運動方程中，x= [x yψ]T是AUV 的廣義坐標(biāo)矢量，v＝N為系統(tǒng)的輸入量。

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

本文的代價函數(shù)選擇了最簡單的二次型，處理系統(tǒng)約束優(yōu)化問題的代價函數(shù)為[9]

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

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

然后求解系統(tǒng)約束優(yōu)化問題：

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

式中：v∈K 為系統(tǒng)的輸入約束；x（ti）∈Ω 為系統(tǒng)的狀態(tài)約束。本文在下一節(jié)中詳細(xì)給出這2 種約束的處理。

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

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

（1）測量ti時刻系統(tǒng)的坐標(biāo)x（ti）。

（2）通過在線求解系統(tǒng)約束優(yōu)化問題（3）從而得到開環(huán)的優(yōu)化控制輸入［ti，ti+TP］。

（3）把控制時域Tc內(nèi)的第一個輸入v*，即［ti，ti+Tc］中的第一個控制輸入施加給系統(tǒng)。

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

4 約束系統(tǒng)

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

4.1 可見性約束

狀態(tài)約束中的可見性約束指的是USBL 對應(yīng)答器可見性的約束[11]，即在AUV 回塢過程中，回收站要始終處在USBL 的工作扇面之內(nèi)。系統(tǒng)的可見性約束如圖3所示，USBL 收發(fā)器的可視角為α，可視角是關(guān)于xB軸對稱的，回收站上應(yīng)答器的位置范圍為［-yT，yT］，USBL 的工作區(qū)域為［ f2，f1］。回收站上應(yīng)答器的位置在USBL 的工作區(qū)域內(nèi)，即［-yT，yT］?［f2，f1］。將可視角α 平移到AUV 的浮心，與回收坐標(biāo)系的y 軸交于A′和B′兩點，USBL可視范圍變?yōu)椋踗2′，f1′］。由直角三角形A′O′OB和B′O′OB得：

圖3 水平面內(nèi)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 輸入約束

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

因此，求解約束優(yōu)化問題：

滿足約束條件：

目標(biāo)函數(shù)為

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

5 仿真驗證

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

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

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

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

6 結(jié)語

本文針對自主水下機(jī)器人AUV 回收對接控制問題，提出基于模型預(yù)測控制的移動回收站對接控制算法。回收站在海流的作用下發(fā)生移動，采用模型預(yù)測控制MPC 算法設(shè)計回收對接預(yù)測控制器，來完成回收對接。在MPC 算法中，重點研究了狀態(tài)約束中的可見性約束和自身約束中的輸入約束。最后，通過采用REMUS AUV 的模型參數(shù)進(jìn)行仿真，仿真結(jié)果證明了自主水下機(jī)器人的移動回收站對接控制算法是有效的。