自動駕駛路徑規劃-Lattice Planner算法

本文整合了網絡上關于百度 算法的詳細介紹和相關Q&A

大家好，今天很高興能給大家分享 3.0新發布的規劃算法。

算法隸屬于規劃模塊。規劃模塊以預測模塊、模塊、高精地圖和定位的結果作為輸入，通過算法，輸出一條平穩、舒适、安全的軌迹，交給控制模塊去執行。我們可以看到，規劃模塊在中是一個承上啟下的重要模塊。

Lattice算法隸屬于規劃模塊。規劃模塊以預測模塊、routing模塊、高精地圖和定位的結果作為輸入，通過算法，輸出一條平穩、舒适、安全的軌迹，交給控制模塊去執行。我們可以看到，規劃模塊在Apollo中是一個承上啟下的重要模塊。

一個合格規劃算法，必須滿足幾個條件。首先，必須能夠使自動駕駛汽車到達目的地；其次，必須符合交規；第三，能夠避免碰撞；最後，也需要能保證一定的舒适性。

在Apollo中，規劃算法的輸出是一系列軌迹點連成的軌迹。每一個軌迹點包含位置，速度，加速的等信息。

下面我來介紹一下規劃算法的工作流程。我們以右圖中的場景為例。其中紅車是我們的自動駕駛汽車，藍車是其他障礙車，前面藍色帶尖頭的曲線是藍車的預測軌迹。那麼這是一個前方即将有車輛并入的場景。

面對這樣的場景，有些司機會按照右圖中淺紅色的軌迹，選擇繞開藍色的障礙車。另外有一些司機開車相對保守，會沿着右圖中深紅色較短的軌迹做一個減速，給藍色障礙車讓路。

既然對于同一個場景，人類司機會有多種處理方法，那麼規劃算法的第一步就是采樣足夠多的軌迹，提供盡可能多的選擇

Lattice規劃算法的第二步是計算每一條軌迹計算的cost。這個cost考慮了軌迹的可行性、安全性等因素。我會在後面為大家詳細介紹。

那麼有了軌迹的cost以後，第三步就是一個循環檢測的過程。在這個過程中，我們每次會先挑選出cost最低的軌迹，對其進行物理限制檢測和碰撞檢測。如果挑出來的軌迹不能同時通過這兩個檢測，就将其篩除，考察下一條cost最低的軌迹。

以右圖為例，假設我們首先挑選出cost最低的是深紅色較短的軌迹。但我們發現即便猛踩刹車也無法執行這條軌迹。也就是說，這條軌迹超出了汽車的減速度上限。那麼它就無法通過物理限制檢測，我們會将其篩除。

假設我們下一條選出來cost最低的軌迹是右圖中深紅色較長的軌迹。我們會發現若沿着這條軌迹前進，紅車會和藍色障礙車發生碰撞。也就是說，這條軌迹軌迹無法通過碰撞檢測。于是隻能放棄這條軌迹，考慮下一條cost最低的。

這樣的過程循環繼續下去，假設我們現在挑選出右圖中靠左邊的深紅色軌迹，它既符合汽車的物理性狀，也不會有碰撞風險。

我們最終就将這條軌迹作為規劃軌迹輸出。

那麼下面我們對每一個步驟，進行詳細的說介紹。

首先是采樣過程。在正式介紹采樣過程之前，作為鋪墊，我先來介紹一下Frenet坐标系。在二維平面中，我們通常采用X-Y坐标系來描述問題。但在自動駕駛規劃問題中，我們的工作是基于道路的。這種情況下，X-Y坐标系并不是最方便的。所以我們這裡需要使用基于車道線橫向和縱向的Frenet坐标系。

