1. 概述

《Baidu Apollo EM Motion Planner》是Apollo官方的出的文章（雖然是官方文章，但其中有表述不是很嚴謹，也有一些小錯誤，大家可以審視的研讀這篇文章，另外可以找一些該文的參考文獻看一看會有很大幫助），網(wǎng)上的講解很多都是機翻，本文根據(jù)自己對《Baidu Apollo EM Motion Planner》的理解整理如下：

Apollo的功能架構(gòu)如下：這篇文章是專門將Motion planner的，其中：

• 高精地圖模塊（HD Map）提供高精度地圖信息；
• 定位模塊（Localization）和感知模塊（Perception）提供當(dāng)前車輛周圍動態(tài)環(huán)境；
• 預(yù)測模塊（Prediction）提供預(yù)測的未來環(huán)境信息；
• 路由模塊（routing）基于專用的路由地圖routing_map，輸出車輛在從出發(fā)點到目的地的過程中經(jīng)過的所有路段（類似百度地圖A*）；
• 運動規(guī)劃模塊（MotionPlanning）接收所有信息生成安全舒適的軌跡發(fā)送給車輛控制模塊。

備注：Routing類似于現(xiàn)在開車時用到的導(dǎo)航模塊，通常考慮的是起點到終點的最優(yōu)路徑（通常是最短路徑），Routing考慮的是起點到終點的最短路徑，而Planning則是行駛過程中，當(dāng)前一小段時間如何行駛，需要考慮當(dāng)前路況，是否有障礙物。Routing模塊則不需要考慮這些信息，只需要做一個長期的規(guī)劃路徑即可

論文提出一種適用于高速公路和低速城市場景的多車道-單車道分層架構(gòu)：

上層針對多車道：用于變道決策（通過比較lane-level trajectory來處理變道場景）

下層針對單車道：基于Frenet frame，通過動態(tài)規(guī)劃&二次規(guī)劃進行路徑和速度規(guī)劃

備注：對于規(guī)劃的覆蓋范圍:至少提供8秒或者200米的運動規(guī)劃軌跡，算法的反應(yīng)時間必須在100ms（人是300ms反應(yīng)時間）內(nèi)做出反應(yīng)。

1.1. Multilane Strategy

一般來講，L4級的自動駕駛應(yīng)當(dāng)包含變道，而變道分為主動變道和被動變道。主動變道是routing模塊因為要到達最終目的地而發(fā)出的換道請求，被動變道是本車道被障礙物擋住被迫避讓變道。而Apollo的EM planner提出了自己的解決方案：

對于候選車道（一般是本車道或相鄰車道，可理解為多個候選凸域，詳見百度技術(shù)學(xué)院中對Apollo規(guī)劃中非凸問題的講解），所有障礙物和環(huán)境信息都投影到Frenet frame中；

并將交通規(guī)則與候選車道綁定；

每個候選車道基于規(guī)劃器，都生成一條從當(dāng)前車道換至本車道的最佳軌跡；

Crosslane trajectory decider模塊會根據(jù)代價函數(shù)和安全選出要換到哪條車道。

1.2. Path-Speed Iterative Algorithm

基于Frenet frame（SLT）方法的軌跡規(guī)劃一般分為2種：直接在3維空間下優(yōu)化求解，或?qū)⒙窂胶蛙囁俳怦钋蠼?#xff08;軌跡包含兩部分：路徑和路徑上每個點的速度）。

• 直接在3維空間下優(yōu)化求解：一般會用trajectory sampling或lattice search，不過計算量較大，如果因為計算量問題對分辨率或采樣率妥協(xié)， 則容易只得出次優(yōu)解。
• 將路徑和車速解耦求解：路徑規(guī)劃時考慮靜態(tài)障礙物或低速障礙物，速度規(guī)劃時考慮動態(tài)障礙物（有動態(tài)障礙物時，該解耦法可能不是最優(yōu)解）。

備注：對于高速動態(tài)障礙物，考慮到安全問題，Apollo EM planner傾向于變道而不是減速避讓。

1.3. Decisions and Traffic Regulations

