多AUV 分布式模型預(yù)測深度和縱向速度控制

邊有鋼，張俊杰，李崇康，徐彪，秦兆博，胡滿江?

（1.湖南大學(xué)機(jī)械與運載工程學(xué)院，湖南長沙 410082；2.湖南大學(xué)無錫智能控制研究院，江蘇無錫 214115）

自主水下航行器（Autonomous Underwater Ve?hicle，AUV）是一種高度智能的裝備，它不依賴駕駛員的操作，并且與母船沒有物理連接［1］.由于能夠攜帶各種設(shè)備在載人航行器無法到達(dá)的水域長時間工作，AUV 在水下救援［2］、資源勘探［3］、管道探測［4］等方面發(fā)揮著重要作用.隨著資源勘探和海底檢測等商業(yè)和軍事應(yīng)用的任務(wù)規(guī)模和復(fù)雜性不斷增加，多AUV 協(xié)同相較于單AUV 在經(jīng)濟(jì)性和可靠性上的優(yōu)勢越來越大［5-6］.因此，協(xié)同控制作為可以實現(xiàn)多AUV 的協(xié)調(diào)運動的控制方法，受到研究者的普遍關(guān)注.

關(guān)于AUV 協(xié)同控制，現(xiàn)有研究采用了多種控制方法，如基于共識的方法、滑?？刂品ā⒎床娇刂品ê腿斯輬龇椒ǖ?Yan等［7］設(shè)計了一種共識控制策略來解決具有模型不確定性和外部干擾的跟蹤共識問題，其中考慮了固定的和切換的通信拓?fù)?Gao等［8］提出了一種帶擾動觀測器的滑?？刂品椒?，該方法能在有限時間內(nèi)實現(xiàn)擾動下的全局穩(wěn)定.Rout等［9］在Lyapunov 方法的基礎(chǔ)上開發(fā)了一種反步法來實現(xiàn)領(lǐng)航者-跟隨者隊列控制.Sahu 等［10］將模糊人工勢函數(shù)應(yīng)用于多AUV 的控制，該方法在障礙物環(huán)境下優(yōu)于傳統(tǒng)的數(shù)學(xué)方法.此外，許多研究者采用了自適應(yīng)控制方法［11-12］.

對于AUV 的運動控制，Yao 等［13］引入了一種改進(jìn)的模型預(yù)測控制（Model Predictive Control，MPC）方法來解決深度控制問題，該方法中權(quán)重矩陣隨誤差變化而變化，但在Yao 的研究中沒有涉及到縱向速度的控制.在Zhou 等［14］的研究中，提出了一種基于優(yōu)選的最優(yōu)控制方法，可以根據(jù)控制器數(shù)據(jù)庫選擇最優(yōu)控制方式.Khodayari 等［15］提出了一種自適應(yīng)模糊PID 控制方法來實現(xiàn)自主水下航行器的深度、縱向速度和航向控制，與傳統(tǒng)的PID 控制器相比，該方法具有更好的魯棒性和動態(tài)特性.除此之外，強化學(xué)習(xí)［16］和滑?？刂疲?7］也被用于AUV 的運動控制.

