基于改進MPC 的分布式大間距多車編隊控制研究（上）

鄭屹天，郭志強，趙建新，張宏映

（北方自動控制技術研究所，太原 030006）

0 引言

隨著無人平臺及智能技術發展，多平臺編隊控制在運輸、偵察、突擊等方面有著廣泛應用，多智能體系統（MAS）的編隊控制成為重要課題［1-2］，純無人編隊方法已經應用于無人機［3-5］、無人艦艇［6-8］等空中、海上戰場領域。然而，對于地面領域［9］，其地形及電磁場環境復雜，平臺視野受限且通信網絡并不穩定，純無人車編組自主分析、決策能力不足，由控制中心遠程控制的一般編隊方法少有實現。

常用的編隊控制方法有領航跟隨法、虛擬結構法和基于行為法［10-14］，其中，領航跟隨法因其簡單快速、控制穩定被廣泛運用。文獻［15-16］可以看出MPC 方法在軌跡跟蹤方面能夠處理其他方法難以解決的多約束、非線性等問題，這為編隊控制提供了很好的設計思路，很多學者在車輛編隊控制中采用MPC 方法設計控制器。文獻［17］設計了一種基于樣條曲線的分布式MPC 控制器，并通過交替方向乘子法解算，完成了高實時性的穩定編隊控制，然而該方法并不適用于一般的非完整約束模型。文獻［18］設計了一種分布式非線性MPC 控制器，采用GPNN算法求解控制器優化問題，完成了基于領航跟隨法的編隊控制，然而，其虛擬領航者軌跡同樣是事先規劃的已知路徑。上述文獻實驗中，領航者與跟隨者編隊間距較小，且系統工況接近期望軌跡。

值得注意的是，系統工作點與期望軌跡相近的假設條件在實際工況中并不滿足，同時編隊軌跡并不總是預先規劃的。因此，已有文獻中的方法無法直接應用。進而，本文針對多車編隊問題，考慮到編隊間距大、系統工作點遠離期望軌跡等約束，設計了分布式改進MPC 方法，有效實現了大間距多車編隊的穩定運行。

本文主要貢獻點為：針對系統工作點遠離期望軌跡的問題，修正線性化展開點優化預測模型，使系統能夠在狀態誤差大的情況下收斂，提升了系統的保守性，同時減輕了對軌跡規劃的依賴，降低了控制器設計的復雜度，有效地增強了實時軌跡跟蹤性能。

1 問題描述

本章首先介紹車輛二階2 自由度非完整約束模型，然后根據線性MPC 的特點對車輛模型進行線性離散化處理，最后引出多車編隊控制問題，得出本文假設條件下的編隊控制目標。

1.1 車輛2 自由度非完整約束模型

車輛運動學模型如圖1 所示，規定逆時針轉角為正，順時針轉角為負，全局坐標系XOY 采用笛卡爾坐標系，（x，y，φ，v）分別為車輛的后軸軸心橫、縱坐標及車輛的橫擺角和速度，a 為車輛加速度，δ 為車輛前輪偏角，L 為車輛軸距長度，ρ 為車輛轉彎半徑。

圖1 車輛運動學模型Fig.1 Dynamic model of the vehicle

假設車輛在運動過程中不存在橫向滑動，考慮平面上的n 個結構相同的車輛，即軸距L 相同：

對于任意第i 個車輛，根據運動學關系及幾何關系可得二階2 自由度非完整約束運動學模型方程：

1.2 模型線性離散化

非線性模型能夠精確地預測模型的輸出響應，但非線性模型預測控制器（NMPC）計算量大，實時性能較差。對模型線性化處理，設計線性模型預測控制器（LMPC）能夠提高控制器運算速度，滿足編隊控制的實時性需求。

設定車輛i 的追蹤軌跡為Zir，若參考系統能夠完全追蹤Z ，參考系統在任意時刻滿足：