約束分兩類：
硬約束：交通規(guī)則
軟約束：避讓、超車、接近障礙物的決策

Apollo EM planner傾向先做決策在做規(guī)劃（和其他算法相比，該算法請決策重規(guī)劃，該特點會在后面闡述），因為決策過程可以明確變道可行性，這樣可大大減小搜索空間。

在EM planner的decision階段有如下幾個步驟（1.1中的描述類似，其實可以理解為是一個東西，只是描述側(cè)重不一樣），下面的全文都是對這幾個步驟的詳解：

用一個粗糙但可行的軌跡描述本車行駛意圖

基于此軌跡，衡量本車與障礙物之間的距離

規(guī)劃器根據(jù)該軌跡，會生成凸的可行空間（用于平滑軌跡）；

然后二次規(guī)劃會用于平滑軌跡（路徑和速度）。

2. EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY

本節(jié)講EM planner的多車道 multilane strategy架構(gòu)（對1.1和1.3的重復(fù)描述，但更詳細了）：

reference line generator會根據(jù)交通規(guī)則和障礙物，產(chǎn)生一些車道級備選reference line。該過程是基于HD map 和 Routing中的導(dǎo)航信息實現(xiàn)的。（如左右相鄰車道？這個reference line沒搞明白是什么）；

基于reference line生成對應(yīng)的Frene Frame；

將障礙物等環(huán)境信息投影到path的Frenet frame中；

生成平滑path profile；

將障礙物等環(huán)境信息投影到speed的Frenet frame中；

生成平滑speed profile。

reference line decider會在所有l(wèi)ine-level的最佳軌跡中，根據(jù)車輛狀態(tài)、交通規(guī)則、代價函數(shù)選擇出最佳軌跡

3. EM PLANNER AT LANE LEVEL

本節(jié)將EM planner的單車道 lane level架構(gòu)：

Path E-step：
1、靜態(tài)、低速和來向障礙物投影到SL坐標(biāo)中；
2、動態(tài)障礙物用EM planner的變道策略去cover（考慮到安全問題，path optimization時遇到動態(tài)障礙物時傾向于避讓，因此用變道策略cover該場景）；
3、path的E-step最終輸出的是帶有靜態(tài)障礙物、動態(tài)障礙物軌跡和預(yù)測碰撞區(qū)的SL。

Speed E-step：
1、所有障礙物（低速、高速、來向）都投影到基于path profile的ST坐標(biāo)中；
2、如果障礙物軌跡和path有重合點，則在ST圖中該處則會生成碰撞陰影區(qū)；
3、speed的E-step最終輸出的是帶有前后障礙物的ST（其M-step可在空白區(qū)域搜索可行車速）。

備注：靜態(tài)障礙物是直接投影，但動態(tài)障礙物是把運動軌跡投影到Frenet frame中。且目前得到的仍是非凸問題。

M-step：使用動態(tài)規(guī)劃和二次規(guī)劃生成平滑的path和speed
1、E-step后，該問題是一個非凸優(yōu)化問題；
2、動態(tài)規(guī)劃能夠得到一個粗略解，決策應(yīng)該避讓nudge、減速yield還是超車overtake；
3、基于粗略解，可將1個非凸問題分解為多個凸問題。

3.1. SL and ST Mapping (E-step)

SL
1、在SL坐標(biāo)系中，我們用S、L、側(cè)向速度dL、側(cè)向加速度ddL、側(cè)向加加速度dddL，這5個量描述本車狀態(tài)；
2、靜態(tài)障礙物的位置是時不變的，在投影時比較簡單；
3、動態(tài)障礙物的位置，則需要根據(jù)當(dāng)前時刻本車和障礙物的S方向的速度，預(yù)測T時刻后其所處的位置（障礙物的預(yù)測位置是prediction module給出的），以此來判斷是否有碰撞。
4、path的E-step最終輸出的是帶有靜態(tài)障礙物、動態(tài)障礙物軌跡和預(yù)測碰撞區(qū)的SL。

下圖中大概3s左右會出現(xiàn)碰撞：

ST
speed的E-step最終輸出的是帶有前后障礙物的ST（其M-step可在空白區(qū)域搜索可行車速）。

