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

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

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

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

關于AUV 協同控制，現有研究采用了多種控制方法，如基于共識的方法、滑模控制法、反步控制法和人工勢場方法等.Yan等［7］設計了一種共識控制策略來解決具有模型不確定性和外部干擾的跟蹤共識問題，其中考慮了固定的和切換的通信拓撲.Gao等［8］提出了一種帶擾動觀測器的滑模控制方法，該方法能在有限時間內實現擾動下的全局穩定.Rout等［9］在Lyapunov 方法的基礎上開發了一種反步法來實現領航者-跟隨者隊列控制.Sahu 等［10］將模糊人工勢函數應用于多AUV 的控制，該方法在障礙物環境下優于傳統的數學方法.此外，許多研究者采用了自適應控制方法［11-12］.

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

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

本文的貢獻可總結如下：

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

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

1 AUV建模

1.1 運動學和動力學建模

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

深度控制子系統可以描述為［14，26］：

縱向速度控制子系統可以表述為［14］：

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

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

縱向速度子系統可以簡化為［14］：

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

以上述整體模型作為AUV 計算優化問題和更新系統狀態的模型是一種普遍使用的方法，為與本文提出的雙子系統DMPC 方法區別，稱之為整體系統DMPC方法.

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

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

1.2 通信拓撲建模

跟隨AUV 間的通信拓撲通常用有向圖GN={VN，εN，AN}來描述，VN={1，2，...，N}表示頂點集，即所有跟隨AUV 的索引；εN=VN×VN表示邊集，即各個跟隨AUV 之間的連接；AN是鄰接矩陣，代表AUV和鄰居的通信情況，表達式為：

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

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

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

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

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

2 AUV控制器設計

2.1 多AUV系統的控制目標

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

2.2 優化問題的設計

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

對于AUVi的深度控制子系統，定義優化問題Pd，i：

約束條件為：

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

對于AUVi的縱向速度控制子系統，定義優化問題Ps，i：

約束條件為：

其中，Fi是i的狀態約束，ls，i(·)是縱向速度控制子系統優化問題的代價函數，表示為：

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

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

2.3 雙子系統DMPC算法設計

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

表1 雙子系統DMPC算法Tab.1 The DMPC algorithm of two-subsystem approach

圖1 AUV數據流動圖Fig.1 Data flow diagram

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

3 仿真實驗驗證

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

表2 AUV的參數Tab.2 AUVs’parameters

圖2 通信拓撲Fig.2 Communication topology

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

約束條件為：

其中，Yi是AUVi的狀態約束，lu，i(·)是整體系統優化問題的代價函數，可表示為：

該方法與雙子系統DMPC 方法的區別在于更新狀態和建立預測優化問題都依賴于耦合模型（5），需在一個優化問題中求解兩類控制量.

3.1 仿真場景1

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

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

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

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

圖4 整體系統DMPC方法場景1仿真結果Fig.4 Simulation results in Scenario 1（Coupling model approach）

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

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

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

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

3.2 仿真場景2

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

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

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

4 結論

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