那麼如何用坐标系來表示一輛汽車的狀态呢？首先我們有一條光滑的參考線（右圖中紅線），我們可以按右圖所示将汽車的坐标點投影到參考線上，得到一個參考線上的投影點（圖中藍色點）。從參考線起點到投影點的路徑長度就是汽車在坐标系下的縱向偏移量，用S表示。而投影點到汽車位置的距離則是汽車在坐标系下的橫向偏移量，用L表示。因為參考線是足夠光滑的，我們也可通過汽車的朝向、速度、加速度來計算出坐标系下，橫向和縱向偏移量的一階導和二階導。

這裡需要注意的是，我們将橫向偏移量L設計成縱向偏移量S的函數。這是因為對于大多數的汽車而言，橫向運動是由縱向運動誘發的。

有了坐标系的概念，我們下面來介紹一下如何生成一條軌迹。首先我們可以通過計算得到自動駕駛汽車在坐标系下的在零時刻的起始狀态，也就是汽車的當前狀态。為了生成一條軌迹，第一步就是在坐标系下采樣一個在T1時刻的末狀态。

第二步就是将末狀态和起始狀态做多項式拟合。分别形成橫向和縱向的多項式軌迹。

有了橫向軌迹和縱向軌迹之後，第三步就是二維合成。給定一個時刻T*，我們可以計算出在T*時刻的縱向偏移量和橫向偏移量，再通過參考線，即可還原成一個二維平面中的軌迹點。通過一系列的時間點T0，T1，...，Tn，可以獲得一系列的軌迹點P0，P1，…，Pn，最終形成一條完整的軌迹。了解了如何生成一條軌迹之後，下面我來介紹一下如何采樣一系列軌迹。

首先介紹如何采樣橫向軌迹。橫向軌迹的采樣需要涵蓋多種橫向運動狀态。現在Apollo的代碼中設計了三個末狀态橫向偏移量，-0.5，0.0和0.5，以及四個到達這些橫向偏移量的縱向位移，分别為10，20，40，80。用兩層循環遍曆各種組合，再通過多項式拟合，即可獲得一系列的橫向軌迹。

對于縱向軌迹的采樣，我們需要考慮巡航、跟車或超車、停車這三種狀态。

在停車狀态中，給定停車點，末狀态的速度和加速度都是零，所以末狀态是确定的。
那麼我們隻需用一層循環來采樣到達停車點的時間即可。

在介紹跟車/超車的采樣邏輯之前，我們需要介紹一下S-T圖的概念。以左圖中的場景為例，藍色障礙車從車道右側切入，在T_in時刻開始進入當前車道。那麼這個場景對應的S-T圖就如右圖所示。從T_in時刻開始出現一塊斜向上的陰影區域。這塊陰影區域的高度就是藍色障礙車的車身長，上邊界表示車頭，下邊界表示車尾，斜率表示車速。

如果上述場景變成這樣，障礙車從T_in時刻進入車道，然後在T_out時刻離開車道。那麼這個場景對應的S-T圖就會縮短。

有了S-T圖的概念，我們觀察左圖中的兩條規劃軌迹。紅色的是一條跟車軌迹，綠色的是超車軌迹。這兩條軌迹反映在S-T圖中，就如右圖所示。紅色的跟車軌迹在藍色陰影區域下方，綠色的超車軌迹在藍色陰影區域上方。

我們采樣末狀态時，就可以分别在S-T圖中障礙物對應的陰影區域的上方和下方分别采樣。上方的末狀态對應超車，下方的末狀态對應跟車。

如果有多個障礙物，我們就對這些障礙物分别采樣超車和跟車所對應的末狀态。

那麼總結下來就是遍曆所有和車道有關聯的障礙物，對他們分别采樣超車和跟車的末狀态，然後用多項式拟合即可獲得一系列縱向軌迹。

我們将三組縱向軌迹組合起來，就可以獲得所有縱向軌迹。再将所有縱向軌迹和所有橫向軌迹兩兩配對二維合成，就可以完成軌迹采樣的工作。

