標題：Baidu Apollo EM Motion Planner
作者：Haoyang Fan | Fan Zhu2 | Changchun Liu | Liangliang Zhang | Li Zhuang | Dong Li | Weicheng Zhu | Jiangtao Hu | Hongye Li | Qi Kong3
來源：https://arxiv.org/abs/1807.08048
程式碼：https://github.com/ApolloAuto/apollo/tree/master/modules/planning

摘要

基於Baidu Apollo平臺提出一種實時運動規劃系統，該規劃系統包括頂層的多車道和其中的單車道自動駕駛：

1. 系統頂層是一種多車道策略，通過平行計算車道級別軌跡來處理車道變更場景；
2. 在車道軌跡生成器內部，迭代的解決基於Frenet座標的路徑和速度規劃；
3. 針對路徑和速度規劃問題，提出了動態規劃與基於樣條的二次規劃相結合的方法，構造了一個可擴充套件且易於調整的框架，同時處理交通規則、障礙物決策和平滑度問題。

該規劃器可擴充套件至高速公路和城市低速駕駛場景。

---

---

一、簡介

• 高精地圖模組提供了任何人都可以訪問的高精地圖。
• 感知和定位模組提供了動態環境中必要的資訊。
• 運動規劃模組考慮生成一條安全平滑的軌跡再交由汽車的控制模組去執行。
• 運動規劃以安全第一為原則，遵循各種規則。
• 對於規劃的覆蓋範圍:至少提供8秒或者200米的運動規劃軌跡，演算法的反應時間必須在100ms（人是300ms反應時間）內做出反應。
• 必須設有安全應急模組。
• 除了安全以外，乘客的體驗感也很重要，舒適度主要取決於軌跡的平滑性。

---

1.多車道策略

對於L4級別的自動駕駛，變道策略是必須的。一種常見的方法是開發一種搜尋演算法,可以計算所有可能的車道上的代價函式，然後選擇代價最低的一條軌跡。但是該方法存在一些困難。首先，搜尋空間因多車道而增大，導致計算量增大。其次，每個車道的交通規則不同，很難在同一框架下應用相同的演算法的代價函式。此外，避免突然變道的軌跡穩定性也應該被考慮進來。遵循一致的道路駕駛規則是很重要的，以便其他駕駛員知道自動駕駛汽車的駕駛意圖。

通常情況下，多車道策略應涵蓋主動和被迫換道的情況。主動車道變換是routing模組因為要到達最終目的地而發出的換道請求。被動換道時因為當前車道被動態環境阻擋時汽車的自主行為。在主動和被動換道中，我們的目標是提供一個安全、平穩、成功率高的車道變換策略。因此提出了一個並行框架來處理主動和被動的換道。對於候選車道，所有障礙物和環境資訊都投影到Frenet框架上。然後，將交通規則與車道級別的策略繫結。然後在此框架下，每個候選車道基於規劃器都生成一條最佳軌跡。最後，進行多車道軌跡決策確定一個最佳車道。

---

2.路徑-速度(Path-Speed)迭代演算法

在運動規劃中，最優性和效率都是很重要的。因此，許多自動駕駛運動規劃演算法是在Frenet框架下的SLT圖並且藉助於參考線來降低規劃的維度。在Frenet框架下找到最優的軌跡本質上是三維空間約束優化問題。因此有兩種型別方法：直接三維空間優化方法和路徑-速度分解方法。

直接三維空間方法嘗試使用軌跡取樣或lattice search在SLT中找到最佳軌跡。這些方法受到其搜尋複雜度的限制，搜尋複雜度隨著空間和時間搜尋解析度的增加而增加。為了滿足時間消耗要求，必須在增加搜尋網格大小或取樣解析度上做出折衷。因此，所產生的軌跡是次優的。

相反，路徑-速度分解方法是分別將路徑規劃和速度規劃分開。路徑規劃通常考慮靜態障礙。然後，基於生成的路徑[6]產生速度曲線。但當出現動態障礙物時，路徑-速度分解方法可能不是最優的。但是，由於路徑規劃和速度規劃是分離的，因此該方法在路徑和速度規劃上都具有很大的靈活性。

EM Planner迭代優化路徑和速度。在路徑規劃器中,上一個週期的速度曲線被用估計和迎面來的低速動態障礙物的影響。然後，將生成的路徑送到速度規劃器以評估最佳速度曲線。對於高速動態障礙物，EMPlanner出於安全考慮，更傾向於變道躲避而不是接近。

