論文地址：https://arxiv.org/abs/1807.08048
論文類型：Motion Planning

目錄：

文章目錄

• 簡介
• 多車道EM Planner框架
• • 整體框架
  • 多車道策略
  • 路徑-速度迭代算法
  • 決策和交通規則約束
• 車道級EM PLanner框架
• • 整體框架
  • SL和ST投影（E-step）
  • • SL投影
    • ST投影
  • DP路徑（M-step）
  • 樣條QP路徑（M-step）
  • DP速度求解（M-step）
  • QP速度求解（M-step）
  • 解QP問題的說明
  • DP和QP非凸問題的說明
• 案例分析
• 總結

簡介

EM Planner是Apollo面向L4的實時運動規劃算法，該算法首先通過頂層多車道策略，選擇出一條參考路徑，再根據這條參考線，在Frenet坐標系下，進行車道級的路徑和速度規劃，規劃主要通過Dynamic Programming和基于樣條的Quadratic Programming實現。EM Planner充分考慮了無人車安全性、舒適性、可擴展性的需求，通過考慮交通規則、障礙物決策、軌跡光滑性等要求，可適應高速公路、低速城區場景的規劃需求。通過Apollo仿真和在環測試，EM Planner算法體現了高度的可靠性，和低耗時性。

多車道EM Planner框架

整體框架

所有規劃需要的信息在EM Planner的頂層匯集，然后參考線生成器會生成一些基于障礙物和交通規則的候選車道級參考線，這個過程主要是依賴于高精度地圖和Routing模塊給出的全局規劃結果。以下是車道級的規劃過程：

首先會基于給定參考線生成Frenet坐標系，通過給定參考線將所有的自車信息和環境信息轉換到參考線下的Frenet坐標系。

接下來所有的車道級信息將會傳遞給車道級最優求解器，該求解器會求解最優路徑和最優速度。在求解最優路徑時，周圍環境信息將會被投影到Frenet坐標系（E-step），然后基于投影的信息生成一條光滑路徑（M-step）。

同樣的，在求解最優速度時，一旦生成了一條最優路徑，障礙物就會被投影到ST圖中（E-step），然后最優速度求解器會生成一條光滑的速度規劃（M-step）。結合路徑和速度規劃結果，就生成了一條給定車道的光滑軌跡。

最后一步會將所有的車道級軌跡傳遞給參考線軌跡決策器，基于當前車輛狀態、相關約束和每條軌跡的代價，軌跡決策器會決定一條最優的軌跡。

多車道策略

利用搜索算法【2】【3】結合代價估算形成變道策略是一種比較常見的處理變道問題的方法，但是這種方法存在計算量大、難以適用交規以及前后決策可能缺少連貫性等特點。Apollo的解決辦法是將多車道策略劃分為兩種類型：無法通行的被動變道，和能夠通行的主動變道。被動變道一般由道路阻擋造成的，通過全局規劃模塊重新生成全局路徑解決；主動變道是考慮動態障礙物而做出的決策。Apollo通過同步生成多條候選車道的方法解決主動變道問題，在Frenet坐標系下，投影障礙物、考慮交規后生成多條車道級的候選路徑，最后傳遞到變道決策器中選擇出一條最優的車道決策

路徑-速度迭代算法

在Frenet坐標下的軌跡規劃實際上帶約束的3D最優求解問題。該問題一般有兩種求解方法：直接3D最優化求解和路徑-速度解耦求解。直接方法【4】【5】試圖在SLT坐標系下使用軌跡采樣或Lattice搜索,這些方法都受到搜索復雜度的限制，因此搜索結果是次優的。而路徑-速度解耦規劃會分別求解路徑和速度的最優解。速度的生成將會在生產的路徑上進行【6】。雖然結果可能也不是最優的，但會在速度和路徑分別求解時更加靈活。
EM Planner迭代地進行路徑和速度最優求解，通過估計和來向、低速障礙物的交互，上一幀的速度規劃將有助于下一幀的路徑規劃。然后將路徑規劃結果再交給速度最優求解器來推算出一個最優的速度結果。

決策和交通規則約束

交通規則是硬約束，而與障礙物的交互是軟約束。一些決策方法直接考慮的是數值上的最優解【7】，也有像【5】一樣同時進行規劃和決策。而Apollo EM Planner的決策是優先于規劃的，決策模塊將會為規劃帶來更明確的意圖，減少最優求解的搜索空間。決策部分的第一步是將車輛的運動意圖用一根粗略、靈活的軌跡來描述。這條軌跡也可以用來估計與障礙物之間的交互，并且當情景更加復雜時，這種基于軌跡的決策方法也是靈活的。第二步是基于決策生成的軌跡來構造一個凸空間，用來做基于樣條光滑的軌跡生成，主要是通過二次規劃來達到迭代生產路徑、速度解的目的。

車道級EM PLanner框架

整體框架

框架包括了一幀規劃中的兩個E-step和兩個M-step，軌跡信息將會在前后兩幀中傳遞，以下是整個車道級規劃的流程：

