在第4期Apollo自動駕駛公開課中，來自Apollo團隊的資深架構師-朱帆老師對Apollo 2.5自動駕駛規劃控制系統進行了詳細介紹。

這里，我們將整理后的公開課視頻和資料分享給大家，沒能到達現場的開發者可以通過視頻和PPT資料來詳細了解課程內容。

Apollo 2.5自動駕駛規劃控制

技術難點

規劃模塊所面臨的技術難點有三點。

第一，車輛所處的環境復雜度高。因此傳感器檢測返回的數據復雜，障礙物個數種類多；路況信息復雜，在高速上一秒鐘車輛能跑出30米，情況瞬息萬變。

第二，系統對規劃模塊的要求高。規劃模塊需要覆蓋所有的ADAS場景，對模塊計算的實時性和穩定性有著很高的要求。

第三，這是一個全新的未知領域，能夠從外界獲得的信息有限。雖然有過去長期的積累，但是依然有多重難題需要解決。

整體思路

下面來看一下規劃模塊是如何實現的。

想象一個人在開車，要從A點開到B點；他有多種方式可以完成這一任務，但是不同的方式，成本是不一樣的。

那么如何去找一個成本最低的規劃曲線呢？開發者面臨的是一個三維空間中的優化問題，包括路面的二維平面，也包括時間維度。這是一個N立方難度的問題，Apollo 2.5的解決方法是，把這個N立方級別的問題，分拆成兩個N平方級別的問題。也就是在x-y維度上求解，進行路徑規劃；在路徑規劃的基礎上，以規劃出來的路徑為s軸，在s-t維度上進行速度規劃。

這種降低維度，以退為進的方法，是對規劃模塊認識上的一個飛躍。通過放棄最優解的追求轉而追求較優解，換來系統運算速度、穩定性的極大提高，同時滿足無人駕駛系統的整體需求。這一點可以類比機器學習中SGD的思路，放棄對精確梯度方向的尋找，轉而尋找近似梯度的方向，從而換來學習速率上的提升。

整體架構

規劃模塊的架構從整體上來說，可以分為兩個部分：一部分負責對數據的監聽、獲取和預處理；另一部分負責管理各個優化模塊。數據進入后，對其綜合處理為規劃模塊的內部數據結構，由任務管理器調度合適的優化器進行各個優化任務。綜合優化的結果，經過最終的驗證后，輸出給控制模塊。

在設計上，實現了策略的可插拔，使得各個優化器可以靈活配置不同策略，提升迭代效率。

相對于Apollo 2.0，規劃模塊在延續了其架構上統一、靈活、快捷的優點上，在保持已開發算法持續更新優化的同時，又新增了局部坐標系以及一種新的速度和路徑同時優化的Lattice planner。

規劃器

Apollo的規劃模塊目前有三種類型的規劃器：

Apollo 1.0中開始開放的RTK，也就是循跡Planner；Apollo 1.5中開始開放的EM Planner，也就是基于動態規劃Dynamic Programming和二次規劃Quadratic Programming的路徑規劃器與速度規劃器；以及Apollo 2.5中開始開放的Lattice Planner，一種路徑和速度同時規劃的規劃器。

RTK Planner

RTK Planner是規劃預先設置的軌跡，為循即算法使用。

EM Planner

EM Planner可以通過動態規劃進行路徑和速度規劃。比如說，路徑規劃會在范圍內進行機動撒點，離散化解空間進行求解。

要想合理的選擇采樣點，首先應該直觀的感受一下車輛和道路。以最常用的林肯MKZ 2016款車款為例，此車寬度2米1，那么假設道路寬度為3.5米，在當前道路行駛的時候就留下了一共1.4米左右的機動范圍。

比如說，采樣器在垂直于Lane的方向選擇5個點，這種選法每兩個點間隔0.35米，對于車輛調節來說是合適的；而在沿著道路方向，點的選取不需要特別密集，過于密集的選擇白白浪費了計算資源，并且在下一步使用光滑曲線連接點的時候，造成求解的不穩定。所以，往往會根據當前的車速和路況進行選擇，比如每隔15-20米選一組點，共選擇4-5組。這樣選點，結合了車輛與道路的特點，并且所有參數是可以配置的，具有高度的靈活性。

撒點后，通過Cost Function一層一層的進行動態規劃，從而對問題進行了有效的簡化。然后再通過平滑的曲線連接各層采樣點得到最終的路徑。其優勢是明顯的：

靈活性：采樣點從左到右覆蓋了整個可行區間，避免陷入局部最優

可靠性：其利用Cost Function 避免無解

可拓展：低速到高速全覆蓋

EM Planner中的速度規劃，也可以采用和路徑規劃相似的方法。對解空間進行離散化以后，然后通過動態規劃進行求解。從圖中可以看到，在將與主車軌跡有重疊的障礙物映射到ST圖以后，速度規劃可以通過加速減速勻速等操作，從ST圖中搜索到一條可通行區間并生成速度規劃。從最后生成出來的圖里面，可以解讀出來相對于每一個障礙物的縱向決策。

比如說，需要避讓或者跟隨一個障礙物，車輛就從障礙物ST圖中下方通過如果需要超越一個障礙物，那么在ST中車輛就從其上方通過。

Lattice Planner