在實際的編隊中，希望在保持低能耗的同時獲得較好的性能，同時系統(tǒng)的約束條件也需要嚴(yán)格滿足.然而，上述大多數(shù)方法都難以綜合處理這些問題.模型預(yù)測控制在處理系統(tǒng)約束和非線性動力學(xué)方面具有一定的優(yōu)勢［18］.繼承模型預(yù)測控制上述優(yōu)勢的分布式模型預(yù)測控制（Distributed Model Predic?tive Control，DMPC）可以給每一個單體都定義優(yōu)化問題，并且各單體之間獨立地求解優(yōu)化問題，而不是將全局優(yōu)化問題的計算集中在一個控制器中.相對于集中式模型預(yù)測控制，DMPC 可以通過局部信息實現(xiàn)全局協(xié)調(diào)，也大大提高了控制的實時性［19］，在地面車輛領(lǐng)域已經(jīng)得到了廣泛的應(yīng)用［20-21］.AUV 的動力學(xué)模型具有復(fù)雜、非線性特點，模型預(yù)測控制由于具有處理約束和非線性系統(tǒng)的優(yōu)勢，因此被廣泛應(yīng)用于水下航行器［22-23］.此外，由于可以根據(jù)實際需求設(shè)計代價函數(shù)，DMPC 在實際應(yīng)用中具有較大的靈活性.Li 等［22］提出了編隊跟蹤控制問題的滾動時域控制 法（Receding Horizon Control，RHC，也被稱為MPC），并對其可行性和穩(wěn)定性進(jìn)行了嚴(yán)格分析.但是在文獻(xiàn)［22］中要求每一個跟隨AUV 都可以與領(lǐng)航AUV 通信，隨著編隊規(guī)模增大，由于首尾距離增大，跟隨AUV 可能無法保證與領(lǐng)航AUV 的通信，因此該通信拓?fù)浣Y(jié)構(gòu)限制該方法的應(yīng)用場景為小規(guī)模的編隊控制.另外，阻礙DMPC 方法實際應(yīng)用的一個重要因素是求解完整非線性系統(tǒng)對應(yīng)的高維問題的復(fù)雜度，降低了控制器的實時性.

本文的貢獻(xiàn)可總結(jié)如下：

1）針對多AUV 的縱向速度與深度協(xié)同控制問題，考慮AUV 局部通信，設(shè)計了基于DMPC 的多AUV 縱向速度與深度控制方法.相比于LI 等［24］，該方法同時考慮縱向速度和深度的變化，實現(xiàn)縱向速度和深度的聯(lián)合優(yōu)化控制.

2）針對單AUV 縱向速度和深度控制系統(tǒng)的狀態(tài)耦合導(dǎo)致計算效率低的問題，提出了一種可以解耦兩個系統(tǒng)的雙子系統(tǒng)控制方法.在DMPC 框架下，縱向速度控制子系統(tǒng)和深度控制子系統(tǒng)相互共享參考軌跡信息，以實現(xiàn)多變量解耦.仿真結(jié)果表明，與整體系統(tǒng)方法相比，本文提出的雙子系統(tǒng)方法在減少計算量、提高實時性的同時具有滿意的控制效果.

1 AUV建模

1.1 運動學(xué)和動力學(xué)建模

AUV的六自由度動力學(xué)模型具有多變量耦合和非線性的特征［25］，給控制器設(shè)計帶來了很大的挑戰(zhàn).為了在模型精度、控制性能和計算效率之間達(dá)到更好的平衡，可以根據(jù)實際應(yīng)用場景對六自由度動力學(xué)模型進(jìn)行簡化.如Zhou 等［14］將六自由度動力學(xué)模型分為三個運動控制子系統(tǒng)：航向控制子系統(tǒng)、縱向速度控制子系統(tǒng)和深度控制子系統(tǒng).

深度控制子系統(tǒng)可以描述為［14，26］：

縱向速度控制子系統(tǒng)可以表述為［14］：

式中：|·|表示取絕對值；z、θ、q、u和w分別代表AUV的深度、俯仰角、俯仰角速度、縱向速度和垂向速度；下標(biāo)i表示序號為i的AUV 的索引；Iy為沿橫軸的轉(zhuǎn)動慣量；和為流體慣性水動力系數(shù)；M、qX|u|、uXwq和Mw為流體黏性水動力系數(shù)；Mδs為舵角水動力系數(shù)；zg和zb分別是AUV 重力中心和浮力中心的垂向距離；m為AUV 的質(zhì)量；假設(shè)水動力系數(shù)是勻質(zhì)、時不變的.W和B分別是重力和浮力；T和δs為子系統(tǒng)的輸入，分別代表縱向推力和舵角，需滿足約束：Ti∈Ti={Ti，min≤Ti≤Ti，max}和δsi∈Di={δsi，min≤δsi≤δsi，max}，Ti，max和Ti，min分別代表縱向推力的上下界，δsi，max和δsi，min分別代表舵角的上下界.