式（8）形式為連續微分方程，無法直接用于MPC控制器，因此，必須進行離散化處理。在任意k 時刻，對式（8）進行前項歐拉離散化：

式中，T 為采樣周期，整理式（9），狀態量誤差模型的離散狀

1.3 編隊控制問題描述

本文采用基于距離角度（l-φ）的領航跟隨法。如圖2 所示，l 為領航者與跟隨者間相對距離（為簡化計算，以車輛后軸軸心為車輛位置點），θ 為兩者距離連線方向與領航者橫擺角方向夾角。

圖2 編隊結構Fig.2 Formation structure

式中，ZL表示領航者實際位置，C（·）為隊形信息從極坐標系到全局坐標系的映射。

對編隊MAS 有如下假設：

假設1：編隊行進過程中，編隊通信不受限制。

假設2：編隊中每個車輛都能通過通信拓撲與自身鄰居通信并傳遞位姿信息。

假設3：領航者跟隨一條預設軌跡，跟隨者跟蹤領航者實時位置與隊形信息解算得出的軌跡。

上述假設條件下，編隊控制目標為跟隨者與領航者之間的姿態偏差收斂于隊形信息。由式（11）得到局部信息條件下的編隊控制目標：

2 控制器設計

MPC 按預測模型類型可分為LMPC 和NMPC兩大類。本文采用的運動學模型為典型的非線性系統，NMPC 控制更加精確，但約束優化中將引入大量的非線性約束，求解時間將遠大于線性優化問題。LMPC 約束條件為線性約束，計算量小，速度快，更契合實際運用需求。因此，本文控制器設計采用LMPC 方法。

2.1 MPC 原理

MPC 本質是一種最優化控制［21］。如圖3 所示，MPC 基于系統模型與任意k 時刻系統狀態預測未來一段時間（從k+1 到k+Np時刻）內所有的系統狀態，根據設計好的約束條件與代價函數優化求解出第k 到第k+Nc-1 時刻的預測控制量，將預測控制量序列中第1 組控制量作用于受控對象；預測時間窗口Np隨時刻k 滾動，在每一時刻重復以上過程完成控制量求解，同時對系統實際輸出與期望值作比較，反饋于約束優化環節，防止系統輸出偏離期望值，經過反復地在線滾動優化，實現系統輸出對期望值的跟蹤。

圖3 MPC 原理Fig.3 MPC principle

2.2 MPC 控制算法設計

式（10）為狀態量線性誤差模型，控制量在實際物理意義下可以在一定范圍內進行突變，不需要重新設計新的狀態量表示未來的預測輸出。

式中，Q，F，R 分別為系統狀態誤差權重、控制量誤差權重和終端誤差權重。

式（14）代入式（15）：

記狀態量個數為NZ，控制量個數為NU，則為：

整理式（16）：

實際物理系統的速度不會無限增加，瞬時加速度和前輪偏角有一定的范圍限制。對應狀態量與控制量的約束條件：

另外，前輪的轉動受到傳動限制，不可能產生較大突變，同時為了使轉向更加平滑，增加前輪偏角增量約束：

結合式（19）和式（20）的不等式約束，求解式（18）的最小化問題，這將控制問題轉換為帶約束的二次規劃問題，用于求解Np內的預測控制量序列U_ki。

考慮到算法復雜度和仿真實驗的算法實現，二次規劃問題運用內點法求解。

內點法的基本思想是將原問題轉換為只有等式約束和簡單不等式約束的等價問題，求解等價問題的一階最優性條件并進行擾動，運用牛頓法求解KKT條件，得到最優解的搜索方向，最后求取最優解。在MATLAB 仿真平臺中已有成熟的quadprog 算法完成二次規劃問題的求解，故不再贅述其具體算法。

2.3 線性化展開點修正