現在我們來介紹一下軌迹的cost的實現方法。我們前面提到，軌迹規劃所需要滿足的四點要求，分别是到達目的、符合交規，避免碰撞、平穩舒适。針對這四點要求，我們設計了六個cost，cost越高就表示越不滿足要求。下面我們一一介紹這六個cost的設計思路。

首先是到達目的的cost。這裡分成兩種情況，一個是存在停車指令（比如紅燈）的情況，另一個是沒有停車指令的。如果存在停車指令，相對大的車速，其對應的軌迹cost就越大；如果沒有停車指令，那麼低速軌迹的cost就會越大。

怎麼實現這樣的效果呢？我們針對這兩種情況分别設計了參考速度。左圖藍線表示沒有停車指令時的參考速度。

我們可以看到這種情況下，綠色的加速軌迹會獲得一個較小的cost，而紅色的減速軌迹會獲得一個相對較大的cost。那麼如果存在停車指令，參考速度就會想右圖中的藍色曲線一樣呈下降趨勢。那麼這種情況下，同樣的兩條軌迹，他們的cost大小關系就會正好相反。

第二個cost是橫向偏移cost。設計這個cost是為了讓自動駕駛汽車能盡量沿着道路中心行駛。那麼像左圖汽車靠道路一邊行駛，和中圖畫龍的行駛軌迹，他們的cost都相對較高。

第三個cost是碰撞cost。左圖中的兩條軌迹，反映在右圖S-T圖中，我們可以發現紅色的軌迹和藍色障礙車在S-T圖中的陰影區域有重疊，說明有碰撞風險，那麼它的碰撞cost就會相對較高。而綠色的軌迹在S-T圖中反映出來的碰撞風險較小，那麼它的碰撞cost就相對較低。

第四個cost是縱向加加速度的cost。加加速度（jerk）是加速度對時間的導數，表示加速度的變化率。我們用加加速度的最大值值來表示這個cost。

第五個cost是橫向加速度的cost。設計這個cost是為了平穩地換道。那麼像左圖猛打方向盤的軌迹，它的橫向加速度cost就會相對較大。

最後一個cost是向心加速度cost。設計這個cost是為了在轉彎或調頭的時候能夠減速慢行。在彎道處，車速慢的軌迹，其向心加速度cost就會相對較低，那麼就會更容易被率先挑選出來。

這六個cost的加權求和就是軌迹的總cost。開發者可以根據産品的需要，調試這六個權重。

這裡介紹一下限制檢測和碰撞檢測。限制檢測考察的内容有軌迹的加速度、加加速度、和曲率。碰撞檢測則是把自動駕駛汽車的軌迹和其他障礙物的預測軌迹進行比對，觀察是否有軌迹重疊。

對于換道場景，算法僅僅需要對目标車道對應的參考線做一次采樣+選擇的流程。本車道和目标車道均能産生一條最優軌迹。給換道軌迹的cost上增加額外的車道優先級的cost，再将兩條軌迹比較，選擇cost較小的那條即可。