在實際情況中，許多AUV 并不為深度控制配備垂向推進(jìn)器，深度主要依靠舵角來控制，所以垂向速度可以假設(shè)為零，則深度控制子系統(tǒng)可以簡化為［14］：

縱向速度子系統(tǒng)可以簡化為［14］：

公式（3）和公式（4）通過縱向速度u和俯仰角θ耦合在一起，雙子系統(tǒng)DMPC 方法中兩個子系統(tǒng)可以互相交換耦合變量的參考軌跡序列，從而解耦兩個子系統(tǒng).將公式（3）和公式（4）整理為整體模型以作為雙子系統(tǒng)DMPC方法的對比：

以上述整體模型作為AUV 計算優(yōu)化問題和更新系統(tǒng)狀態(tài)的模型是一種普遍使用的方法，為與本文提出的雙子系統(tǒng)DMPC 方法區(qū)別，稱之為整體系統(tǒng)DMPC方法.

定義狀態(tài)變量xi=[zi，θi，qi]T∈R3×1和yi=[zi，θi，qi，ui]T∈R4×1作為AUVi的狀態(tài)，將公式（3）、（4）和（5）以時間Δt離散化，并表示如下：

其中，k為某一離散時刻，R為所有實數(shù)的集合.

1.2 通信拓?fù)浣?/h3>

跟隨AUV 間的通信拓?fù)渫ǔＳ糜邢驁DGN={VN，εN，AN}來描述，VN={1，2，...，N}表示頂點集，即所有跟隨AUV 的索引；εN=VN×VN表示邊集，即各個跟隨AUV 之間的連接；AN是鄰接矩陣，代表AUV和鄰居的通信情況，表達(dá)式為：

其中，N代表所有跟隨AUV 的數(shù)量，(j，i) ∈εN則aij=1，表示AUVi可以從AUVj獲得信息.

定義增廣有向圖GN+1={VN+1，εN+1}來描述領(lǐng)航AUV 和跟隨AUV 的通信.VN+1={0，1，...，N}表示頂點集，εN+1=VN+1×VN+1表示邊集.定義牽引矩陣P來描述跟隨獲取領(lǐng)航信息的情況：

其中，如果(0，i) ∈εN+1，則pi=1 表示AUVi能夠接受領(lǐng)航AUV的信息，否則pi=0表示AUVi不能接受領(lǐng)航AUV的信息.

定義AUVi可以接收到信息的AUV 的集合為Ii，定義可以接收到AUVi信息的AUV 的集合為Qi，其表達(dá)式分別為：

由于本文的研究重點為多AUV 的協(xié)同運動控制，此處假設(shè)AUV 的通信不存在延時和丟包現(xiàn)象.關(guān)于通信失效和延時方面的研究，可以參考Li 等［27］和Bernardo等［28］.

2 AUV控制器設(shè)計

2.1 多AUV系統(tǒng)的控制目標(biāo)

在本文考慮場景中，希望跟隨AUV 精確地跟蹤領(lǐng)航AUV 的運動，隨著時間趨近于無窮，跟隨AUV與領(lǐng)航AUV 之間的縱向速度和深度誤差應(yīng)收斂于零，即：

2.2 優(yōu)化問題的設(shè)計

令(·)i(k|t)表示在時間t之后第k個離散時刻的狀態(tài)（或輸入）和分別表示AUVi的假設(shè)狀態(tài)（或輸入）、預(yù)測狀態(tài)（或輸入）和最優(yōu)狀態(tài)（或輸入）.假設(shè)狀態(tài)（或輸入）來自上一時刻求得的最優(yōu)狀態(tài)（或輸入），用于和預(yù)測狀態(tài)相比較；預(yù)測狀態(tài)通過預(yù)測輸入序列單步遞推得到，作為待優(yōu)化變量；最優(yōu)輸入為優(yōu)化問題的解，第一個實際作用于AUV，余下的用于生成下一時刻的假設(shè)狀態(tài).

