面向NMPC運動規劃系統的數值優化加速技術*

高 鋒，馮德福，胡秋霞

（重慶大學機械與運載工程學院，重慶 400044）

前言

隨著自動駕駛系統智能化等級不斷提升，運動規劃器需同時考慮包括避撞和合規等在內的多種約束，并保證駕駛舒適性等需求［1-3］。目前，典型的運動規劃算法主要有人工勢場法、采樣法、機器學習和基于模型的方法［4］。其中，非線性模型預測控制（nonlinear model predictive control，NMPC）能夠處理車輛動力學等非線性約束，實現安全性、舒適性等多目標優化，已成為復雜交通場景下運動規劃的重要方法［4-5］。然而，非線性和非凸約束給數值優化帶來巨大困難，現有車載處理器的有限資源難以保證實時性。

針對車輛非線性動力學方程，主要采用近似線性化和多重打靶離散化（multiple shooting discretization，MSD）［6］。Falcone 等［7］在設計車道保持系統時，通過1 階泰勒展開近似車輛非線性動力學特性，采用文獻［8］的迭代算法可提高近似精度。然而，當預測時域較長時會產生不可接受的累積誤差［9］。近年來，許多學者采用基于龍格庫塔積分（Runge Kutta，RK）的MSD［10］求解非線性動態微分方程［11］。理論上，只要打靶點足夠密集就能無限逼近原函數。然而，密集打靶點會導致變量維度急劇增加。在避障場景下，Brown 等［12］比較了不同階次RK 的精度，Potena等［13］提出非均勻打靶間隔以減少打靶點。

對于運動規劃的非線性約束，因避障約束非凸，研究多集中于此。常見的類型主要有：時不變線性約束；符號距離；圓形、矩形和橢圓方程等［14］。針對超車場景，Dixit 等［14］采用時不變線性方程描述避障約束；Ziegler 等［15］采用自車（subject vehicle，SV）與障礙物車（obstacle vehicle，OV）間的符號距離作為避障約束。針對符號距離不可微問題，文獻［12］中采用多個圓近似障礙物外形，增加圓的數量可提高近似精度［16］，但會增加約束方程數量。從車輛外形幾何特征出發，文獻［17］和文獻［18］中分別設計了矩形和橢圓約束。相對而言，橢圓約束具有更好的平滑性，但仍然非凸。

針對由車輛非線性和非凸避障約束帶來的NMPC 運動規劃實時性問題，本文采用拉格朗日插值實現車輛動力學離散化，并設計拉格朗日多項式自適應變階次策略，進一步降低優化變量維度。此外，基于風險構建由橢圓和線性時變約束組成的混合避障約束（hybrid obstacle avoidance constraints，HC），在保障行車安全性的同時，實現規劃保守性和數值優化難度的良好平衡。在多障礙物場景下對所提方法的有效性進行了數值仿真和實驗驗證。

1 問題描述與分析

1.1 NMPC運動規劃方法

參照文獻［19］將運動規劃表述為如下問題：

式中：式（1a）為目標函數，包括跟蹤性能、控制輸入和控制量變化率；Q=diag(0.844，1，40)、P=diag(10-5，62.5)和R=diag(10-4，90)分別為權重矩陣；Tf=2 s為預測時域；y=[vxe1e2]T；ydes=[vd0 0]T；vd為目標速度；式（1b）和式（1c）分別為車輛動力學和初始狀態約束；式（1d）為橢圓避障約束［19］；式（1e）和式（1f）分別為控制量及其增量約束。

在預測時域內，周圍目標物通過式（1d）避障約束影響運動規劃系統。為考慮其不確定性，在第3節設計混合避障約束時采用文獻［9］中的方法估計預測時域內目標物的運動狀態。針對常規駕駛工況，為拓展運動規劃系統的適用范圍，采用式（2）的車輛縱橫向動力學耦合模型。該模型適用于低速大轉角工況，且考慮了輪胎側偏特性［20］。

式中：x=[vxvyωse1e2]T為狀態量；u=[FTδf]T為控制量。式（2）中的變量說明見圖1，m和Iz分別為質量和轉動慣量，lf和lr分別為質心到前后軸的距離，Cyf和Cyf分別為前后輪側偏剛度，k為參考車道中心線的曲率。

圖1 車輛動力學耦合模型變量定義

1.2 計算資源消耗分析

基于式（1）問題采用圖2 所示的場景仿真分析計算資源消耗。場景中，SV 通過連續換道超過障礙物車OV4，障礙物車OV1～OV5 平穩運行，初始狀態如圖2所示。

圖2 仿真場景

