面向NMPC運(yùn)動(dòng)規(guī)劃系統(tǒng)的數(shù)值優(yōu)化加速技術(shù)*

高 鋒，馮德福，胡秋霞

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

前言

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

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

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

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

1 問(wèn)題描述與分析

1.1 NMPC運(yùn)動(dòng)規(guī)劃方法

參照文獻(xiàn)［19］將運(yùn)動(dòng)規(guī)劃表述為如下問(wèn)題：

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

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

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

圖1 車(chē)輛動(dòng)力學(xué)耦合模型變量定義

1.2 計(jì)算資源消耗分析

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

圖2 仿真場(chǎng)景

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

表1 NMPC運(yùn)動(dòng)規(guī)劃參數(shù)

表2 車(chē)輛動(dòng)力學(xué)模型參數(shù)

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

圖3 每個(gè)周期的仿真時(shí)間統(tǒng)計(jì)結(jié)果

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

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

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

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

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

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

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

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

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

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

（1）車(chē)輛初始狀態(tài)在vx∈[0，30]m/s 和ω∈[0，45] (°)/s范圍內(nèi)隨機(jī)選?。?9］；

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

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

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

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

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

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

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

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

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

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

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

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

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

3 混合避障約束

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

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

3.1 橢圓避障約束的凸化

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

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

式中：(sdo，i，edo，i)為切點(diǎn)坐標(biāo)和分別為長(zhǎng)軸和短軸。障礙物狀態(tài)(so，ie1，o，i)的預(yù)測(cè)參見(jiàn)文獻(xiàn)［9］。

3.2 混合避障策略

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

圖9 混合避障約束

4 仿真驗(yàn)證和分析

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

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

圖10 仿真場(chǎng)景

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

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

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

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

圖11 SV和OVs的軌跡（場(chǎng)景1）

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

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

圖13 預(yù)測(cè)誤差對(duì)比結(jié)果（場(chǎng)景1）

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

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

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

圖14 SV和OVs的軌跡（場(chǎng)景2）

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

4.3 加速效果分析

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

圖16 運(yùn)行時(shí)間統(tǒng)計(jì)結(jié)果

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

5 實(shí)驗(yàn)驗(yàn)證

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

圖17 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)場(chǎng)景設(shè)計(jì)如下。

（1）場(chǎng)景1：高速超車(chē)

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

圖18 場(chǎng)景1實(shí)驗(yàn)結(jié)果

（2）場(chǎng)景2：低速交通擁堵工況

該場(chǎng)景旨在驗(yàn)證算法在圖19（a）所示的多障礙物場(chǎng)景下的有效性。

圖19 場(chǎng)景2實(shí)驗(yàn)結(jié)果

5.1 高速超車(chē)

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

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

5.2 低速交通堵塞

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

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

5.3 實(shí)時(shí)性驗(yàn)證

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

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

6 結(jié)論

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

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

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

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