因此，EM規劃器的迭代策略可以幫助解決路徑-速度分解框架下的動態障礙。

---

3.決策和道路規則

在EM Planner中，決策和交通規則是兩種不同的約束。交通規則屬於硬約束，是不能改變的，一些障礙則屬於軟約束，是可以協調的。對於決策模組來說，一些規劃器會直接似乎用數值優化來進行決策。在Apollo EM Planner器中，先做決策再做規劃。**決策過程意在於明確道路意圖並縮小軌跡規劃的搜尋空間來尋找最優的軌跡。**許多包含決策的規劃器嘗試讓汽車自我決策生成相應的狀態。這些方法進一步被分為手寫決策和基於模型的決策。手寫方式有侷限性，不夠靈活。基於模型的方法將汽車狀態離散，並使用資料驅動方式來調整模型。

針對L4級別的自動駕駛，決策模組應具有可擴充套件性和可行性。可擴充套件性是場景表達能力。在多障礙時，很難通過有限的自我汽車狀態集準確的描述決策行為。出於可行性考慮，我們的意思是所生成的決策應包括一個可行的區域，自我汽車可以在該區域內進行動態限制。但是，手寫方式和基於模型的決策都不會產生無障礙軌跡來驗證可行性。

在EM Planner的決策步驟中，我們以不同的方式描述行為。首先，通過一個粗略可行的軌跡來描述自我汽車的移動意圖。然後，使用該軌跡來測量障礙物之間的相互作用。即便情景變得更加複雜，這種基於可行的軌跡的決策也是可擴充套件的。其次，規劃器還將基於軌跡生成凸的可行空間，以平滑樣條曲線引數。基於二次規劃的平滑樣條曲線求解器可用於生成遵循該決策的更平滑的路徑和速度曲線。這保證了可行且平滑的解決方案。

---

二、多車道策略的EM Planner框架

本節中，我們先介紹Apollo EM Planner的整體架構，然後重點介紹車道級別的優化器架構。

在頂層的資料中心,所有來源的資訊都被收集和同步。採集資料後，參考線生成器會生成一系列攜帶交通規則資訊和障礙物資訊的參考線。這個過程要基於從Roting模組的導航資訊和HD地圖。在運動規劃中，先構造Frenet框架。在框架中構建車輛與周圍環境的關係。再將構建的關係傳遞給規劃器。規劃器模組執行速度和路徑規劃。在路徑規劃過程中，周圍環境的資訊被投影到Frenet框架中（E-step）。然後基於投影的資訊產生一條平滑的路徑(M-step)。

速度規劃也是類似，在速度優化過程中，一旦路徑優化器生成了一條平滑的路徑，障礙物就會投影到station-time graph 中（E-step）。 然後，速度優化器將生成平滑的速度曲線（M-step）。 結合路徑和速度曲線，我們將獲得指定車道的平滑軌跡。 在最後一步中，所有車道級別的最佳軌跡都將傳送到參考線軌跡決策器。 根據當前的汽車狀態，法規和成本，最後將路徑和速度結合將獲得一條平滑的軌跡。最後一步中將所有得到的軌跡傳送到參考線決策器，根據當前汽車狀態、法規和每條軌跡的代價，軌跡決策器將選擇最優的軌跡交由汽車去執行。

---

三、車道級別的EM Planner

在本節中，我們將討論車道級別優化問題。圖3展示了車道級別規劃內的路徑-速度EM迭代。迭代過程包括兩個E-step和兩個M-step。軌跡資訊將在規劃週期之間迭代。各子模組的解釋如下：

1. 在第一個E-steps中，動態和靜態障礙物都被投影到Frenet框架。靜態障礙物會直接從笛卡爾座標系轉換到Frenet座標系，而動態zhangaiwu 的資訊則以其運動軌跡來描述。在Apollo框架中，可以預測動態障礙物的移動軌跡。考慮到之前的週期規劃軌跡，我們可以在每個時間點估計出動態障礙物和汽車的位置。然後動態障礙物和汽車在某個時間點的重疊可以被對映到Frenet框架中。另外，動態障礙物的出現會最終導致自車做出避讓的決策。因此，出於安全考慮，SL投影只考慮低速和來向障礙物，而對於高速的動態障礙物，EM Planner的平行變道策略會考慮這種情景。