仿真時，采用MSD 離散化，根據表1 和表2 的參數采用CasADi 求解［21］。為提高效率，將算法編譯成機器碼，并集成到Prescan、Carsim 和Matlab/Simulink組成的仿真平臺［22-23］。其中，Prescan模擬交通場景，Carsim 模擬SV 動力學特性，Maltab/Simulink 運行運動規劃算法，控制周期為0.05 s。仿真平臺配置為：Xeon E3-1505M 處理器（3.0 GHz，4 核）；16 GB RAM。

表1 NMPC運動規劃參數

表2 車輛動力學模型參數

通過統計每個循環的仿真時間對計算資源的消耗進行分析，結果如圖3所示。

圖3 每個周期的仿真時間統計結果

由于場景相同，其它環節消耗的計算資源幾乎不變。如圖3 所示，每個控制循環的計算時間隨著打靶間隔和障礙物數量的增加而急劇增加。這是因為搜索空間與優化變量呈指數關系。優化變量由原始優化變量和約束條件的拉格朗日乘子組成。此外，當打靶間隔和障礙物數量增大時，計算時間的波動范圍變大。這說明數值優化的收斂性對環境條件依賴很大，易導致實際應用時不穩定。

為提高實時性，在第2 節提出一種新的離散化方法，以較少采樣點擬合車輛非線性動力學模型，并在第3節簡化橢圓約束以提高數值穩定性。

2 自適應正交拉格朗日插值離散化

MSD 本質上只利用了局部信息。相對而言，正交拉格朗日插值（orthogonal Lagrange interpolation，OL）能夠利用全局信息，原理上可通過較少的離散點保證預測精度。

2.1 基于拉格朗日插值的離散化

將控制和狀態變量采用拉格朗日插值離散化為

式（1a）代價函數離散化為

至此，運動規劃問題的式（1）轉化為式（3）、式（4）、式（6）和式（7）所示的離散形式。

2.2 離散誤差統計分析

為驗證拉格朗日插值離散化的有效性，在以下隨機條件下將其與MSD進行比較：

（1）車輛初始狀態在vx∈[0，30]m/s 和ω∈[0，45] (°)/s范圍內隨機選取［19］；

（2）控制輸入u(t)隨機生成，并采用限速器根據自然駕駛特性［19］進行濾波，統計結果如圖4所示。

圖4 車輛控制輸入的對比結果

生成數據的時長設為3 s［25］，共得到1 703 233組數據。由式（1c）計算車輛狀態真實值，采用真實值與擬合值間的最大誤差衡量精度。當N=8 時，典型的擬合結果如圖5 所示。圖中，朗格朗日插值和拉格朗日正交插值的計算公式均為式（3），兩者差別在于插值點的選擇。前者在預測時域內均勻分布，后者則中間稀疏兩側密集以提高擬合精度。MSD采用歐拉方法進行離散化。

圖5 典型擬合結果

由圖5 可知，OL 的擬合精度最好，均勻拉格朗日插值（uniform lagrange interpolation，UL）的擬合誤差略大于OL，MSD 最差。這是因為MSD 僅利用前一步信息積分得到預測值，而拉格朗日插值可利用全局信息。此外，采用UL時在預測時域的開始和結束階段存在龍格現象，因為這些區域的有效數據較少。OL 在兩側區域采樣點相對中部更加密集，使得整體數據點的可用信息趨于均勻，有利于消除龍格現象。

圖6 給出了N=8 時擬合誤差的整體對比結果。除離散化方法外，擬合誤差也隨工況變化。在大轉向角、低速工況下，車輛動力學非線性強，誤差較大。通過增加離散點可以很好地擬合，但計算量也隨之增加。因此，為在保證插值效率的同時減小誤差，下節提出了一種自適應變階次策略。

圖6 擬合誤差對比結果

2.3 自適應變階次插值策略

為設計階次調整策略，對不同階次的離散化誤差進行對比，結果如圖7（a）所示。總體上，離散化誤差隨插值階次的增加而減小，但當車輛動力學近似線性時，改善較小。如當vx∈［20，30］m/s 時，N=8 與N=6 的OL 離散化誤差幾乎相同。因此，在車輛動力學近似線性的工況下，低階插值即可保證精度。

圖7 插值階次自適應策略

為平衡計算效率和預測精度，根據第2.2 節的分析工況，從低到高階逐步增加離散化階次，當預測時域內所有車輛狀態的最大誤差均小于0.01 時，則此時的離散化階次即為該狀態對應的階次。以橫擺角速度為例，得到的所有狀態點對應的離散化階次區域如圖7（b）所示，從而根據車輛狀態選擇插值階次：