在第一個E-step中，障礙物會被投影到車道Frenet坐標系，障礙物包括了靜態障礙物和動態障礙物。靜態障礙物會直接從笛卡爾坐標系轉換到Frenet坐標系，而動態的信息則以其運動軌跡來描述。通過上一幀的預測信息，和自車的運動信息，可以估算自車和動態障礙物在每個時間點的交互情況，軌跡重疊的部分會被映射到Frenet坐標系中。初次之外，在最優路徑求解過程中，動態障礙物的出現會最終導致自車做出避讓的決策。因此，出于安全的考慮，SL投影只考慮低速和來向障礙物，而對于高速的動態障礙物，EM Planner的平行變道策略會考慮這種情景。

在第二個E-step，所有的障礙物都會在ST中與生成的速度信息進行估計，如果對應的ST中重疊部分，那么對應區域將會在ST中進行重新生成。

在兩次M-step過程中，通過Dynamic Programming和Quadratic Programming生成路徑和速度規劃。然而在進行投影的SL和ST坐標內求解時非凸的，因此，為了解決這個問題，首先使用Dynamic Programming獲得一個粗略的解，同時這個解也能夠提供諸如避讓、減速、超車的決策。通過這個粗略的解，可以構建一個凸的通道，然后使用基于Quadratic Programming的樣條最優求解。

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

SL和ST投影（E-step）

SL投影

SL投影是基于類似于【3】中的G2光滑參考線（曲率導數連續）。給定一個時刻，如果自車與預測的障礙物軌跡有重疊區域，那么這個重疊區域將會在SL坐標系被標注為與動態障礙物的估計交互區域。這個區域可以理解為自車和動態障礙物的包圍盒的重疊區域。圖4展示了這一種案例，紅色代表動態障礙物的預測軌跡，用離散點來表示；藍色表示自車的狀態。

ST投影

ST投影用于幫助我們估計自車的速度規劃。當生成了一條光滑的路徑以后，與自車有交互的動態障礙物和靜態障礙物都會被投影到路徑上，同理，這種交互也定義為包圍盒的重疊。如圖5，這是一個ST圖投影案例。

紅色區域表示在2s處距離自車40m遠切入規劃路徑的動態障礙物ST信息，綠色表示在自車后的動態障礙物ST信息，M-step將會在剩下的區域找到可行光滑最優解。

DP路徑（M-step）

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

基于Dynamic Programming的路徑步驟提供一條粗略的路徑信息，其可以帶來可行通道和繞障決策，如圖6所示，這一步包括Lattice采樣、代價函數、Dynamic Programming搜索。

Lattice采樣基于Frenet坐標系，多行的點在撒在自車前。如圖7所示，行與行之間的點使用五次方多項式連接，而行與行之間的間隔取決于自車速度、道路結構、是否換道等等。出于安全考慮，路徑總長可以達到200m或者覆蓋8s的行駛長度。

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

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

障礙物的代價由以下方程給出，方程中的d由自車bounding box到障礙物bounding box的距離表示。

車道代價由以下方程給出，主要是考慮在道路上與否以及與參考線之間的差異，一般是與車道中心線的差異：

樣條QP路徑（M-step）

基于樣條的路徑可以理解為是Dynamic Programming更精細的版本。通過DP采樣出的路徑生成一條可通行通道，然后在通道中利用基于Quadratic Programming的樣條曲線生產光滑路徑。具體實例如圖8所示，步驟流程可由圖9所示：

QP的目標函數為：

其中$g(s)$為DP規劃的路徑，$f(s)$的一階導表示朝向、二階導表示曲率、三階導表示曲率的導數。該函數描述了避讓障礙物和曲線光滑性之間的權衡。

QP的約束包括邊界約束和動力學可行性。這些約束都會施加在每個s處，通過限制l來將車輛限制在車道內。由于EM Planner使用的是自行車模型，因此這樣對l的限制也是不夠的。如圖10所示，為了使得邊界約束變凸并且線性，在自車的前后兩端各增加了一個半圓。前輪到后輪中心的距離用$l_f$表示，車寬用w表示，因此車的左前角的橫向位置可以用以下方程給出：

通過線性化可以變為：

同理，其余三個角的位置都可以被線性化，顯然因為$\theta$足夠小，小于pi/12，因此可以這樣線性化。

$f(s)$的二階導和三階導與動力學可行性相關，除了邊界條件以外，生成的路徑還應該和自車的初始條件相匹配。因為所有的約束都是線性的，所以使用Quadratic Programming求解非常迅速。

具體的光滑樣條曲線和QP問題可以在附錄中查閱。

DP速度求解（M-step）

M-step的速度規劃是在ST圖中求解最優速度規劃，即求解出最優函數$S(t)$。與求解最優路徑相似，在ST圖中求解最優速度規劃也是非凸的最優化問題。同樣也采用Dynamic Programming配合樣條曲線Quadratic Programming來找到光滑速度規劃。圖12是速度求解的pipeline：