2. 在第二個E-steps中，基於生成的路徑在s-t座標系下，所有障礙物包括高速、低速和即將到來的障礙物都將被估計,生成速度曲線。如果障礙物軌跡和已經規劃的路徑重疊，將在s-t座標系下對應區域將會在進行重新生成。

3. 在兩個M-steps中，通過動態規劃和二次規劃生成路徑和速度曲線。儘管我們將障礙物投影到SL和ST座標系下，但是最優的路徑和速度解仍在非凸空間中。因此，我們使用動態規劃先獲得一個粗略解，同時，這個解可以提供避讓、減速、超車等障礙決策。通過這個粗略的解，可以構建一個凸包，然後使用基於二次規劃的樣條優化器來求解。

接下來的部分將會詳細介紹框架中的步驟。

---

SL和ST投影（E-step）

SL投影

SL投影是基於類似於G2（曲率連續）平滑參考線。在笛卡爾空間中，障礙物和車的狀態用（x,y,θ）以及曲率(k)和曲率的導數(dk)描述。然後將它們轉換到Frenet座標系中(s, l, dl, ddl, dddl)，它們代表了當前的狀態，橫向和橫向導數。由於靜態障礙物的位置不會隨著時間改變，因此轉換很簡單。對於轉換動態障礙物，我們需要藉助上一個週期的軌跡來實現。把上一個週期的運動軌跡被投影到Frenet座標系上，以提取狀態方向的速度曲線。給定確定的時間就會估計出汽車狀態的座標。估計的汽車狀態座標將會幫助估計動態障礙物的影響。一旦汽車狀態座標同時與障礙物軌跡發生交叉時，SL地圖上的陰影區域將被標記。在此，交叉區域被定義為汽車與障礙物的重疊區域。例如，如圖4所示，從預測模組估計的迎面而來的動態障礙物和相應的軌跡被標記為紅色，小車標記為藍色。首先將迎面而來的動態障礙物的軌跡隨時間離散成若干個軌跡點，然後將這些軌跡點投影到Frenet座標上。一旦我們發現自車的位置座標與投射的障礙點有互動作用，重疊區域（圖中紫色部分）將在Frenet座標中標記出來。

---

ST投影

ST投影可以幫助我們評估自車的速度曲線。當路徑優化器在Frenet座標中生成光滑的路徑曲線後，與自車有互動的靜態障礙物及動態障礙物的軌跡都將被投影在路徑上。同理，這種互動也定義為包圍盒的重疊。紅色區域表示在2s處距離自車40m遠切入規劃路徑的動態障礙物ST資訊，綠色表示在自車後的動態障礙物ST資訊，M-step將會在剩下的區域找到可行光滑的最優解。

---

M-Step DP Path（E-Step）

M-step求解Frenet座標系下的最優路徑規劃，實際上在一個非凸的區間（從左和從右避讓是兩個區域性最優情景）就是找到一個最優的l = f(s)方程。主要包括兩步：基於動態規劃的路徑決策和基於樣條的路徑規劃。

基於動態規劃的路徑步驟提供一條粗略的路徑資訊，其可以帶來可行通道和避障決策，如圖6所示，這一步包括Lattice取樣、代價函式、動態規劃搜尋。

Lattice取樣器是基於Frenet座標的。如7圖所示，首先在車輛之前對多行lattice進行取樣。不同行之間的點通過五次多項式來平滑連線。行之間的間隔距離取決於速度，道路結構，變換車道等。該框架允許根據場景自定義取樣策略。例如，車道變換的取樣間隔可能比當前車道行駛更長。此外，出於安全考慮，路徑總長可以達到200m或者覆蓋8s的行駛長度。

構造完lattice後，通過代價函式的總和來評估每條路徑。我們使用從SL投影得到的資訊結合交通規則、汽車動力學模型去構建函式。總的代價是平滑度，避障和車道代價函式的線性組合。

每段Lattice路徑的代價通過光滑程度、障礙物避讓、車道代價來評價：

而光滑程度又通過以下方程來衡量，一階導表示朝向偏差，二階導表示曲率，三階導表示曲率導數：

障礙物的代價由以下方程給出，方程中的d由自車bounding box到障礙物bounding box的距離表示。
Cnudge定義為單調遞減函式。出於安全考慮，dc設定保留緩衝區。可根據情況微調dn範圍。Ccollision是碰撞代價函式，可幫助檢測不可行的路徑。