式中S(vx，ω)是階次自適應策略（ALD）。為保證整個預測時域的精度，最終插值階次O為

式中：(vx，i，ωi)，i=0，…，N；N為預測的車輛狀態。

3 混合避障約束

式（1）問題中的橢圓約束（elliptic obstacle avoidance constraint，EC）［19］可表示為

式中：sx，o和sy，o分別是橢圓的長軸和短軸；SV 和障礙物的質心分別為(s，e1)和(so，e1，o)。如圖3（b）所示，非凸約束會極大降低效率，為此須根據風險構造混合避障策略。

3.1 橢圓避障約束的凸化

對于低風險障礙物，由預測的自車和障礙物狀態根據橢圓約束方程式（11）在切點處線性化，從而將非凸非線性避障約束轉化為線性凸約束（如圖8所示），得到動態線性約束（linear constraint，LC）集：

圖8 基于動態線性約束集的凸化

式中：(sdo，i，edo，i)為切點坐標和分別為長軸和短軸。障礙物狀態(so，ie1，o，i)的預測參見文獻［9］。

3.2 混合避障策略

為降低LC 的保守性，采用碰撞時間（time to collision，TTC）度量障礙物風險，將動態線性約束和橢圓約束組合成混合避障約束（hybrid obstacle avoidance constraint，HC）。如圖9 所示，對于高風險OV 使用橢圓約束；其它OV，使用式（11）表示的線性約束。采用所提混合避障約束，雖然優化問題仍非凸，但能夠減少非凸避障約束的數量，從而降低數值求解難度。

圖9 混合避障約束

4 仿真驗證和分析

考慮如下因素，采用MSD&EC、LD&HC、ALD&EC、ALD&LC 和ALD&HC 共5 種組合進行仿真分析驗證ALD 和HC 的加速效果：（1）在數值求解上已采用主流的二次規劃求解器CasADi［21］；（2）車輛動力學模型直接線性化無法滿足大轉角等非線性工況的預測精度要求；（3）MSD 和LC 是設計NMPC運動規劃系統時典型的簡化加速技術。仿真平臺和參數同1.2節，MSD 打靶間隔為60，拉格朗日插值的初始階次為8。考慮高速路合流和無保護左轉彎是具有一定難度的工況［26］，設計的仿真場景如下：

（1）場景1 中包含5 個障礙物，SV 和OV 的初始狀態如圖10（a）所示。

圖10 仿真場景

（2）場景2 中包含8 個障礙物，SV 和OV 的初始狀態如圖10（b）所示。與場景1 相比，速度更低，障礙物更多。

仿真時，車輛的初始狀態如圖10 所示，SV 受本文設計的運動規劃算法的控制，障礙物車保持初始速度沿設定的軌跡運行。

4.1 場景1仿真結果及分析

在該場景下，SV 須進行連續兩次避障操作，車輛軌跡如圖11所示。

圖11 SV和OVs的軌跡（場景1）

由圖11 可見，ALD&LC 成功實現避障，但速度波動較大，軌跡更接近路緣。這是由于LC的可行空間較小，導致其速度較保守（如圖12（b））。這不僅對駕駛舒適性不利，且易違反交通規則。相反，基于HC的算法通過降低保守性實現了更平順的加減速。在9 s左右，MSD&EC導致SV與OV3碰撞。此時，SV剛完成避障，又須立即再次執行避障操作，控制和狀態變量均發生劇烈變化（如圖12 所示）。ALD 能夠自適應提高階次保證預測精度，MSD 的離散化誤差隨非線性增強而增大，導致無法保證安全性。從圖13 的預測誤差統計結果也可看出，MSD 的精度遠低于ALD。

圖12 SV狀態量和控制輸入（場景1）

圖13 預測誤差對比結果（場景1）

4.2 場景2仿真結果及分析

該場景中SV 通過交叉路口，與場景1 相比障礙物車更多。此時，由于非線性更強且橢圓約束增多，導致MSD&EC 的最大計算時間超過2 s，因此只給出其它算法的結果。

如圖14 和圖15 所示，里程在20～40 m 時，SV 需安全通過OV1-OV4 和OV6 組成的狹窄空間。如圖15（b）所示，非LC 算法均成功實現超車和避障。LC算法因減小了可行域導致無足夠的超車空間，通過降速使SV 保持在原車道行駛。當s>30 m 時，SV須通過由OV1、OV5 和OV8 限制的狹窄空間。如圖15（a）所示，LC 不能準確描述OV 形狀，導致前輪轉角發生劇烈變化，并在15 s 時發生碰撞，如圖14 所示，而HC則成功通過了交叉口。