3.2. M-Step DP Path

path M-step：其作用是基于Frenet frame生成平滑的path profile（但此時SL空間是非凸的，eg：左右車道避讓均可能為局部最優(yōu)解），分為兩大步驟：
1、基于動態(tài)規(guī)劃的path decision：動態(tài)規(guī)劃會提供一個粗略的、帶有可行區(qū)域和障礙物避讓策略（nudge decision）的path profile。其步驟主要包括：lattice sampler、cost function和dynamic programming search；
2、基于樣條曲線的path planning：二次規(guī)劃。

lattice sampler
1、lattice sample的不同行（這個“行”是垂直車道線的，即L方向）之間的點通過五次多項式來平滑連接（應(yīng)該每段都有一個五次多項式，piecewise就會有多個五次多項式）。行內(nèi)sample points的間隔距離（L方向）取決于速度，道路結(jié)構(gòu)，變換車道等；
2、具體采樣策略可根據(jù)場景自定義；
3、但出于安全考慮，lattice sample的S方向的距離最好大于8s或200m。

cost function
總的代價函數(shù)是：平滑度、障礙物避讓、車道代價3部分線性疊加得來的。

• $C_{smooth}$中$f^{\prime }(s)$是航向角，$f^{\prime \prime }(s)$是曲率，$f^{\prime \prime \prime }(s)$是曲率變化率；
• $C_{obs}$中$d$是障礙物到本車的距離，$d_c$考慮安全的緩沖距離，$C_{nudge}$應(yīng)當(dāng)定義為遞減函數(shù)，$C_{collision}$是碰撞代價；
• $C_{guidance}$分兩部分：: guidance line cost 和 on-road cost。guidance line cost是規(guī)劃出的path距離guidance path的距離，這個guidance path一般是車道中心線。on-road cost是防止車輛沖出道路邊緣的代價。公式中把guidance line function定義為$g(s)$。

會基于代價函數(shù)，用動態(tài)規(guī)劃搜索最合適的path，這就涉及到了決策decision，即應(yīng)對障礙物的策略：nudge、overtake還是yield。

3.3. M-Step Spline QP Path

path M-step中的DP已經(jīng)可以求解出一個粗略的feasible tunnel，而QP要做的就是平滑這個DP輸出來的guidance line。具體效果類似下圖：

目標(biāo)函數(shù)：

• path的QP本質(zhì)是優(yōu)化一個帶有線性約束的目標(biāo)函數(shù)，其代價函數(shù)如下。可以看出，其前3項是盡量平滑（$f^{\prime }(s)$是航向角，$f^{\prime \prime }(s)$是曲率，$f^{\prime \prime \prime }(s)$是曲率變化率。曲率其實是考慮了車輛的動力學(xué)可行性）；第4項是盡量和DP的path保持一致（不要偏離太遠，以免發(fā)生碰撞）。

約束：
DP的約束有兩個：boundary constraints 和 dynamic feasibility。

• boundary constraints：邊界約束也可以理解為碰撞約束，可以在車頭車尾加兩個半圓作為車輛邊界以保證約束的線性和凸性。
  
  

• dynamic feasibility：DP給出了由很多points（每個row上一個point）組成guidance line，QP可以此為基礎(chǔ)調(diào)整每個point在L方向的位置以實現(xiàn)平滑path。但調(diào)整是有可行范圍的，該范圍可量化為QP的約束（lmin和lmax）。另外曲率$f^{\prime \prime }(s)$和曲率變化率$f^{\prime \prime \prime }(s)$也可以用dynamic feasibility去約束衡量（如下圖）。

3.4. M-Step DP Speed Optimizer

• speed的M-step會基于ST生成speed profile。這也是個非凸問題，同樣可以使用動態(tài)規(guī)劃+二次規(guī)劃生成speed profile。

• 動態(tài)規(guī)劃包含cost function、ST graph、dynamic programming search。輸出物為：分段線性speed profile（是QP的guidance line）、feasible tunnel（a convex region）、speed decision（yield or overtake）。

DP Speed Optimizer的代價函數(shù)，其中：