車道代價由以下方程給出，主要是考慮在道路上與否以及與參考線之間的差異，一般是與車道中心線的差異。車道代價函式包括兩個部分：引導線代價和車道代價。引導線定義為周圍沒有障礙物時的理想路徑，通常這條線提取的時道路中心線。車道代價函式通常被道路邊界決定，道路外的路徑點將受到很高代價的懲罰：

最終總的路徑代價函式包含平滑度、障礙和車道代價。然後邊緣代價會用於通過動態規劃去選擇最低成本的候選路徑。候選路徑也將確定障礙物決策。

---

樣條QP路徑（M-Step）

樣條QP路徑步驟是對動態規劃路徑的改進。在動態規劃路徑中，會根據所選的路徑生成可行的走廊。然後基於樣條QP步驟將在該走廊中生成一條平滑路徑，如圖8所示：

通過QP求解器求解具有線性約束的目標函式生成最優的QP路徑。如圖9所示：

QP路徑的目標代價函式是平滑度代價和引導線代價的線性組合。該步驟中的引導線是動態規劃挑選出來的路徑。引導線提供了避開障礙物的估計值。在數學上QP路徑步驟優化以下函式：

g(s)是動態規劃的結果。f(s),f"(s), f"’(s)分別為航行、曲率和曲率的導數。該目標函式描述了避開障礙物和平滑度之間的平衡。

QP路徑中的限制條件包含了邊界約束條件和動力學可行性。這些限制被應用於f(s),f"(s), f"’(s)在一系列狀態座標s0，s1，…，sn。為了提取邊界約束，提取各狀態點的可行區間。這個可行區間在每一個點被描述為Llow,i<=Lhigh,i。在EM Planner中，車輛被看成自行車模型。因此僅僅提供l=f(s)的範圍是不夠的，因為車的方向也很重要。

在圖10中，為了保持邊界約束的凸包性和線性，我們在汽車的前後兩後端分別新增兩個半圓，其中前輪到後輪的中心距離為Lf和車輛寬度為w。

然後左前角的橫向位置由以下表示：

θ是汽車和道路狀態方向之間的航向角。可用下面的不等式進一步近似地線性約束：

類似地，線性優化剩下的三個角度。線性約束足夠好是因為 θ通常很小。當 θ<pi/12時，在橫向上有限性約束比沒有線性約束的要小2-3cm。

f"(s)和f"’(s)的區間限制可表示為動力學約束，因為曲率與曲率導數有關。除了邊界約束外，生成的路徑還可以匹配汽車的橫向初始位置和導數(f(s0),f"(s0), f"’(s0))。由於所有約束相對於樣條曲線引數都是線性的，因此可以使用二次規劃求解器來快速解決問題。光滑樣條和二次規劃的細節問題將在第VI部分介紹。

---

DP速度優化器（M-Step）

M-Step速度規劃器在ST圖裡面生成一個速度曲線，該速度曲線表示為相對於時間S(t)的狀態函式。與路徑規劃器相似，在ST圖上尋找最優速度曲線也是一個非凸問題，所以採用DP與QP相結合的方法在ST圖上尋找一個光滑的速度曲線。在圖12中，DP速度規劃步驟包括代價函式，ST圖網格和DP搜尋。生成分段性速度曲線，可行的走廊和障礙物速度決策。如圖11所示。在QP速度時採用DP生成的速度曲線作為引導線，利用可行走廊生成凸域。

具體來說，障礙物資訊首先在ST圖上離散成網格。將（t0,t1…tn)表示為在時間軸上具有間隔dt的等距評估點。分段性速度曲線函式在網格上表示為S=（s0,s1,…sn）。此外，利用有限差分方法對導數進行了逼近。

目標是在ST圖中優化一個帶有約束的代價函式。準確地說，DP速度優化器的代價表示為：

第一項是速度保持代價。表示當沒有障礙物或者沒有紅綠燈時，車輛應按照指定的速度行駛。Verf 描述的是參考速度，參考速度由道路速度限制、曲率和其他交通規則來確定。g函式被用來對小於或者大於Verf值施加的不同懲罰。加速度a和加加速度jerk描述了速度曲線的平滑度。最後一個Cobs值描述了總障礙代價，對車輛到所有的障礙物的距離進行評估，確定總障礙物代價。