對于AUVi的深度控制子系統(tǒng)，定義優(yōu)化問題Pd，i：

約束條件為：

其中，公式（12）是對初始時刻狀態(tài)的約束，要求預(yù)測狀態(tài)的初值與當(dāng)前實際狀態(tài)值相等；公式（13）是對預(yù)測狀態(tài)的約束，要求預(yù)測狀態(tài)按照AUV 的動力學(xué)模型計算；公式（14）和（15）是對狀態(tài)和輸入的約束；公式（16）是對預(yù)測終端狀態(tài)的約束，要求終端狀態(tài)與鄰居終端參考軌跡終端狀態(tài)平均值相等.Xi表示狀態(tài)約束，Km：n表示集合[m，m+1，…，n-1，n]，n>m，NP表示預(yù)測時域，|Ii|表示求向量Ii的基.ld，i(·)是深度控制子系統(tǒng)優(yōu)化問題的代價函數(shù)，表示為：

對于AUVi的縱向速度控制子系統(tǒng)，定義優(yōu)化問題Ps，i：

約束條件為：

其中，F(xiàn)i是i的狀態(tài)約束，ls，i(·)是縱向速度控制子系統(tǒng)優(yōu)化問題的代價函數(shù)，表示為：

縱向速度控制子系統(tǒng)的約束條件和代價函數(shù)設(shè)計的思路和深度控制子系統(tǒng)一致，具體意義可參考上文的敘述.

對于雙子系統(tǒng)DMPC 方法，一方面，在DMPC 框架下，多AUV 系統(tǒng)中每個單體AUV 都和自己的鄰居通信，獲取必要信息.另一方面，單體AUVi的縱向速度控制子系統(tǒng)的參考狀態(tài)近似地作為下一時刻深度控制子系統(tǒng)中縱向速度的真實值，深度控制子系統(tǒng)的參考狀態(tài)近似地作為縱向速度控制子系統(tǒng)中俯仰角的真實值，以實現(xiàn)預(yù)測模型的分別迭代計算，如式（13）和（20）所示.由此在DMPC 框架下，各個單體獨立地求解優(yōu)化問題，同時每個單體的兩個子系統(tǒng)也可獨立地求解優(yōu)化問題Pd，i和Ps，i.

2.3 雙子系統(tǒng)DMPC算法設(shè)計

雙子系統(tǒng)DMPC 算法（表1）設(shè)計的目的在于實現(xiàn)兩個子系統(tǒng)之間的數(shù)據(jù)傳輸、獨立求解優(yōu)化問題以及DMPC 框架下各個AUV 之間的協(xié)同.如圖1 所示，由于每一時刻都需要計算優(yōu)化問題，所以在每一個采樣時刻t（t>0），AUVi的深度和縱向速度兩個控制子系統(tǒng)分別傳輸參考軌跡中的俯仰角和縱向速度序列到對方，實現(xiàn)兩個子系統(tǒng)解耦，這兩個子系統(tǒng)在計算方面是獨立的，并且可以部署于同一硬件設(shè)備，從而擁有相同的CPU 時鐘與存儲器，可直接進(jìn)行同步的數(shù)據(jù)訪問.同時，AUVi發(fā)送自身的參考軌跡給鄰居，接收鄰居的參考軌跡，從而兩個子系統(tǒng)分別計算優(yōu)化問題，得到最優(yōu)控制輸入序列和將和作為實際控制輸入，其余最優(yōu)輸入用于生成下一時刻的參考軌跡.由于領(lǐng)航穩(wěn)定狀態(tài)下是以零舵角和較小的縱向速度航行，所以令=0 和=0以使控制系統(tǒng)穩(wěn)定［18］.

