高 鋒，馮德福，胡秋霞

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

前言

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

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

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

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

1 問題描述與分析

1.1 NMPC運動規(guī)劃方法

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

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

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

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

圖1 車輛動力學(xué)耦合模型變量定義

1.2 計算資源消耗分析

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

圖2 仿真場景

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

表1 NMPC運動規(guī)劃參數(shù)

表2 車輛動力學(xué)模型參數(shù)

通過統(tǒng)計每個循環(huán)的仿真時間對計算資源的消耗進行分析，結(jié)果如圖3所示。

圖3 每個周期的仿真時間統(tǒng)計結(jié)果

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

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

2 自適應(yīng)正交拉格朗日插值離散化

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

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

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

式（1a）代價函數(shù)離散化為

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

2.2 離散誤差統(tǒng)計分析

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

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

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

圖4 車輛控制輸入的對比結(jié)果

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

圖5 典型擬合結(jié)果

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

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

圖6 擬合誤差對比結(jié)果

2.3 自適應(yīng)變階次插值策略

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

圖7 插值階次自適應(yīng)策略

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

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

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

3 混合避障約束

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

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

3.1 橢圓避障約束的凸化

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

圖8 基于動態(tài)線性約束集的凸化

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

3.2 混合避障策略

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

圖9 混合避障約束

4 仿真驗證和分析

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

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

圖10 仿真場景

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

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

4.1 場景1仿真結(jié)果及分析

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

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

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

圖12 SV狀態(tài)量和控制輸入（場景1）

圖13 預(yù)測誤差對比結(jié)果（場景1）

4.2 場景2仿真結(jié)果及分析

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

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

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

圖15 SV狀態(tài)量和控制輸入（場景2）

4.3 加速效果分析

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

圖16 運行時間統(tǒng)計結(jié)果

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

5 實驗驗證

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

圖17 實驗平臺

實驗場景設(shè)計如下。

（1）場景1：高速超車

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

圖18 場景1實驗結(jié)果

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

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

圖19 場景2實驗結(jié)果

5.1 高速超車

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

MSD&EC 與ALD&HC 的對比結(jié)果如圖18所示。14 s 時，ALD&HC 控制的SV 減速左轉(zhuǎn)避開OV1，然后在36 s 時加速右轉(zhuǎn)避開OV2，最后切回右車道。與ALD&HC 相比，MSD&EC 在超車過程中由于控制量優(yōu)化精度和實時性不夠?qū)е?6.3 s 沖出右側(cè)路沿。與4.1 節(jié)的分析一致，在連續(xù)避障工況下車輛動力學(xué)的非線性更強，導(dǎo)致MSD的預(yù)測誤差更大。

5.2 低速交通堵塞

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

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

5.3 實時性驗證

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

圖20 計算時間統(tǒng)計結(jié)果

6 結(jié)論

本文提出了ALD 和HC 兩種方法對NMPC 運動規(guī)劃系統(tǒng)的數(shù)值優(yōu)化進行加速。通過數(shù)值分析和實驗驗證，得到以下結(jié)論：

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

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

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