1. 概述

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

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

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

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

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

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

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

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

1.1. Multilane Strategy

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

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

并將交通規則與候選車道綁定；

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

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

1.2. Path-Speed Iterative Algorithm

基于Frenet frame（SLT）方法的軌跡規劃一般分為2種：直接在3維空間下優化求解，或將路徑和車速解耦求解（軌跡包含兩部分：路徑和路徑上每個點的速度）。

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

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

1.3. Decisions and Traffic Regulations

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

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

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

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

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

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

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

2. EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY

本節講EM planner的多車道 multilane strategy架構（對1.1和1.3的重復描述，但更詳細了）：

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

基于reference line生成對應的Frene Frame；

將障礙物等環境信息投影到path的Frenet frame中；

生成平滑path profile；

將障礙物等環境信息投影到speed的Frenet frame中；

生成平滑speed profile。

reference line decider會在所有line-level的最佳軌跡中，根據車輛狀態、交通規則、代價函數選擇出最佳軌跡

3. EM PLANNER AT LANE LEVEL

本節將EM planner的單車道 lane level架構：

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

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

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

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

3.1. SL and ST Mapping (E-step)

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

下圖中大概3s左右會出現碰撞：

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

3.2. M-Step DP Path

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

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

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

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

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

3.3. M-Step Spline QP Path

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

目標函數：

• path的QP本質是優化一個帶有線性約束的目標函數，其代價函數如下?？梢钥闯?#xff0c;其前3項是盡量平滑（$f^{\prime }(s)$是航向角，$f^{\prime \prime }(s)$是曲率，$f^{\prime \prime \prime }(s)$是曲率變化率。曲率其實是考慮了車輛的動力學可行性）；第4項是盡量和DP的path保持一致（不要偏離太遠，以免發生碰撞）。

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

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

• dynamic feasibility：DP給出了由很多points（每個row上一個point）組成guidance line，QP可以此為基礎調整每個point在L方向的位置以實現平滑path。但調整是有可行范圍的，該范圍可量化為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。這也是個非凸問題，同樣可以使用動態規劃+二次規劃生成speed profile。

• 動態規劃包含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的代價函數，其中：

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

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

3.5. M-Step QP Speed Optimizer

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

其代價函數如下：

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

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

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

3.6. Notes on Solving Quadratic Programming Problems

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

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

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

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

4. CASE STUDY

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

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

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

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

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

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

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

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

5. COMPUTATIONAL PERFORMANCE

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

6. CONCLUSION

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

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

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

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。