參數自優化的有人與無人車輛編隊魯棒模型預測控制

宋佳睿，陶剛，李德潤，臧政，吳紹斌，龔建偉

(1.北京理工大學 機械與車輛學院，北京 100081;2.中汽研汽車檢驗中心(天津) 有限公司，天津 300300)

0 引言

近年來，為實現無人裝備集群化、規模化和可指揮化，無人裝備集群行駛在軍事領域中得到了越來越多的應用［1］。當前，有人與無人車輛編隊領航跟蹤控制作為無人裝備集群行駛的一項關鍵技術，其發展也被越來越多的學者關注。車輛編隊控制可以分為領航-跟隨法［2］、行為控制法［3］、滑模控制法［4-5］、魯棒控制方法［6］、模糊控制法［7］和模型預測控制(MPC)［8-9］等。

MPC 方法在車輛編隊控制方面具有可預測性和可約束性等眾多優點，廣泛應用于集群車輛列隊系統中［10-13］。魯若寧等［14］針對單向通信拓撲的車輛編隊自適應巡航控制問題，提出一種滿足隊列穩定的模型預測控制策略。Zhai 等［15］提出基于多目標優化問題的切換控制策略以協調多車自適應巡航系統中的安全性、舒適性、協同性和燃油經濟性等控制目標。Ozkan 等［16］提出一種應用于同質量重型卡車成隊列行駛的燃油經濟性模型預測控制策略，該策略將領航車的燃油最優控制方法與跟隨車的協同控制方法相結合，提升了系統的燃油經濟性。李永福等［17］在編隊控制基礎上提出一種雙向領導跟隨通信拓撲結構以考慮車車通信時延對控制器的影響。

上述研究提供了豐富的無人車輛編隊行駛控制策略，但是由于軍用無人車隊通常行駛在非結構化道路，且其控制器模型存在結構和參數的不確定性，所以軍用無人車編隊行駛控制存在受地面外部擾動和模型失配的問題。因此，由外部擾動和模型失配的引起的魯棒控制問題是編隊控制急需解決的關鍵問題。針對此類魯棒問題，孫中奇［18］對具有輸入邊界限制和有界不確定影響的輪式機器人展開研究，提出基于不變集的魯棒模型預測控制(RMPC) 方法，該方法將實際狀態軌跡收緊在魯棒不變集內并保證系統漸近穩定性。Zambelli 等［19］在模型預測控制方案的基礎上，引入2 階積分滑膜校正項，保持和增強了車隊在巡航時的一致性。Zhuang 等［20］設計了集中式擾動觀測器獲得穩定狀態參考，并利用RMPC 控制器保證隊列穩定和實現軌跡跟蹤，同時減小參數不確定性的影響。Meng 等［21］提出分布式RMPC 與終端不變集結合的方案用于多地面無人車輛控制，有效地應對突發狀況與通信數據包丟失問題。

上述方法均在解決車輛系統的參數不確定、結構不確定或外部擾動不確定方面取得了成功。但是不同于無人車編隊行駛，對于有人車領航與無人車跟蹤控制問題，有人領航車通常為處理突發場景需要進行緊急加減速和轉向，進而產生隨機不連續的擾動輸入且難以預測。同時隨機擾動的魯棒邊界不易確定，邊界過小或過大會引起控制系統魯棒性不足和保守性過大的問題。因此，綜合考慮有人領航車隨機擾動輸入并優化其魯棒邊界是有人車領航與無人車跟蹤控制需要解決的重要問題。

根據上述分析，本文提出一種參數自優化的有人與無人車輛編隊RMPC 算法。該算法在RMPC控制器中考慮了有人領航車的輸入擾動，并設計了基于線性矩陣不等式的局部反饋魯棒控制器抑制該擾動。基于貝葉斯優化方法對魯棒邊界進行優化，保證控制器魯棒性和低保守性。最后通過ROSVREP 仿真實驗和實車試驗驗證該算法在有人領航與無人跟隨控制中的可靠性。本文的總體框架如圖1所示。

圖1 總體框架圖Fig.1 Overall frame diagram