表1 雙子系統(tǒng)DMPC算法Tab.1 The DMPC algorithm of two-subsystem approach

圖1 AUV數(shù)據(jù)流動圖Fig.1 Data flow diagram

此處只給出了雙子系統(tǒng)DMPC 算法，整體系統(tǒng)DMPC 算法和雙子系統(tǒng)算法僅在子系統(tǒng)傳輸數(shù)據(jù)、求解優(yōu)化和狀態(tài)更新上有差異，其余的過程一致，可以參考Li等［24］.

3 仿真實驗驗證

為驗證雙子系統(tǒng)DMPC 方法的有效性，設(shè)計了兩個仿真場景，并在場景1 下對比了雙子系統(tǒng)DMPC方法與整體系統(tǒng)DMPC 方法的計算時間和控制性能.圖2 表示AUV 之間的通信拓?fù)?，? 列舉了模型與控制參數(shù)，部分參考自Lapierre 等［29］的工作.仿真計算平臺為PC（CPU：Intel Core i5-7300HQ 2.50GHz，GPU：Nvidia GeForce 1050，RAM 8 GB）.特別地，這里采用MATLAB 的FMINCON 函數(shù)進(jìn)行原始優(yōu)化問題的直接求解.這里未對求解過程進(jìn)行單獨優(yōu)化，但對兩種方法計算效率的對比仍然是具有意義的.圖3-5 中每一幅圖中（a）～（d）表示各個AUV 狀態(tài)隨時間的變化，（f）～（i）各跟隨AUV與領(lǐng)航AUV的狀態(tài)誤差隨時間的變化，（e）和（j）分別代表跟隨AUV 的舵角和縱向推力隨時間的變化.

表2 AUV的參數(shù)Tab.2 AUVs’parameters

圖2 通信拓?fù)銯ig.2 Communication topology

為了在計算時間和控制效果方面將雙子系統(tǒng)DMPC方法與整體系統(tǒng)DMPC 方法作對比，定義優(yōu)化問題Pu，i：

約束條件為：

其中，Yi是AUVi的狀態(tài)約束，lu，i(·)是整體系統(tǒng)優(yōu)化問題的代價函數(shù)，可表示為：

該方法與雙子系統(tǒng)DMPC 方法的區(qū)別在于更新狀態(tài)和建立預(yù)測優(yōu)化問題都依賴于耦合模型（5），需在一個優(yōu)化問題中求解兩類控制量.

3.1 仿真場景1

仿真場景1 驗證多AUV 在速度頻繁變化與下潛時的跟蹤性能.所有跟隨AUV 在隨機(jī)初始誤差下跟蹤領(lǐng)航AUV，其俯仰角速度和縱向速度設(shè)定為：

如圖3 所示，在雙子系統(tǒng)DMPC 方法下，所有跟隨AUV的深度［圖3（a）（f）］、縱向速度［圖3（b）（g）］、俯仰角［圖3（c）（h）］和俯仰角速度［圖3（d）（i）］均可跟蹤領(lǐng)航AUV，在狀態(tài)穩(wěn)定后，所有跟隨AUV 和領(lǐng)航AUV 的誤差趨近于零，滿足控制目標(biāo).因此，雙子系統(tǒng)DMPC方法的控制性能符合要求.

圖3 雙子系統(tǒng)DMPC方法場景1仿真結(jié)果Fig.3 Simulation results in Scenario 1（Two-subsystem approach）

圖4 為整體系統(tǒng)DMPC 方法場景1 下的仿真結(jié)果.在整體系統(tǒng)DMPC方法下，所有跟隨AUV和領(lǐng)航AUV的狀態(tài)誤差趨近于零，也滿足控制性能要求.

圖4 整體系統(tǒng)DMPC方法場景1仿真結(jié)果Fig.4 Simulation results in Scenario 1（Coupling model approach）