Lattice規劃算法已經在新石器無人駕駛微型物流車和長沙智能駕駛研究院重卡中落地。我們也将繼續深度合作，提高技術的同時，拓展更多産品的應用。
Apollo問答 | 關于Lattice Planner規劃算法的若幹問答
1、Q:Lattice Planner将規劃統一成代價函數，尋找代價最小的。在規劃的上層是否還需要決策層？
A: 在規劃上層的決策僅僅包含了來自交規的停車指令（比如紅綠燈），其餘的策略均有下層采樣+cost來完成。
2、Q:Lattice Planner适用于哪些場景？
A: Lattice Planner現在已經在低速園區和高速公路的場景中由産品落地。對于普通城市道路，對于相對複雜的交規處理還有待完善。
3、Q:這個算法适合多彎道複雜的場景下嗎？複雜的停車場等？
A: 該算法可以處理多彎道的場景。對于停車場暫不使用，因為這個算法首先需要參考線，而複雜的停車場很難做出一條參考線。
4、Q:Cost裡面已經考慮了碰撞，為什麼還要做碰撞檢測
A: Cost裡面的碰撞僅僅是把有碰撞風險的軌迹的cost值設置的比較高，為了把這樣的軌迹優先級排到比較後，從而使得我們能夠優先考察其他更安全的軌迹，但它并沒有起到删選軌迹的作用，是一個比較soft的限制。而後面的碰撞檢測是出于安全的考慮，把這條軌迹篩除，這是一個hard的限制。
5、Q:Lattice Planner和EM Planner的區别是？或者說分别應用在什麼場景下？
A: Lattice Planner主要基于采樣+選擇，而EM Planner的思路是逐層優化叠代。從規劃層面來說，兩者均可用于各種場景。從決策層來看，Lattice的決策相對簡單，适用于相對簡單的場景，如低速園區，高速公路。EM算法對交規的決策做的相對更完善，可以處理相對複雜的普通城市道路。
6、Q:高橫向軌迹和縱向軌迹倆倆組合咋樣理解?是橫向的一條軌迹和縱向的所有軌迹組合嗎？
A: 兩兩組合指的是每一條橫向軌迹和每一條縱向軌迹的組合。
7、Q:計算量是不是有點大？普通CPU可以嗎？
A: 以目前的經驗來看，普通CPU是可以處理的。當然，這個算法可以随着計算機性能的提升，采樣更多的軌迹，使得我們對解空間的涵蓋更加完備。
8、Q:關于軌迹的生成，Lattice使用的是多項式拟合，一般使用三項還是五項？另外，老師說是在st圖上下陰影處取樣，那是取多少個點？那個末狀态是指超車結束的狀态點嗎？
A: 對于橫向軌迹的多項式拟合，因為有有初始狀态的零階導，一階導，二階導，和末狀态的零階導，一階導，二階導，一共六個變量，所以拟合五次多項式。對于縱向軌迹，在停車和跟車狀态，也都是五次多項式，但對于巡航狀态，由于我們不需要确定末狀态的S值，所以隻有五個變量，那麼用四次多項式就可以了。在ST圖上下取樣，點的數目可以由開發者自行決定，這個沒有限制。末狀态指的就是超車結束的狀态點。
9、Q:隻有起始狀态和末狀态怎麼進行橫向和縱向的拟合？
A: 構建多項式，使得多項式在起始時刻和末時刻的各階導數和起始狀态末狀态吻合。
10、Q:Lattice Planner是路徑規劃的算法，但也涵蓋了部分行為規劃的處理内容？
A: 是的。橫向軌迹主要針對路徑，縱向軌迹主要針對速度、加速度等行為。
11、Q:多項式拟合，是什麼多項式？拟合後如何保證滿足無人車的運動學和動力學要求？
A: 多項式指的就是普通的多項式，拟合時已經通過高階導數考慮了動力學要求。
12、Q:Apollo的代碼中出現了S-L坐标系和frenet坐标系的名稱，這兩個是否是指同一個坐标系？
A: 這兩個指的是同一個坐标系。
13、Q:高精度地圖的輸出格式是怎樣的？包括用于導航的矢量圖、用于定位的點雲圖以及全局的路徑。對于高精度地圖這一塊希望能夠有詳細的介紹。
A:可以參考
https://github.com/ApolloAuto/apollo/blob/master/modules/map/data/README.md 如 README 所示，xml, bin, txt, lb1 都是不同的文件格式，适配不同的讀取器，内容是一緻的。
14、Q:多項式拟合具體方法是什麼？如何避免拟合的曲線超出路面範圍？
A: 拟合的具體方法是求解多項式系數的線性方程組。開發者可以通過添加在路檢測的檢查validity check來避免曲線超出路面範圍。
15、Q:換道場景，是要提前獲取目标車道和當前行駛車道的參考線嗎？
A: 是的，這個信息在Apollo裡都是有的。
16、Q:Frenet坐标系下面是一個車道一個中心線？還是一條線一個中心線？這兩的s值的0點是從什麼地方開始算的？
A: 一個車道一個中心線。S值的0點是從參考線的起點開始。參考線的起點會随着主車的位置做實時動态調整，通常是在主車車身後30米左右的位置。
17、Q:在障礙車輛較多的環境下可能需要頻繁的規劃路徑，由于cost值有多個評價組成，有可能多次出現最佳軌迹的橫向方向完全相反的情況，可能造成車輛左右微微擺動，如何解決這種情況？
A: 在這種情況下，建議額外補充一個和上一個計算周期相似性的cost。
18、Q:對于單向雙車道場景，規劃的時候是隻規劃行駛車道還是規劃雙車道?
A: 如果有換道需求的話，會同時規劃兩條車道。
19、Q:初始狀态本車的theta是車頭朝向還是指車速朝向， 初始狀态曲率怎麼計算，用方向盤角度推？
A: 初始狀态的theta是車頭方向。曲率是基于IMU的信息計算出來的。
20、Q:在運行Apollo demo時 ，在隻有軟件模拟沒有硬件的情況，能不能測試和調試規劃算法，能的話怎麼做？
A: Apollo有一個開放的仿真平台，azure.apollo.auto，開發者可以在仿真平台中調試規劃算法。
21、Q:多項式具體形式如何？拟合拟合後如何保證滿足無人車的運動學和動力學要求？
A: 多項式就是普通的幂次多項式。拟合出來以後，可以通過限制檢測來篩選出符合無人車動力學模型的軌迹。
22、Q:Lattice算法的輸入也是referenceLine吧，這個是通過routing模塊給出的route segment生成的，和EM Planner一樣，理解正确麼？
A: 正确。
23、Q:有碰撞風險的軌迹為什麼不在cost環節或者之前直接删掉？
A: Cost環節僅僅是一個soft的排序，并不做删除的工作。
24、Q:如果如果在過彎道的時候，reference line上的end point有噪音，在不停都抖動，如何在這種情況下規劃一條穩定的軌迹，謝謝。
A: 這就需要我們不斷優化reference line的平滑算法。
25、Q:現在Apollo的代碼中設計了三個末狀态橫向偏移量，-0.5，0.0和0.5，以及四個到達這些橫向偏移量的縱向位移，分别為10，20，40，80。能解釋下為什麼這樣定意思這些常量？
A: 這些常量是根據平時路測的經驗得到的。開發者可以根據自己的産品和場景來調整這些常量。
26、Q:隻有起始狀态和末狀态怎麼進行橫向和縱向的拟合？
A: 通過求解多項式的系數是的起始點和終止點的各階導數吻合。
27、Q:關于軌迹的生成，Lattice使用的是多項式拟合，一般使用三項還是五項？另外，老師說是在st圖上下陰影處取樣，那是取多少個點？那個末狀态是指超車結束的狀态點嗎？
A: 一般是5階多項式，在cruise狀态下，用4階多項式。在st圖中的取點個數沒有規定，開發者可以根據需要來調試。
28、Q：
 
圖中确定了縱向拟合曲線之後，如何确定橫向的拟合曲線？
A、橫向軌迹在第18頁ppt中有介紹。
29、Q:軌迹采樣時縱向位移選取10 20 40 80 ，這是經驗值嗎？是不是需要根據車速進行調整？
A: 這是經驗值。我們也正在改進，使得它能根據車速調整。
30、Q:規劃軌迹中的速度如何設定，是否根據起始點速度、終點速度、以及軌迹距離，計算出一個平穩加速度，然後平穩地加速過去？但是我看PPT中跟車的場景，到達reference speed的過程加速度似乎并非為一個常量。
A: 軌迹的平穩性可以通過cost來篩選。所以就要求我們在sample的過程中涵蓋面盡可能的廣泛。
31、Q:橫向運動是由縱向運動誘發的，該如何理解？
A: 普通的車子的輪胎不會轉到水平位置來做單純的橫向運動。