1 領航跟蹤及有人和無人車輛模型

1.1 領航跟蹤幾何模型

領航跟蹤需要控制器精確跟蹤期望路徑并與領航車輛維持期望間距。領航跟蹤控制系統使用l-φ方法建立表述車間相對位置的領航跟蹤幾何模型。有人領航車和無人跟隨車如圖2 所示。圖2 中，坐標系OgXgYgZg為大地坐標系，(Xi，Yi) 為領航車輛i的全局位置坐標，θi為車輛i 的航向角，(Xj，Yj)為跟隨車輛j 的全局位置坐標，θj為車輛j 的航向角，定義車輛j 和車輛i 的質心間距為dij，定義車輛i、j的質心連線與車輛j 航向的角度偏差為φij。

圖2 領航跟蹤幾何模型Fig.2 Cooperative control geometry model

車輛節點i、j 的相對位置關系可表述為

1.2 輪式車輛運動學模型

阿克曼前輪轉向車輛的運動示意圖如圖3 所示。圖3 中:(Xf，Yf)、(X，Y) 分別為全局坐標系下車輛前、后軸中心坐標;δ 為車輛前輪轉角;L 為車輛前后軸軸距;v 為車輛后軸中心前進速度;θ 為車輛航向角;黑色曲線為參考軌跡。

圖3 輪式車輛運動學模型Fig.3 Kinematic model of a wheeled vehicle

由此可得笛卡爾坐標系下的車輛運動學微分方程［22］如下:

進一步，該微分方程可重寫為控制系統的一般形式:

式中:ξack為控制系統狀態量，ξack=［X，Y，θ］T;uack為控制系統控制量，uack=［v，δ］T。

1.3 履帶車輛運動學模型

由于傳統運動學模型容易忽略履帶車輛滑移、滑轉等運動特性，而動力學模型的參數隨環境變化較大且不易獲得，本文選取在Frenet 坐標系下基于瞬時轉向中心的履帶速差轉向車輛運動學模型［23］。將狀態向量［s，ed，eθ］T替代［X，Y，θ］T對車輛位姿進行描述，模型如圖4 所示，并作出如下假設:

1) 假設履帶車輛只在水平面上運動;

2) 假設履帶車輛的幾何中心與質心重合;

3) 車體坐標系原點Ol位于幾何中心。

圖4 中:s 為沿曲線移動的切向距離;ed為沿曲線的法向距離;eθ為航向角跟蹤誤差;vx、vy為履帶車輛的縱向速度和橫向速度;ω 為履帶車輛的橫擺角速度;θ 為車輛的航向角;Oc為履帶車輛的瞬時轉向中心;(xc，yc) 為車體坐標系下車輛瞬時轉向中心坐標;Ole、Ori分別為左右兩側履帶的瞬時轉向中心;(xle，yle)、(xri，yri) 分別為兩側履帶的瞬時轉向中心坐標;vl、vr分別為左右兩側履帶的卷繞速度;vMq、vNq為左右兩側履帶牽連速度;B 為左右兩側履帶中心距。

圖4 履帶車輛運動學模型Fig.4 Kinematic model of a tracked vehicle

進而可得Frenet 坐標系下履帶車輛運動學模型微分方程的矩陣形式如下:

式中:κs為履帶車輛幾何中心Ol在參考曲線上對應點Pr的曲率值。

由于基于瞬時轉向中心的運動學模型更加有利于控制系統的建模，在實車控制時通常運用該模型進行建模，其運動學微分方程如下:

式中:ξ 為系統狀態量，此處ξ=［s，ed，eθ］T;u 為系統控制量，此處u=［vr，vl］T。

2 領航車軌跡預測與跟蹤控制器設計

領航軌跡預測與跟蹤控制器首先對有人領航車軌跡進行預測，并使用MPC 控制器跟蹤預測軌跡，實現編隊控制。

2.1 基于混合整數線性優化的有人領航車軌跡預測

有人車的未來狀態可由軌跡樹狀圖表示。以當前車輛位置作為根節點，并通過對車輛行為進行采樣拓展可得到未來時刻的狀態和預測軌跡，如圖5所示。