• 第1項是車速保持代價，該項想表征的是：在沒有障礙物和交通燈約束時車子的期望車速，Vref一般由道路車速限制、曲率和其他交通規(guī)則所決定；
• 第2項和第3項是加速度$a$和加加速度$jerk$的平順性代價；
• 第4項是障礙物距離代價。

這里的約束主要是車輛動力學(xué)約束，如加速度$a$和加加速度$jerk$的約束，另外還有個比較重要的是車輛直行不倒車的約束（下面的Speed QP會著重解決該問題）。

3.5. M-Step QP Speed Optimizer

Speed的M-step中的QP主要是為了平滑速度（具體效果類似下圖），包含三部分：cost function、linearized constraint 和 spline QP solver。

其代價函數(shù)如下：

• 第1項是QP得到的S距離DP給出的guidance line之間的距離；
• 第2、3項是平滑度衡量。
  
  

speed M-step的QP的線性約束如下，其中：

• 第1項是單調(diào)性：保證車輛只能前行不能倒車；
• 第2項是ST曲線在某一時刻的上下界，算是表征碰撞的約束；
• 第3項是車速上限，一般從交通規(guī)則或車輛動力學(xué)得出；
• 第4項是加速度限制，一般由車輛動力學(xué)性能、舒適性決定；
• 第5項是加加速度限制，一般也由車輛動力學(xué)性能、舒適性決定；

3.6. Notes on Solving Quadratic Programming Problems

出于安全的考慮，Apollo評估了大概100個不同位置或時間點的路徑和速度。約束的數(shù)量超過600。對于選擇所需的路徑和速度優(yōu)化器，我們發(fā)現(xiàn)五項多項式是比較合適的。樣條曲線包括3到5個多項式，大約有30個參數(shù)。

3.7. Notes on Non-convex Optimization With DP and QP

• DP：如前文所述，DP算法通過采樣在每個凸域內(nèi)生成局部最優(yōu)解，然后通過代價函數(shù)求得整個非凸空間內(nèi)的近似全局最優(yōu)解（該句待定）。由于計算時間的限制，采樣密度會受到限制，而有限網(wǎng)格內(nèi)的優(yōu)化會得到一個粗糙的DP解。換句話說，DP不一定能所有的場景下都取得最優(yōu)解。例如，DP求出從左側(cè)避開障礙物的路徑，但不能選擇具有最佳距離的路徑。
• QP：相反，QP是基于DP的粗略解進行曲線平滑。如果沒有DP，單純的QP是無法實現(xiàn)決策規(guī)劃的。例如，如果一個障礙物在車輛前面，QP需要一個決策，例如從左側(cè)變道，從右側(cè)變道，跟隨或者超車，來生成其約束。隨機或基于規(guī)則的決策容易使QP陷入失敗或者局部極小值。
• DP+QP：DP+QP算法將兩者的局限性降到最低：（1）EM規(guī)劃器首先用DP在網(wǎng)格內(nèi)搜索，以求得粗略的解；（2）DP的結(jié)果被用于生成凸域并引導(dǎo)QP；（3）QP被用于在凸域內(nèi)尋找全局最優(yōu)解。

總的來講，DP對非凸問題做決策（nudge from right or left or follow or overtake）。QP以DP決策為基礎(chǔ)，在凸域內(nèi)平滑曲線。

4. CASE STUDY

如上所述，雖然大多數(shù)高級的規(guī)劃算法都是基于重決策的，但EM planner是一個基于輕決策的規(guī)劃器。確實，基于重決策的算法，或者依賴規(guī)則的算法，很容易理解和解釋。但是缺點也很明顯：它可能被困在角落的情況下(陷入局部最優(yōu)解的頻率與限制條件的復(fù)雜性和數(shù)量密切關(guān)),并不總是最優(yōu)的。在本節(jié)中，我們將通過幾個案例來說明基于輕決策的規(guī)劃算法的優(yōu)點。這些案例都是在百度很多的重決策規(guī)劃模塊中的日常測試中暴露出來的，并最后由輕決策解決。