為比較兩種方法的計算效率和控制效果，在相同的條件下對兩種方法進(jìn)行了11 次仿真，兩種方法的總計算時間如表3 所示.雙子系統(tǒng)DMPC 方法的平均計算時間為636.13 s，整體系統(tǒng)DMPC 方法的平均計算時間為2 384.11s.比較整體系統(tǒng)DMPC 方法和本文提出的雙子系統(tǒng)DMPC 方法，由于雙子系統(tǒng)DMPC 方法解耦了兩個子系統(tǒng)，分別求解兩類控制量，使單次求解優(yōu)化問題的變量維度變小，計算時間由此降低了73.3%.此外，在本仿真實驗中，兩個優(yōu)化問題串行求解，而在實際應(yīng)用中，雙子系統(tǒng)DMPC 方法可通過并行求解兩個子系統(tǒng)的優(yōu)化問題，進(jìn)一步提高計算效率.

表3 兩種方法的計算時間比較Tab.3 Computational time comparison between the two approaches

為比較兩種方法的控制性能，定義收斂時間tc，z和tc，u為所有的跟隨AUV 與領(lǐng)航AUV 縱向速度和深度誤差第一次收斂到零的時間.兩種方法的收斂時間如表4 所示.采用雙子系統(tǒng)DMPC 方法和整體系統(tǒng)DMPC 方法的平均深度收斂時間分別為30.7 s 和12.0 s，平均縱向速度收斂時間分別為3.4 s 和16.2 s.在雙子系統(tǒng)DMPC 方法中，由于假設(shè)狀態(tài)與真實的未來狀態(tài)存在偏差，會對計算精度有一定影響，但是模型預(yù)測控制本身具有魯棒性特點，并且從仿真結(jié)果上看，雙子系統(tǒng)DMPC 方法具有更好的縱向速度控制性能，雖深度控制性能有所下降，考慮到計算時間的大幅降低（73.3%），在某些應(yīng)用情況下適當(dāng)犧牲深度控制性能是可以接受的.

表4 兩種方法的收斂時間比較Tab.4 Convergence time comparison between the two approaches

3.2 仿真場景2

仿真場景2 驗證多AUV 在行進(jìn)過程中避開障礙物的能力.跟隨AUV 在隨機(jī)初始誤差下跟蹤動態(tài)領(lǐng)航AUV，其縱向速度為恒定值0.5m/s，俯仰角速度為：

在垂直平面上做正弦運動是檢驗深度控制子系統(tǒng)控制性能的一種有效方法.如圖5 所示，跟隨AUV 在第一次下潛（t<20s）時與領(lǐng)航AUV 在深度上有誤差，但是在第一次上升（25s

圖5 雙子系統(tǒng)DMPC方法場景2仿真結(jié)果Fig.5 Simulation results in Scenario 2（Two-subsystem approach）

4 結(jié)論

本文研究了多AUV 的深度和縱向速度控制問題，將多變量耦合非線性復(fù)雜模型分解為兩個子系統(tǒng)，即縱向速度控制子系統(tǒng)和深度控制子系統(tǒng).基于DMPC 方法，多AUV 系統(tǒng)中各個單AUV 的兩個控制子系統(tǒng)相互傳輸必要的數(shù)據(jù)，即俯仰角和縱向速度，使得兩個子系統(tǒng)解耦，從而獲得更好的計算效率.然后，設(shè)計了局部開環(huán)優(yōu)化問題，并分別求解優(yōu)化問題得到最優(yōu)控制輸入.通過不同場景下的數(shù)值模擬，驗證了該方法的有效性.本研究的一個延伸是嚴(yán)格分析所提出方法的穩(wěn)定性.另外，傳遞給另一個子系統(tǒng)的假設(shè)軌跡只迭代一次，應(yīng)多次迭代假設(shè)軌跡，直到誤差在一個合適的范圍，從而更好地平衡控制效果與計算效率.