圖5 車輛軌跡樹示意圖Fig.5 Vehicle trajectory tree diagram

圖5 中χij為車輛軌跡樹的節點，后文統一將節點以χ 表示:

式中:t 為當前節點的時間戳;Jχ為節點的代價。

每次向軌跡樹中添加新的節點時，則向擴展列表β 中添加新節點對應所有可能擴展節點及其軌跡，并進行下一步探索和擴展。擴展列表中，使用邊γ 表示兩節點之間的狀態轉移。邊γ 由帶有轉移代價的轉角輸入和固定速度組成，表述為

式中:Jγ為力γ 的代價。狀態轉移方程來自輪式車運動學模型的2 階Runge-Kutta 離散。

軌跡樹一步擴展算法流程如圖6 所示。

圖6 軌跡樹一步擴展算法偽代碼Fig.6 Pseudo code of one-step extension algorithm of the trajectory tree

根據有人車駕駛員在實際駕駛過程的行為設計節點χ 的代價Jχ計算公式為

式中:Jχ中兩項分別表征該節點與參考路徑上最近點的距離和速度偏差;χref為參考距離;vref為參考速度;wχ、wv分別為各項的權重系數，分別表征了預測軌跡與有人車駕駛過程的實際軌跡和實際運動的相關程度，對應權重越大則軌跡預測器越基于對應實測值進行預測，二者需根據駕駛員行為確定。為使有人車的轉向控制量盡可能小，設計邊γ 的代價Jγ計算公式為

式中:wδ為權重系數。將軌跡優化計算模塊中優化問題表述為圖5 中邊的流問題，ne表示邊的數量，ns表示圖中節點的數量，流問題的決策變量表述為

式中:so 表示源節點，si 表示目標節點;0 表示節點連接經過該邊，1 表示節點連接不經過該邊。

為決策變量的求解構建如下目標函數:

有人車軌跡要保證軌跡樹根結點位于車輛初始位置，并保證流入和流出某節點的邊的數量一致，構建如下優化問題求解決策變量

上述決策變量求解得到有人車的預測軌跡。

2.2 模型預測控制器設計

為實現編隊任務下的無人車輛跟蹤控制，本節將設計模型預測控制器。

參考點上的任意值均滿足系統微分方程，有

對于基于瞬時轉向中心的運動學模型，泰勒展開作差，可得

參考點處ξr=［sr，ed，r，eθ，r］，ur=［vr，r，ve，r］T，同時對系統進行離散化，得

控制器需精確跟蹤期望路徑，即令無人車輛的實際橫向偏差、航向偏差最小，控制器選取路徑跟蹤的代價函數項設計為

控制器需以盡量小的無人車控制量完成任務，選取控制量的代價函數項為

控制器還需考慮車輛行駛的平順性及穩定性，即前后采樣時間內的控制量變化不能太大，由此設計目標函數為

無論是打贏“三大攻堅戰”，還是深化經濟、政治、文化、社會、生態、黨建等各領域改革，都不可能畢其功于一役，必須鍥而不舍、久久為功。看清楚時和勢，保持定力和耐心，不為任何風險所懼，不為任何干擾所惑，方能堅如磐石、行穩致遠。

最后控制器需考慮無人車輛的間距保持功能，即無人履帶車和其他車輛節點的實際間距與預設的期望間距差異最小，由此設計代價函數為

式中:wd為距離偏差權重參數;Δdt為實際間距與期望間距的跟蹤誤差。從而可得總的代價函數為

同時由于控制量及增量需要符合執行機構的客觀要求，因此對控制量及控制增量不等式約束如下:

則在當前控制周期k 最終發送給車輛底層執行器的控制量如下:

3 參數自適應優化的魯棒控制器

3.1 魯棒局部反饋控制器

魯棒局部反饋控制器用于消除有人領航車緊急加減速和緊急轉向控制輸入、無人跟隨車系統模型不確定性和外部環境的擾動。基于1.3 節建立的無人履帶車運動學模型建模結果，設具有建模誤差的實際系統為