上圖是EM規(guī)劃器如何在規(guī)劃周期和迭代周期之間迭代以獲得最佳軌跡的實際例子。在這個案例研究中，我們演示了當(dāng)障礙物進入我們的路徑時如何生成軌跡。假設(shè)車輛的速度為10m/s，并且有一個動態(tài)障礙物以10m/s的速度朝我們移動，EM規(guī)劃器將按照一下步驟迭代生成路徑和速度曲線。

原始規(guī)劃（圖15a）。在歷史規(guī)劃速度曲線中，即動態(tài)障礙物進入前，車輛以10m/s的恒定速度直線前進；

路徑曲線的迭代（b）。在這一步中，速度曲線以10m/s的速度從原始曲線巡航。根據(jù)該速度推算，車子和動態(tài)障礙物將在s=40m處相撞，因此，避免該障礙物的最佳方法是在s=40m處從右側(cè)躲避。

速度曲線迭代（c）。從步驟1開始，根據(jù)從右側(cè)避開的路徑曲線，原始根據(jù)其與障礙物的距離來調(diào)整期望速度。因此，如乘客所期望的一樣，車輛速度將減至5m/s以較低速度通過障礙物。

路徑曲線迭代2（d）。在新的速度曲線下，車輛不會在s=40m處通過障礙物，而是在s=30m處通過。因此應(yīng)躲避障礙物的路徑更新為一個新的路徑，以使躲避的距離最大化為s=30。

速度曲線迭代2（e）。在新的路徑曲線下，在s=30m處執(zhí)行而不再需要在s=40m處進行減速。新的速度曲線顯示，車輛可以在s=40處加速，在s=30處仍然能平穩(wěn)通過。

因此，基于這四個步驟生成的最終軌跡整體過程是:在s=30處緩慢躲避障礙物，然后車輛通過障礙物再加速，這才是人類駕駛員在這種情況下可能的操作。
請注意，規(guī)劃并不一定要始終執(zhí)行四個步驟。它根據(jù)不同的場景可以采用更少或者更多的步驟。一般來說，環(huán)境越復(fù)雜，可能需要的步驟就越多。

5. COMPUTATIONAL PERFORMANCE

由于將三維狀態(tài)橫向速度問題分解為狀態(tài)橫向速度和狀態(tài)速度兩個二維問題，極大降低了EM Planner的計算復(fù)雜度。因而具有很高的規(guī)劃效率。假設(shè)我們有$n$個障礙物、$M$條候選路徑和$N$條候選速度曲線，則該算法的計算復(fù)雜度為$O(n(M+N))$。

6. CONCLUSION

EM Planner是一個基于輕決策的規(guī)劃算法。與其他基于重決策的算法相比，Em規(guī)劃器的優(yōu)勢在于能夠在復(fù)雜的多障礙場景下執(zhí)行。當(dāng)基于重決策的方法試圖預(yù)先確定如何處理每個障礙物時，困難是顯而易見的：（1）很難理解和預(yù)測障礙物如何與主車相互作用，因此它們的跟隨運動難以描述，很難用約束和規(guī)則來表征；（2）當(dāng)多個障礙物阻塞道路時，無法找到滿足所有預(yù)定決策的軌跡概率大大提高，從而導(dǎo)致規(guī)劃的失敗。

自動駕駛汽車的一個關(guān)鍵問題是對安全性和通過性的挑戰(zhàn)，嚴格的規(guī)則增加了汽車的安全性，但降低了通過性，反之亦然。以換道為例，如果后面有車輛，只要有簡單的規(guī)則，就可以很容易地暫停換道過程。這樣可以保證安全，但大大降低了通過性。本文所描述的EM-planner，在解決潛在決策與規(guī)劃不一致的同時，也提高了自主駕駛車輛的通過性。

EM計劃器通過將3維的（SLT）問題轉(zhuǎn)換為兩個兩維問題（SL和ST），大大降低了計算復(fù)雜度。 它可以大大減少處理時間，從而提高了整個系統(tǒng)的交互能力。

總結(jié)

以上是生活随笔為你收集整理的自动驾驶之轨迹规划6——Apollo EM Motion Planner的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。