本文主要是對英文論文【Baidu Apollo EM Motion Planner】（By Haoyang Fan, Fan Zhu）的學習總結。
自動駕駛的技術，從軟體層面來說，分成基於嵌入式系統的軟體平台部份，以及演算法部份。前者提供類似OS（如Windows、MacOs，或手機上的IOS、Android）一樣的執行支持和資源分配管理功能，後者更接近於提供具體功能的APP。
自動駕駛的演算法一般主要分成感知、融合、規劃、控制，其中規劃處於承上啟下的位置，作用是根據感知結果和環境資訊，產生車輛的移動軌跡，從而讓控制演算法按照這個軌跡控制車輛的移動。
這篇文章說的就是規劃演算法之中一個常用的演算法：EM Planner。它針對的是L4級別的自動駕駛場景。
EM planner同時覆蓋多車道和單車道的自動駕駛，基於EM-type iterative algorithm。
一、從總體來看
整體思路：
- 平行計算每條車道的軌跡，基於Frenet frame來叠代計算最優的path和speed。
- 比較每條車道的軌跡，判斷換道策略。
- 計算最優的path和speed：結合動態規劃和基於樣本的二次規劃求解。
主要考慮的自動駕駛安全形度 ：
- 交通法規
- 覆蓋範圍：8秒或200公尺的軌跡
- 工作周期：100ms內
- 緊急安全：當前方輸入模組出錯，輸出緊急保險策略
阿波羅頂層軟體架構：
考慮的駕駛體驗及最佳化方向：
- 場景覆蓋（基本行駛、換道、擁堵等）
- 交通法規
- 舒適度（軌跡的smoothness）
多車道策略
對於L4自動駕駛，必須考慮換道策略。要決定車輛在不同車道中，走哪一條車道的哪個軌跡，常規做法是在所有車道中，基於cost來搜尋最優軌跡，但這樣的弊端在於：
- 搜尋空間太大成本高；
- 交規在不同車道是不同的；
- 應避免突然換道。
EM中的換道策略分成主動換道和被動換道，主動換道由routing模組觸發，預設車道被阻擋則觸發被動換道。
所有障礙物和環境資訊都會投射到每個備選車道的Frenet frames，每個車道再各自符合交規，每個車道都會根據cost生成一條最優軌跡，再選出全域最優軌跡。
求解最有軌跡的演算法： Path-Speed Iterative Algorithm
在F座標裏面找最優軌跡是一個3D約束最佳化問題，一般有兩種方式：
1、直接求解：
軌跡采樣或柵格（lattice）搜尋。受限於隨著空間時間而增加的搜尋復雜度，因此得出的軌跡是suboptimal次優的。path主要考慮靜態障礙，然後在生成的路徑上計算速度。對於動態障礙可能找不到最優解。
2、path-speed去耦求解：
首先，由最新的speed資訊評估未來的、低速的動態障礙，從而得出基於這個速度的最優path。然後生成的這個path又返回給speed計算針對新的path的最優的speed。
註意，這裏的speed不會再馬上傳給path，所以叠代不是在同一個計算周期裏進行，而是在下一個周期再進行叠代的。這裏最好想一想，理解一下。
對於高速障礙，EM采用換道而不是閃躲來處理，因此對於高速障礙，EM也能處理。至於為什麽要換道而不是閃躲，我猜測是因為閃躲對於動態障礙物的預測精度要求更高，而去耦求解的EM的預測前提是「預測都是基於當前周期的speed和path來進行的」，並不會考慮基於這個預測作出的行動會影響到障礙物的軌跡，這種互動對於高速動態障礙物來說，足夠產生無法忽略的預測偏差，導致不能進行安全閃躲。
決策和交規
EM中，為了平滑軌跡，在規劃開始之前就進行決策。
常見決策有兩種模式：
Hand-tuning decision：可以調，但受限於規模。還有可能出現超出其定義的決策規則。
Model-based decision：將車輛狀態分解成有限的駕駛狀態，用大量數據來訓練模型。
EM的決策同時采用了以上兩種模式。但說到模型應該更多在於預測模組對障礙物軌跡的預測，EM的決策目標還是提供一個大概的可選空間，決策的內容主要是換道、重用、針對每個障礙物判斷是否閃躲超車等。
更抽象來說，EM中的決策，首先將ego car的移動意圖描述成一個粗略但靈活的軌跡，在這條軌跡上評估與障礙的互動，從而保證scalable。然後，planner還會生成一個凸空間，用於在軌跡上平滑樣點參數。最後，一個二次規劃求解器依據decision生成平滑的path和speed。
二、多車道策略的軟體架構
（EM PLANNER FRAMEWORK WITH MULTILANE STRATEGY ）
首先，所有數據匯總到data center，生成車道級的參考線（帶交規和障礙物，基於高精地圖和導航資訊）。對每個車道，首先基於參考線構建F座標系，隨後所有評估均基於此。
隨後的決策和最佳化，均基於場景管理器和工作管理員進行。在一個場景下，按順序執行定義好的tasks，其中就包含各種path和speed的決策和最佳化tasks。
具體來看，經過決策後將可選軌跡空間限定縮小範圍，重構數據再傳給Optimizer（最佳化器），進行path和speed最佳化。
在path最佳化時，周圍資訊投影到F座標，最佳化的目標是在決策限定的空間內生成一條平滑的path，可以想象成閃躲障礙物往前走的遊戲，玩家需要選擇一條路線透過這條狹窄的通道，舒適又快捷地透過。
在speed最佳化時，障礙物投影到ST圖（station-time graph），同樣是為了生成一段平滑的speed profile（也就是盡量減少加速剎車）。
然後，組合path和speed，獲取該車道的平滑軌跡。
最後，所有車道的最佳軌跡匯總，軌跡決策器基於當前車輛狀態、交規、每條軌跡的成本，選擇最優軌跡。
三、車道級的EM Planner
現在分析單個車道的規劃過程。
首先，叠代是發生在不同的cycle之間。計算新軌跡是基於上一個周期計算的軌跡結果來開始的。
- SL投影：
障礙物投影到F座標。
直接透過Cartesian-Frenet 轉換（笛卡爾座標到F座標）。
在Apollo中透過障礙物移動軌跡描述，結合上一個cycle計算得到的軌跡，可以評估動態障礙物和ego car之間的在不同時間點的位置關系。其中兩者重疊的部份會被標記在F座標中。
（由於動態障礙物往往會導致閃避nudge，所以SL投影裏只會考慮低速和靠近中的障礙物）
2. ST投影：
根據生成的path，將所有障礙物（包含高速的）投影到station-time frame，這個ST圖是基於參考線做的，但無論是障礙物的軌跡，還是自車的軌跡，都是基於每條path來儲存及對比是否重疊的以及何時重疊的。
如果障礙物軌跡與規劃軌跡重疊，便標記出這個區域。
3. 兩個M-Steps：
透過動態規劃獲取一個rough solution（從此獲取障礙物策略：閃躲、讓道、超車等），然後基於此決定二次規劃樣點最佳化器的凸空間主體（a convex hull for the quadratic-programming-based spline optimizer）。
（雖然所有計算都基於凸空間——內部所有點的連線線都在空間內——但實際上最優軌跡在非凸空間裏。）
4. 以上模組再詳細來分析一下：
A. SL and ST Mapping (E-step)
SL投影基於G2(continuous curvature derivative) 平滑參考線。
在笛卡爾座標系，障礙物和自車狀態透過定位和朝向(x,y,θ) ，車道曲度及曲度變化率(κ, dκ) 來描述。
在F座標下，這些資訊轉換成(s, l, dl, ddl, dddl) ，即station、lateral及它的導數。
靜態障礙物與時間無關，因此座標轉換能直接進行。
動態障礙物的座標轉換需要上一次計算出的自車軌跡。首先自車軌跡投射到F座標，獲取the station direction speed profile（提供自車在某個時間點的station座標，從而評估與動態障礙物的相對關系）。自車station座標與障礙物軌跡重疊部份就可以計算出來，如下圖紫色部份：
ST投影能幫助評估自車的speed profile。
當path投影到F座標後，若障礙物軌跡會跟自車產生互動，就會在圖上顯示。
上圖中，一個障礙物在2秒後駛入自車道40m前；空白部份就是speed profile的可選空間。
B. M-Step DP Path
尋找橫向座標 l = f(s) 的最優解（在SL空間中基於station座標計算），包含兩步：
- 動態規劃
- spline-based 規劃
（但實際上Apollo 6.0之中，path的DP過程被簡化成獲取粗略可選空間的path boundary，並不需要復雜的cost函式最佳化求解過程。）
包含cost計算、lattice采樣、動態搜尋。其中lattice采樣需要在自車前獲取數row的點，不同row的點平滑連線。row裏撒點的間距由速度、道路結構、換道等決定，可以自訂。距離覆蓋8秒或200公尺。
Lattice建立後，每個圖邊緣透過cost函式評估——基於SL投影、交規、車動態資訊。總edge cost函式是「平滑性、避障、車道成本」的線性組合。
平滑cost函式中，f ′ (s)代表車道與自車的偏差角， f ′′ (s)是車道曲率。w則代表不同項的權重。
障礙物cost函式，與障礙物距離d大於設定的安全值dn時為0，小於碰撞距離dc則是無窮大。
參考線cost，代表與參考線的距離成本，其中g(s)是參考線函式。
C. M-Step Spline QP Path
QP的目標，是在DP產生的path可選空間中，透過QP spline solver對由線性化約束的目標函式進行最佳化，在其內生成一條平滑的path。
QP的目標函式是平滑cost和guidance line cost的線性組合。後者用到的比較項是DP產生的可選空間裏的大量path，用於評估障礙物的閃避距離。然後透過最佳化找最優解的方式，找出其中cost最低，也就是最優的path。
由於車輛的朝向會影響車輛邊界與周圍的位置關系，也就是需要確定車輛四個角與station點的關系，而不只是l=f(s)。
這樣就需要知道車頭朝向，其他三個角類似。
邊界的cost函式都可以進行線性化處理，隨後就可以用二次規劃快速求解。
D. M-Step DP Speed Optimizer
DP在ST圖裏生成speed profile。包含cost函式，ST圖柵格，動態搜尋。
具體來說，首先障礙物資訊分散到ST的柵格中，柵格根據間隔dt平均劃分，速度的具體參數大概可以認為：
目標函式是令cost最低：
Vref 是參考速度，一般是車道的限速。第一項代表維持速度的cost，其後兩項代表平滑性，最後是障礙物cost——自車與所有障礙物的距離。
E. M-Step QP Speed Optimizer
其中必須滿足的邊界約束：
平滑擬合的速度曲線：
F. Notes on Solving Quadratic Programming Problems
基於安全考慮，Apollo EM評估的點約100個，約束條件大於600個。
QP中計算結果包含3到5個多項式，約30個參數。
因此二次規劃的目標函式較小而約束較多，求解較容易。同時為了加速計算，采用上周期的結果作為計算起點（hot start），平均3ms就能得到結果（看算力）。
G. Notes on Non-convex Optimization With DP and QP
DP和QP在非凸空間都有各自的不足。
DP：依賴於采樣柵格，有限的柵格數量只能給出一個粗略的DP解。即很可能不是最優解。譬如DP能得出向左閃躲的路徑，但閃躲的距離不一定是最好的。
QP：相反，QP解依賴於凸空間，因此不能脫離DP使用。譬如，沒有DP的decision，QP無法決定該怎麽走。
DP+QP：將各自的不足最小化。先找一個粗略的解，再在這個凸空間裏找最優。
四、 CASE STUDY
根據初始速度計算交匯點，然後得出path，同時根據約束計算speed，speed更改後交匯點發生變化於是重新計算path，根據新的path重新計算交匯之後的speed（交匯之前的若也變化，那這個叠代就需要不斷進行了）。
五、算力
COMPUTATIONAL PERFORMANCE
本來的三維問題（station-lateral-speed）分解成兩個二維問題（station-lateral和station-speed），因此計算復雜度大大降低。對n個障礙物，M條備選path和N條備選speed，計算復雜度是O( n(M+N) ) 。
六、結論
EM planner是一個輕決策演算法，與其他基於大量規則的演算法不同，無需提前預估障礙物之間的相互影響，也不會因為規則變多導致找不到合適軌跡而規劃失敗。
安全性和透過性互相矛盾，安全規則增加後，可選的路徑就會減少，而且很容易導致走到一半的換道突然被中斷，而EM Planner則更可能保持連貫性，且得到可用的軌跡。
EM的計算復雜度較低。
EM已在北京和CA實地測試過，截止20180516，跑了3380小時、68000公裏。並在仿真環境跑了10w小時和100w公裏。
以上是論文的結論。
EM Planner整個框架能處理L4的自動駕駛場景，而且采用場景管理和任務管理，在去耦的角度看處理的也比較好，將三維問題分解成兩個二維問題，是這個演算法最大的特點了。整個演算法中，最核心的部份在於要理解Cost函式的構造和求解。之後我會寫關於這方面的學習備忘。