式中:dk為實際系統未建模部分的系統擾動誤差。MPC 控制器對應名義系統為

將實際系統與名義系統做差，可得表征擾動不確定系統的誤差轉移方程為

為保證上述系統的漸進穩定性，設計魯棒控制器線性狀態反饋控制率為ue，k=Kξe，k，并假設擾動誤差dk=Bωωk，則擾動不確定系統可表述為

式中:ωk為具有單位協方差的零均值白噪聲;Bω為有界噪聲標準差矩陣。

為使系統極點均位于D(q，r) 內，D(q，r) 表示左半復平面的中心在-q +j0(q ＞0)、半徑為r(r ＜q)的圓盤，則可以通過求解如下線性矩陣不等式問題［24］。

當存在對稱正定矩陣P 和矩陣Y 使得該問題有解，則ue，k=KLMIξe，k，KLMI=YP-1可使系統漸進穩定，且滿足上述要求，證明如下:

將線性矩陣不等式基于Schur 補定理進行展開整理，可得

式中:Am=Ak+BkKLMI，Am為反饋后閉環系統矩陣。從而有

根據Lyapunov 穩定性理論可知，矩陣(Am+qI)/r 的特征值均在單位圓內，因此Am的極點均在圓盤D(q，r) 內。同時根據式(35) 易得0，即系統漸進穩定，證畢。

3.2 有界擾動和魯棒不變集確定

對于基于線性矩陣不等式求解的魯棒局部反饋控制器，擾動誤差的邊界是控制器的重要參數。本文將利用無人車輛歷史測試數據中系統真實狀態與名義系統預測模型輸出的系統狀態作差求解有界擾動，偏差求解如下:

式中:ξk-1與uk-1分別為在k -1 時刻記錄的車輛狀態量和控制量。需要說明的是，上述歷史測試數據是以當前時刻為起點向前固定時段的車輛實時反饋數據，并作實時更新以確定在不同工況下的狀態偏差和擾動邊界。對在某一工況下的歷史數據進行計算，可得圖7 所示一系列狀態偏差值，取狀態偏差值的最大值作為系統的噪聲邊界dbd:

由此，確定系統的擾動邊界如下:

由圖7 可知，狀態偏差的均值近似為0。因此將上述系統噪聲近似簡化為零均值白噪聲，并對擾動邊界進行適當放縮以權衡系統的魯棒性和保守性，可以確定有界噪聲的標準差矩陣為

圖7 模型一步預測誤差Fig.7 Frame diagram of the model predictive co-controller

式中:α 為-1～1 的魯棒邊界放縮系數。

在已得到反饋控制律KLMI的基礎上計算系統狀態偏差的魯棒不變集。對于狀態偏差的閉環系統方程:

式中:Reachf，k表示第k 個控制周期狀態偏差的魯棒不變集。在第k+1 個控制周期狀態偏差的魯棒不變集如下:

式中:⊕表示集合求并運算。通過對上述步驟的循環計算，直至Ωk+1=Ωk或到達循環次數時，將Ωk作為系統狀態偏差的魯棒不變集Z，即

魯棒不變集的具體計算流程如圖8 所示。

圖8 魯棒不變集計算流程偽代碼Fig.8 Computation flow pseudo code of the robust invariant set

3.3 名義模型預測控制器設計

名義模型預測控制器使用2.2 節的MPC 控制器，并在約束中加入對名義狀態量和控制量的收緊約束，以保證RMPC 的穩定性條件。對名義模型預測控制器的魯棒性約束如下:

式中:?為集合求相對補集運算。

名義模型預測控制器的數學描述如下:

根據優化問題求解器求解名義模型預測控制器的最優控制量，得到控制量序列如下:

則RMPC 控制器在第k 個控制周期的最終控制量如下:

將控制量uk發送至無人履帶車底層執行器，并在下個控制周期重復上述控制器計算過程。

3.4 控制器參數的貝葉斯優化

RMPC 控制器中魯棒邊界放縮系數α 和距離偏差wd、橫向偏差we、航向偏差wθ、控制量wu以及其變化率wΔu5 個權重系數待優化調整。使用貝葉斯優化方法對上述參數參數優化［25］，如圖9 所示。