根據線性離散化過程可知，A?和B?動態變化，依賴任意k 時刻實時的期望值更新，與系統狀態無關。當車輛與期望軌跡的狀態誤差較小時，模型的狀態量仍能收斂于期望軌跡；當狀態誤差過大，預測模型無法預測原非線性模型的未來變化趨勢。如下頁圖4 所示，車輛實際狀態與參考點狀態的橫擺角完全不同，基于參考點展開的線性化模型所設計的MPC控制器，在短時間內使實際的車輛模型速度方向趨于水平，不指向期望點，根據設定好的權重矩陣 ，R 和預測時域Np，MPC 控制器將陷于局部最優解中無法跳出。

圖4 車輛模型遠離期望軌跡Fig.4 The vehicle model away from the desired trajectory

基于上述問題，對車輛模型遠離參考軌跡情形下的參考點作出修正：

式（21）的修正方向即為圖4 中藍色點劃線方向。狀態誤差較小時，φir切換回實際的參考點狀態。

2.4 MPC 分布式化

編隊內每個個體都設計單獨的MPC 控制器，能夠將編隊控制問題拆分為單個個體對期望軌跡的跟蹤問題，減小控制器運算負擔，提高編隊系統的實時性。

在編隊控制中，領航者跟蹤預設軌跡，跟隨者個體i 的期望軌跡Zir是領航者狀態生成的實時軌跡。因此，只需將領航者實時狀態作為跟隨者的期望點，并將其代入式（18），即可完成單個MPC 控制器的期望設計。

整理式（12）：

編隊目標與MPC 代價函數的優化目標一致，結合式（12）、式（22），式（15）的第1 部分Ja與第3 部分Jc變形為：

分布式MPC 編隊控制算法流程如圖5 所示。

圖5 編隊控制算法流程圖Fig.5 Flowchart of formation control algorithm

3 系統仿真及分析

為驗證所提編隊控制方法的有效性，在Matlab中進行仿真實驗。選取1 輛領航者和5 輛跟隨者完成編隊跟蹤控制仿真。

編隊仿真結果如圖6～圖8 所示。

圖6 領航者仿真結果Fig.6 Simulation results of the navigator

圖6（b）、圖6（d）表明單個車輛跟隨遠距離軌跡在10 s左右狀態量誤差收斂為0，能夠在一般軌跡條件下實現快速跟蹤。

圖7（b）、圖7（d）表明在15 s 左右編隊內跟隨者位置狀態誤差第1 次收斂為0，20 s 時隊形間距改變后5 s～8 s 內位置狀態誤差第2 次收斂為0，30 s時的軌跡變換與35 s 時的隊形切換帶來的狀態誤差基本可以忽略不計。這3 個時間點系統對誤差的響應表明，系統跟蹤誤差收斂耗時的原因在于遠距離軌跡跟蹤，軌跡變換情況下系統重新收斂的時間可以忽略不計，隊形等間距切換情況下系統重新收斂的時間在3 s～5 s。

圖7 跟隨者仿真結果Fig.7 The simulation results of the followers

圖8 編隊隊形仿真結果Fig.8 The simulation results of the formation rank

圖8 顯示編隊行進中的實時隊形，結合圖6、圖7，編隊在短時間內完成特定隊形并保持，且在軌跡變換和隊形切換過程中基本不受影響，驗證了本文所設計方法在遠距離、大間距編隊控制問題上的有效性。

4 結論

本文設計了一種分布式改進MPC 控制方案，能夠有效地解決遠離期望軌跡約束下大間距多車編隊控制問題。與已有文獻相比，通過直接修正線性化展開點設計狀態誤差過大時系統的期望狀態，不依賴軌跡優化直接跟蹤實時遠距離期望軌跡，能夠降低控制器復雜度，提升系統保守性。

需要指出的是，本文考慮的是標準工況，實際上編隊通信拓撲并不總是全連通的，因此，下文將根據編隊內各跟隨者狀態信息設計基于鄰居信息的期望軌跡，并針對一般通信情況進行設計處理，以完成通信受限情況下的編隊控制。