圖14 SV和OVs的軌跡（場景2）

圖15 SV狀態量和控制輸入（場景2）

4.3 加速效果分析

對每個控制循環的計算時間進行統計分析，結果如圖16 所示。由于MSD&EC 在場景2 的仿真被迫終止，僅顯示其部分計算時間。

圖16 運行時間統計結果

由圖16 可見，MSD&EC 消耗的計算資源最多。此外，MSD 的預測誤差也大于ALD（見圖13（a））。這導致場景1 中SV 在9 s 附近（見圖11）高速避讓OV3 時發生碰撞。相比之下，ALD&LC 所需計算資源最少，但LC 不能很好地表征車輛外形，具有較高保守性。在場景2 中，受其它障礙物限制（見圖14）可行域進一步縮小，導致15 s左右與OV8發生碰撞。此外，由圖12 和圖15 可見，求解算法的保守性還導致控制輸入和車輛狀態大幅波動。總體而言，ALD&HC 保證了仿真場景下的安全性，具有較好的計算效率和駕乘舒適性。

5 實驗驗證

本節通過實車驗證所設計NMPC 運動規劃器的性能。實驗平臺如圖17 所示，運動規劃算法運行在MicroAutoBox II（四核ARM A57 CPU）上。實驗過程中，運行環境通過激光雷達進行感知，由數據融合系統通過以太網輸出周圍障礙物的方位和速度信息。車輛縱向運動通過加/減速度控制，分別由發動機管理系統和動力學穩定性控制系統執行。車輛橫向運動通過方向盤轉角控制，由動力轉向系統執行。

圖17 實驗平臺

實驗場景設計如下。

（1）場景1：高速超車

該場景旨在驗證車輛的連續避障能力圖18（a）。為安全起見，采用車輛在環技術，通過Prescan 模擬環境和目標物［27］。

圖18 場景1實驗結果

（2）場景2：低速交通擁堵工況

該場景旨在驗證算法在圖19（a）所示的多障礙物場景下的有效性。

圖19 場景2實驗結果

5.1 高速超車

初始條件見圖18（a），所有OV 保持初始速度不變，OV1按設定軌跡切入右車道，OV2始終保持在左車道勻速行駛。

MSD&EC 與ALD&HC 的對比結果如圖18所示。14 s 時，ALD&HC 控制的SV 減速左轉避開OV1，然后在36 s 時加速右轉避開OV2，最后切回右車道。與ALD&HC 相比，MSD&EC 在超車過程中由于控制量優化精度和實時性不夠導致26.3 s 沖出右側路沿。與4.1 節的分析一致，在連續避障工況下車輛動力學的非線性更強，導致MSD的預測誤差更大。

5.2 低速交通堵塞

如圖19（a）所示，本實驗在雙車道上進行，SV 位于右側車道，所有障礙物車均由駕駛員根據場景要求按正常駕駛習慣控制。該場景中，OV1 停車導致交通堵塞，OV2勻減速至停車，同時OV3以不斷波動的速度行駛于SV 前方（見圖19（b）），SV 需同時避免與3輛障礙物車相撞。由于MSD&EC 的計算時間遠超設定的50 ms周期，因此僅給出ALD&HC結果。

如圖19（c）所示，SV首先避開停止的OV1，然后跟隨OV3并等待超車機會。在450～480 s時，OV3與SV 的距離較小，同時SV 須避免與OV1 碰撞。此時，SV采用“起停”的方式跟隨OV3行駛。500 s后，OV2的停車為SV 超車并加速到期望速度提供了足夠的空間。

5.3 實時性驗證

由第4.3 節的仿真分析已得到不同組合算法加速效果的相對關系，且過差的實時性易導致事故，因此僅對部分算法每個控制循環的時間進行統計，結果如圖20 所示。結合第4.3 節的結果可知，ALD 和HC 能夠顯著提升實時性，兩種場景下，ALD&HC 的最大計算時間均小于50 ms，而MSD&EC 的最小計算時間超過80 ms。

圖20 計算時間統計結果

6 結論

本文提出了ALD 和HC 兩種方法對NMPC 運動規劃系統的數值優化進行加速。通過數值分析和實驗驗證，得到以下結論：

（1）隨著優化變量維度和約束條件數量的增加，計算資源的消耗幾乎呈指數級增長；

（2）ALD 能以比MSD 更少的離散點數保證離散精度，HC能在規劃保守性和數值優化難度之間取得較好的平衡；

（3）本文設計的NMPC 運動規劃系統通過引入ALD和HC，能夠處理多障礙物和縱橫向運動耦合的場景，并具有良好實時性。