圖9 基于貝葉斯優化的參數自優化框架圖Fig.9 Frame diagram of parameter self-optimization based on Bayesian optimization

考慮到有人領航與無人跟隨編隊任務中路徑跟蹤和間距保持的需求，建立如下全局目標函數:

式中:wbo，e為全局目標函數橫向偏差項系數;wbo，d為全局目標函數間距偏差項系數。通過調整優化參數矩陣WBo=［wd，we，wθ，wu，wΔu，α］T使得全局目標值最小:

式中:Ω 為保證控制器穩定的參數有界域。由于全局目標函數的梯度及凹凸性等性質未知，假設全局目標函數JG為高斯過程:

式中:μ(WBo) 為高斯過程的均值函數;為高斯過程的協方差函數。

設NBo次采樣的參數集為次計算的全局目標值，并對采集到的樣本數據集(Θ，Υ) 進行高斯過程回歸，得到替代模型高斯過程的后驗均值和后驗方差，并由上置信邊界采集函數得到下一次迭代建議采樣優化參數WBo。貝葉斯優化算法偽代碼如圖10 所示。

圖10 貝葉斯優化算法偽代碼表Fig.10 Pseudo code of the Bayesian optimization algorithm

4 試驗驗證

為證明本文控制算法的有效性，試驗設計分為仿真與實車兩部分進行試驗驗證。仿真實驗使用V-REP 仿真平臺和ROS 系統聯合仿真進行，實車試驗使用一輛有人東風猛士越野車和一輛雙側獨立電驅動中型無人履帶平臺。

4.1 ROS-VREP 聯合仿真

仿真實驗使用一輛有人輪式車和兩輛無人履帶車，車輛平臺參數如表1 所示。

表1 各平臺實驗參數Table 1 Parameters of each platform

仿真實驗在V-REP 軟件中建立兩種車輛仿真模型如圖11 所示，同時建立包括直線、直角彎、S 形彎和U 形彎等典型行駛工況的仿真場景，仿真場景及參考路徑如圖12 和圖13 所示。選用MPC 和RMPC 兩種控制器進行對比仿真，并且在對比仿真之前使用貝葉斯優化確定控制器的待優化參數。

圖11 ROS-VREP 聯合仿真車輛模型Fig.11 ROS-VREP co-simulation vehicle model

圖12 ROS-VREP 聯合仿真場景Fig.12 ROS-VREP co-simulation environment

圖13 有人車與無人車參考路徑Fig.13 Reference paths for manned and unmanned vehicles

貝葉斯優化試驗中使用RMPC 控制器，并根據控制器全局目標函數RMPC 中控制器參數進行優化。全局目標函數橫向偏差項系數和間距偏差項系數分別為8 和1，貝葉斯優化結果和RMPC 各優化參數如圖14 所示。

圖14 貝葉斯優化結果Fig.14 Bayesian optimization results

對比貝葉斯優化結果，選取全局目標值最低結果作為仿真實驗和實車試驗控制器參數值，參數如表2 所示。

表2 控制器參數Table 2 Parameters of the controller

為驗證控制器在不確定擾動下MPC 與RMPC兩種控制器的控制效果，設計圖15 所示場景中的仿真實驗，并對控制器在沿路徑長、橫向偏差和航向偏差3 個方向施加相同的有界隨機擾動。圖15(a) 中①～④表示路徑中的4 個代表位置。3 個方向擾動最大邊界值分別為0.3 m、0.25 m 和6°，控制效果如表3 和圖16 所示。

圖15 參考路徑和場地圖Fig.15 Reference paths and field map