接下來看一下Apollo 2.5中開放的新算法Lattice Planner。

Lattice Planner和EM Planner在設計上最大的區別在于，Lattice是橫向縱向綜合求解的，而EM是分開求解的。Lattice Planner源于斯坦福大學參加DARPA無人駕駛大賽的規劃算法，Apollo 2.5吸收了其精華，并針對原有算法進行了改進和升級。

和EM planner一樣，Lattice Planner也將軌跡規劃問題分解成橫向和縱向兩個1維空間的獨立的軌跡規劃問題, 降低規劃難度。橫向仍然是SL問題，也就是Station Lateral，縱向上也還是ST，也就是Station Time問題。

Lattice Planner會根據起點和終點的狀態，在位置空間和時間上同時進行撒點。撒點的起始狀態和終止狀態各有6個參數，包括了3個橫向參數，即橫向位置、橫向位置的導數也就是Heading、Heading的導數；3個縱向參數，即縱向位置、縱向位置的一階導數也就是速度、縱向位置的二階導數（也就是加速度）。

起點的參數是車輛當時真實的狀態，或者Stitch的狀態來進行設計，終止狀態則是撒點枚舉的各個情況。在確定了終點和起點狀態以后，再通過五階或者四階的多項式連接起始狀態和終止狀態，從而得到規劃的橫向和縱向軌跡。

這個步驟是Lattice算法的精髓所在，它直接決定了算法的效率以及解空間。舉個例子來說，在撒點的時候可以的加入車輛物理動力學性能的約束，同時根據障礙物進行可行域的預篩選等，有效的裁剪無效空間，從而達到更優的性能。

在生成所有橫向和縱向的一維軌跡以后，將其排列組合起來，合成大量的二維軌跡，然后根據損失函數篩選出最好的合成軌跡。和EM Planner類似，Lattice Planner的損失函數也可以分為，安全相關、體感相關以及目標完成度相關三類。

下面來看一下Lattice Planner在模擬器中的效果。

Lattice Planner在Apollo模擬器Azure中場景通過率達到了100%。

Planner 對比

Lattice Planner的優點是很明顯的，同時產生軌跡的幾何形狀與速度分配，直接考慮到車輛的動力學狀態，尤其是橫向動力學狀態，產生更適合高速運動的軌跡，簡單可靠，參數少于10個。它利用了撒點算法避免了軌跡陷入局部最優，性能也非常的優秀，生成超過1000軌跡，軌跡的生成加上選擇，用時少于100ms，確保了能做到10赫茲的Planning規劃。

Planner 軌跡后處理

在軌跡生成以后，往往還可以通過一些后處理，對軌跡進行平滑和驗證。可以通過幾何的算法或者更加高粒度的采樣，對于生成軌跡每個點，進行額外的碰撞檢查。也可以同時驗證規劃的軌跡是否符合車輛的物理學動力特性，比如看其是否有超過車輛加減速度上下界限，以及轉彎半徑界限的時候。

回退保障機制

秉著安全第一的原則，Apollo 2.5同時提供冗余機制以保證整個系統的穩定性。車輛在路上行駛的過程中，遇到的情況千差萬別，如果真的遇到了無論是人工駕駛還是機器駕駛都無法解決的特殊惡劣情況，如何保證Planning不會崩潰，并且輸出一條相對最安全的軌跡，以保證生命和財產的安全，就顯得尤其的重要了。

因此，無論是使用DP還是QP的EM planner，還是同時進行速度和路徑優化的Lattice Planner，Apollo 2.5針對路徑規劃和速度規劃都加入了后備規劃。在正常的邏輯并不能在給定假設情況下計算出一條合理的路徑的時候，針對當時的情況，對車輛進行合理的減速最后停車，以確保安全。

高效的測試與迭代

如何幫助大家進行能夠高效的迭代也是Apollo所關心的一個重要問題。因此，Apollo的Planning有一套推薦的測試迭代流程。即單元測試，單周期的集成測試，模擬器測試，最后再實車測試的流程。讓開發者可以在開發迭代的過程中，隨時進行回歸，保證開發的質量以及效率。

坐標系

Apollo 2.5又為大家打開了另外一扇窗--相對坐標系。此前的絕對坐標系是以世界為背景的一種全局定位，世界上每一個點，都會有一個相應的唯一的坐標。它的好處很明顯，因為是全局唯一，所以說表達一個物體的真實位置的時候非常的容易，做多時刻位置對齊的時候也會比較的容易。

車身相對坐標系也是一種常見的坐標系，它是以車輛自身位置坐標原點，以車輛的朝向為x，車輛左方為y的坐標系。其優點也是非常明顯的，可以去除開發者們對于地圖定位的強依賴，反饋的數值也更加的直觀方便開發者的理解。

相對坐標系

助力于相對坐標系這個新feature，在Apollo 2.5中同時推出了L3的高速方案。也就是基于車道線的感知方案和不基于地圖和定位的規劃方案。可以理解成為是類似于Tesla Autopilot或者Mobileye的方案，利用相對坐標系下的車道線代替全局坐標系的地圖與定位，進行L3級別的自動駕駛功能。

就像John Kennedy說的：“We choose to go to the moon in this decade and do the other things，not because they are easy，but because they are hard.”

這是Apollo的口號，也是我們一直以來的信念。