動態規劃搜尋空間必須在動力學約束範圍內。動態約束包括加速度、加加速度jerk和單調性約束。因為我們要求生成的軌跡在道路上不執行倒車的操作，只能在泊車或者其他指定情況下直行倒車。搜尋演算法是向前的，基於車輛動態約束的一些必要修剪也被應用以加速該過程。詳細的搜尋過程在此不展開討論。

---

QP速度優化器（M-Step）

---

由於分段線性速度曲線不能滿足動力學要求，需要用QP來解決這個問題。在圖13中，樣條速度包括三個部分：代價函式、線性約束條件和樣條QP求解器。

代價函式可以這樣表示：

第一項是DP速度參考線Sref和最後需要生成的S之間的距離。加速度和jerk是速度曲線平滑度。因此，目標函式是為了平衡引導線和平滑度。

QP優化器是帶有線性約束的。QP速度優化器的限制條件包括了邊界限制、初始速度和加速度匹配的約束：

第一個約束是單調性。第二、第三和第四 五個約束是來自交通規則和車輛動力學約束的要求。在完成代價函式和約束後，QP求解器將生成一條平滑可行的速度曲線，如圖14結合路徑曲線，EM規劃器將給控制模組生成一條平滑的軌跡。

---

求解二次規劃問題筆記

出於安全的考慮，我們評估了大概100個不同位置或時間點的路徑和速度。約束的數量超過600。對於選擇所需的路徑和速度優化器，我們發現五項多項式是比較合適的。樣條曲線包括3到5個多項式，大約有30個引數。因此，二次規劃問題有相對較小的目標函式，但約束條件較多。因此，主動設定QP求解器有助於解決該類問題。除了加速二次規劃求解外，我們還將上一個迴圈的計算結果作為hot start（牛頓法）。QP問題平均可以在3ms內解決，滿足了我們對時間消耗的需求。

---

求解DP和QP非凸問題筆記

DP和QP在非凸領域都有各自的侷限性。DP和QP的結合將充分利用兩者的優點，以達到最理想的解決方案。

• DP：如前文所述，DP演算法依賴於一個取樣步驟來生成候選解。由於處理時間的限制，取樣網格限制了候選樣本的數量。有限網格內的優化會得到一個粗糙的DP解。換句話說，DP不一定能所有的場景下都取得最優解。例如，DP求出從左側避開障礙物的路徑，但不能選擇具有最佳距離的路徑。
• QP：相反，QP是基於凸域生成解。如果沒有DP步驟的幫助，它是不可用的。例如，如果一個障礙物在車輛前面，QP需要一個決策，例如從左側變道，從右側變道，跟隨或者超車，來生成其約束。隨機或基於規則的決策容易使QP陷入失敗或者區域性極小值。
• DP+QP：DP+QP演算法將兩者的侷限性降到最低：（1）EM規劃器首先用DP在網格內搜尋，以求得粗略的解；（2）DP的結果被用於生成凸域並引導QP；（3）QP被用於在凸域內尋找全域性最優解。

---

四、案例學習

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

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

1. 原始規劃（圖15a）。在歷史規劃速度曲線中，即動態障礙物進入前，車輛以10m/s的恆定速度直線前進；
2. 路徑曲線的迭代（b）。在這一步中，速度曲線以10m/s的速度從原始曲線巡航。根據該速度推算，車子和動態障礙物將在s=40m處相撞，因此，避免該障礙物的最佳方法是在s=40m處從右側躲避。
3. 速度曲線迭代（c）。從步驟1開始，根據從右側避開的路徑曲線，原始根據其與障礙物的距離來調整期望速度。因此，如乘客所期望的一樣，車輛速度將減至5m/s以較低速度通過障礙物。
4. 路徑曲線迭代2（d）。在新的速度曲線下，車輛不會在s=40m處通過障礙物，而是在s=30m處通過。因此應躲避障礙物的路徑更新為一個新的路徑，以使躲避的距離最大化為s=30。
5. 速度曲線迭代2（e）。在新的路徑曲線下，在s=30m處執行而不再需要在s=40m處進行減速。新的速度曲線顯示，車輛可以在s=40處加速，在s=30處仍然能平穩通過。

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

---

五、計算效能

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

---

六、結論

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

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

EM計劃器通過將three-dimensional-station-lateral-speed問題轉換為兩個two-dimensional-station lateral/station-speed問題，大大降低了計算複雜度。 它可以大大減少處理時間，從而-tisheng 整個系統的互動能力。