Apollo 2.5自動駕駛規(guī)劃控制系統(tǒng)詳細(xì)介紹

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

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

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

技術(shù)難點(diǎn)

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

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

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

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

整體思路

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

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

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

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

整體架構(gòu)

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

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

相對于Apollo 2.0，規(guī)劃模塊在延續(xù)了其架構(gòu)上統(tǒng)一、靈活、快捷的優(yōu)點(diǎn)上，在保持已開發(fā)算法持續(xù)更新優(yōu)化的同時(shí)，又新增了局部坐標(biāo)系以及一種新的速度和路徑同時(shí)優(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，一種路徑和速度同時(shí)規(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ī)動撒點(diǎn)，離散化解空間進(jìn)行求解。

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

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

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

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

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

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

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

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

Lattice Planner

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

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

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

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

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

這個(gè)步驟是Lattice算法的精髓所在，它直接決定了算法的效率以及解空間。舉個(gè)例子來說，在撒點(diǎn)的時(shí)候可以的加入車輛物理動力學(xué)性能的約束，同時(shí)根據(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)點(diǎn)是很明顯的，同時(shí)產(chǎn)生軌跡的幾何形狀與速度分配，直接考慮到車輛的動力學(xué)狀態(tài)，尤其是橫向動力學(xué)狀態(tài)，產(chǎn)生更適合高速運(yùn)動的軌跡，簡單可靠，參數(shù)少于10個(gè)。它利用了撒點(diǎn)算法避免了軌跡陷入局部最優(yōu)，性能也非常的優(yōu)秀，生成超過1000軌跡，軌跡的生成加上選擇，用時(shí)少于100ms，確保了能做到10赫茲的Planning規(guī)劃。

Planner 軌跡后處理

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

回退保障機(jī)制

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

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

高效的測試與迭代

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

坐標(biāo)系

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

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

相對坐標(biāo)系

助力于相對坐標(biāo)系這個(gè)新feature，在Apollo 2.5中同時(shí)推出了L3的高速方案。也就是基于車道線的感知方案和不基于地圖和定位的規(guī)劃方案?？梢岳斫獬蔀槭穷愃朴?a href="http://ttokpm.com/tags/te/" target="_blank">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的口號，也是我們一直以來的信念。