在第4期Apollo自動駕駛公開課中，來自Apollo團隊的資深架構(gòu)師-朱帆老師對Apollo 2.5自動駕駛規(guī)劃控制系統(tǒng)進(jìn)行了詳細(xì)介紹。

這里，我們將整理后的公開課視頻和資料分享給大家，沒能到達(dá)現(xiàn)場的開發(fā)者可以通過視頻和PPT資料來詳細(xì)了解課程內(nèi)容。

Apollo 2.5自動駕駛規(guī)劃控制

技術(shù)難點

規(guī)劃模塊所面臨的技術(shù)難點有三點。

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

第二，系統(tǒng)對規(guī)劃模塊的要求高。規(guī)劃模塊需要覆蓋所有的ADAS場景，對模塊計算的實時性和穩(wěn)定性有著很高的要求。

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

整體思路

下面來看一下規(guī)劃模塊是如何實現(xiàn)的。

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

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

這種降低維度，以退為進(jìn)的方法，是對規(guī)劃模塊認(rèn)識上的一個飛躍。通過放棄最優(yōu)解的追求轉(zhuǎn)而追求較優(yōu)解，換來系統(tǒng)運算速度、穩(wěn)定性的極大提高，同時滿足無人駕駛系統(tǒng)的整體需求。這一點可以類比機器學(xué)習(xí)中SGD的思路，放棄對精確梯度方向的尋找，轉(zhuǎn)而尋找近似梯度的方向，從而換來學(xué)習(xí)速率上的提升。

整體架構(gòu)

規(guī)劃模塊的架構(gòu)從整體上來說，可以分為兩個部分：一部分負(fù)責(zé)對數(shù)據(jù)的監(jiān)聽、獲取和預(yù)處理；另一部分負(fù)責(zé)管理各個優(yōu)化模塊。數(shù)據(jù)進(jìn)入后，對其綜合處理為規(guī)劃模塊的內(nèi)部數(shù)據(jù)結(jié)構(gòu)，由任務(wù)管理器調(diào)度合適的優(yōu)化器進(jìn)行各個優(yōu)化任務(wù)。綜合優(yōu)化的結(jié)果，經(jīng)過最終的驗證后，輸出給控制模塊。

在設(shè)計上，實現(xiàn)了策略的可插拔，使得各個優(yōu)化器可以靈活配置不同策略，提升迭代效率。

相對于Apollo 2.0，規(guī)劃模塊在延續(xù)了其架構(gòu)上統(tǒng)一、靈活、快捷的優(yōu)點上，在保持已開發(fā)算法持續(xù)更新優(yōu)化的同時，又新增了局部坐標(biāo)系以及一種新的速度和路徑同時優(yōu)化的Lattice planner。

規(guī)劃器

Apollo的規(guī)劃模塊目前有三種類型的規(guī)劃器：

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

RTK Planner

RTK Planner是規(guī)劃預(yù)先設(shè)置的軌跡，為循即算法使用。

EM Planner

EM Planner可以通過動態(tài)規(guī)劃進(jìn)行路徑和速度規(guī)劃。比如說，路徑規(guī)劃會在范圍內(nèi)進(jìn)行機動撒點，離散化解空間進(jìn)行求解。

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

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

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

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

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

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

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

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

Lattice Planner

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

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

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

Lattice Planner會根據(jù)起點和終點的狀態(tài)，在位置空間和時間上同時進(jìn)行撒點。撒點的起始狀態(tài)和終止?fàn)顟B(tài)各有6個參數(shù)，包括了3個橫向參數(shù)，即橫向位置、橫向位置的導(dǎo)數(shù)也就是Heading、Heading的導(dǎo)數(shù)；3個縱向參數(shù)，即縱向位置、縱向位置的一階導(dǎo)數(shù)也就是速度、縱向位置的二階導(dǎo)數(shù)（也就是加速度）。

起點的參數(shù)是車輛當(dāng)時真實的狀態(tài)，或者Stitch的狀態(tài)來進(jìn)行設(shè)計，終止?fàn)顟B(tài)則是撒點枚舉的各個情況。在確定了終點和起點狀態(tài)以后，再通過五階或者四階的多項式連接起始狀態(tài)和終止?fàn)顟B(tài)，從而得到規(guī)劃的橫向和縱向軌跡。

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

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

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

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

Planner 對比

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

Planner 軌跡后處理

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

回退保障機制

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

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

高效的測試與迭代

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

坐標(biāo)系

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

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

相對坐標(biāo)系

助力于相對坐標(biāo)系這個新feature，在Apollo 2.5中同時推出了L3的高速方案。也就是基于車道線的感知方案和不基于地圖和定位的規(guī)劃方案。可以理解成為是類似于Tesla Autopilot或者M(jìn)obileye的方案，利用相對坐標(biāo)系下的車道線代替全局坐標(biāo)系的地圖與定位，進(jìn)行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的口號，也是我們一直以來的信念。