結合表3 和圖16 可以得出，在安全性方面，普通MPC 控制器下的無人車1 非常接近魯棒邊界，無人車2 無法抵抗不確定性擾動而在位置⑤和位置⑦處超過魯棒邊界，大大增加了無人車的碰撞風險。而RMPC 控制器在相同擾動下，可以保證兩個無人車在魯棒邊界內行駛。在速度控制和間距保持方面，雖然RMPC 控制器具有更大的間距絕對偏差(如位置⑦) 和誤差均值，但不影響控制器任務完成能力且可以一定程度上提高控制器的安全性能。此外，由統計數據分析可知，在不確定性擾動下，無人車1 和無人車2 的RMPC 控制器相比MPC 控制器，橫向絕對偏差均值能減小17.284%和11.696%，航向絕對偏差均差能減小11.852%和5.101%。

圖16 領航跟蹤控制效果Fig.16 Piloting and tracking control effects

表3 仿真實驗結果統計Table 3 Simulation results

綜上所述，在不確定性擾動下相比與普通MPC控制器，RMPC 控制器在保證安全性的前提下，降低較小的間距保持功能，并能提高無人車的路徑跟蹤功能，有更好的控制效果和更優異安全性能和通過性能。

基于ROS-VREP 的聯合仿真對算法進行了初步的驗證，為實車試驗的設計提供了重要參考。

4.2 實車驗證

實車試驗使用1 輛東風猛士有人車和1 輛雙側獨立電驅動中型無人履帶車進行試驗。東風猛士車輛參數如表4 所示。無人履帶車搭載了衛星信號接收機、激光雷達、碼流相機、感知定位工控機、規劃控制工控機及其他必要的軟硬件。規劃控制工控機處理器為Intel? CoreTMi7-6820EQ，裝有Linux 操作系統，通過致遠電子的CANET 設備與車輛底層進行信息交互。

表4 車輛參數Table 4 Vehicle parameters

實車試驗選擇在內蒙古阿拉善某測試場的越野、水泥鋪路面組合進行測試驗證，試驗天氣為常規試驗天氣。有人車和無人車的全局參考軌跡和實際場地見圖15。

實車試驗使用MPC 和RMPC 控制器進行對比實驗，有人車和無人車行駛軌跡、行駛速度、間距偏差和跟蹤偏差如圖17 和表5 所示。

表5 實車試驗結果統計Table 5 Experimental results

圖17 領航跟蹤實車試驗跟蹤結果Fig.17 Piloting and tracking real vehicle test tracking results

通過對比兩種控制器的跟蹤效果可以得出，在間距保持方面，兩種控制器均通過調速與有人車保持了一定的間距，且最終都沒有突破安全間距邊界，表明兩控制器均有良好的安全性能。在跟蹤精度方面相比于MPC 控制器，RMPC 橫向絕對偏差降低了27.2%，航向絕對偏差降低了4.1%;在擾動的影響下，兩控制器的航向偏差都存在振蕩現象，但RMPC控制器下的抵抗振蕩效果要優于MPC 控制器。

綜上所述，兩種控制器均能在實車場景下完成領航跟蹤任務中的間距保持功能。同時RMPC 控制器具有更高的跟蹤精度和更小的振蕩效果，從而避免車輛沖出道路邊界，減小碰撞風險，更好地完成編隊任務。

5 結論

本文針對有人與無人車輛編隊控制中，有人領航車緊急加減速和轉向控制輸入的擾動問題，建立了參數自優化的有人與無人車輛編隊RMPC 算法，保證控制在擾動下的魯棒穩定性。同時進行了ROS-VREP 仿真實驗和實車試驗的算法驗證。得出主要結論如下:

1) 通過建立具有魯棒局部反饋的RMPC 控制器并確定擾動噪聲和魯棒邊界，有效地抑制了來自有人領航車緊急加減速和轉向控制輸入、無人跟隨車系統模型不確定性和外部環境的擾動。

2) 使用貝葉斯優化的方法對魯棒邊界放縮系數和代價函數權重系數進行參數自優化，提升了RMPC 控制器的跟蹤性能和魯棒性能。

3) ROS-VREP 仿真實驗和實車試驗結果表明，與傳統MPC 控制器相比本文設計的RMPC 控制器，在跟蹤精度和抵抗擾動方面有明顯的提升。控制器減少了橫向和航向誤差，并且降低了擾動造成的振蕩和不穩定性，試驗證明了本文提出方法的有效性和可行性。