DP速度求解包括代價函數、ST柵格圖以及Dynamic Programming搜索。生成的結果包括分段線性的速度規劃、可通行通道以及障礙物速度決策。如圖11所示，該結果在QP中用來作為參考速度規劃，通過該參考速度生成凸的區域。

在柵格圖中，使用有限差分法來估計速度、加速度和jerk：

從DP生成的速度中選擇出最優的一條的方法是最小化以下的代價函數：

第一項是速度誤差，g用來懲罰與$V_{r}ef$的不同的誤差。第二項、第三項用來描述曲線的光滑程度。最后一項用來描述障礙物代價，以到障礙物的距離來衡量。

DP搜索空間也收到車輛動力學約束，并且也有單調性約束，因為不希望車輛倒退。一些對于動力學約束的必要簡化也用來加速算法。

QP速度求解（M-step）

因為分段線性的速度規劃不能滿足動力學的要求，所以需要使用Quadratic Programming來填補動力學空缺。圖13是樣條曲線QP速度求解的pipeline：

QP速度求解包括三部分：代價函數、線性約束以及樣條曲線QP求解器。
除了初始條件約束以外，主要有以下的邊界約束：

第一個約束是單調性約束；第二、第三、第四約束主要是交通規則和車輛動力學約束。通過約束、cost函數計算以后，spline QP speed會生成一條如圖14中的光滑可行的速度規劃。

結合路徑規劃，EM Planner最終會生成一條光滑軌跡。

解QP問題的說明

為了安全考慮，路徑和速度大概在100個不同的位置或時間點，那么約束就有超過600個。對于速度、路徑求解，分段五次多項式已經足夠，因此樣條曲線大概有3-5個多項式，大概就有30個參數。因此Quadratic Programming就變成了相對小的目標函數，和相對大的約束。QP能比較好的解決這個問題，并且使用了上一幀的解作為熱啟動，加速求解過程。實踐中，QP問題解的平均時間3ms。

DP和QP非凸問題的說明

在非凸問題上，DP和QP都有他們單獨的限制。DP和QP的組合，能夠很好吸收兩者優點，并求得一個理性解。

• DP:DP的優劣受到撒點分辨率和時間分辨率的影響，通常在運行時間限制的情況下，一般只會得出一個粗糙解而非最優解，比如會從障礙物左側繞開，但并不是按照最完美的路徑繞開。
• QP:QP需要在凸空間求解，因此必須借助DP的解來形成凸空間。隨機的或者基于規則的決策，通常會給QP帶來非凸的空間，因此解QP問題會失敗或者陷入局部最優。
• DP+QP:（1）通過DP尋求粗糙解；（2）DP解能夠生成凸空間；（3）QP在DP解形成的凸空間內，很大可能能夠獲得全局最優解。

案例分析

圖15展示了EM Planner在規劃周期內，幀與幀之間完成最優軌跡規劃的過程。

假設自車以10m/s的速度行進，一動態障礙物沿著相反方向朝著我們以同樣10m/s的速度駛來，EM Planner按以下步驟迭代生成速度和路徑規劃：

歷史規劃（圖15-a）：在動態障礙物出現之前，自車以恒定速度10m/s向前行駛。

路徑規劃迭代1（圖15-b）：基于當前車速和動態障礙物的車速，兩者將會在S=40m處相遇，因此，最好的方法是在S=40m處繞開障礙物。

速度規劃迭代1（圖15-c）：基于路徑規劃結果，即從右側避開障礙物，自車將調整其速度規劃，在避開障礙物之前減速到5m/s。

路徑規劃迭代2（圖15-d）：由于產生了新的速度規劃，自車將不再會與動態障礙物在S=40m處避開，而會在一個新的位置S=30m處避開障礙物。因此，路徑規劃結果也將會隨速度規劃改變而重新更新。

速度規劃迭代2（圖15-e）：由于路徑規劃已經更新，新的繞障位置在S=30m處，因此在S=40處減速也就沒有必要了，新的速度規劃使得自車可以在S=40m處加速而在S=30m處形成一個光滑的繞障。

經過迭代之后，最終車輛將在S=30m處減速繞障，并且繞障結束之后會加速，這樣一個過程和人類駕駛員的表現很相似。
但值得注意的是，并不是每次規劃都必須采取如上四步驟，根據場景不同可能會產生更多或更少的步驟。一般而言，場景越復雜，所需要的步驟就越多。

總結

EM Planner是一種基于弱決策的算法，相比于強決策算法，EM Planner在復雜場景、多障礙物情況下表現更好。強決策依賴于提前制定出的決策行為，并且有難以理解和預測與障礙物交互的缺陷、難以滿足大量障礙物阻擋生成基于規則的最佳軌跡的缺陷。
EM Planner通過將三維規劃問題轉化為兩個二維規劃問題，顯著地降低了運算復雜度，因此會帶來運行時間的壓縮和整個系統的可交互性。

總結

以上是生活随笔為你收集整理的Apollo EM Planner阅读笔记的全部內容，希望文章能夠幫你解決所遇到的問題。

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