Lattice算法隸屬於規劃模塊。規劃模塊以預測模塊、routing模塊、高精地圖和定位的結果作爲輸入，通過算法，輸出一條平穩、舒適、安全的軌跡，交給控制模塊去執行。我們可以看到，規劃模塊在Apollo中是一個承上啓下的重要模塊。
這是Apollo中規劃模塊的工作流程。首先是依據routing和定位，通過平滑算法，生成一條平滑的參考線（平滑的道路中心線）。再通過規劃算法，生成一條符合交規，安全舒適的規劃軌跡。那麼Lattice算法就是Apollo開源平臺中，其中的一種規劃算法。
一個合格規劃算法，必須滿足幾個條件。首先，必須能夠使自動駕駛汽車到達目的地；其次，必須符合交規；第三，能夠避免碰撞；最後，也需要能保證一定的舒適性。
在Apollo中，規劃算法的輸出是一系列軌跡點連成的軌跡。每一個軌跡點包含位置，速度，加速的等信息。
二、Lattice 規劃算法的工作流程
下面我來介紹一下Lattice規劃算法的工作流程。我們以右圖中的場景爲例。其中紅車是我們的自動駕駛汽車，藍車是其他障礙車，前面藍色帶尖頭的曲線是藍車的預測軌跡。那麼這是一個前方即將有車輛併入的場景。
面對這樣的場景，有些司機會按照右圖中淺紅色的軌跡，選擇繞開藍色的障礙車。另外有一些司機開車相對保守，會沿着右圖中深紅色較短的軌跡做一個減速，給藍色障礙車讓路。
既然對於同一個場景，人類司機會有多種處理方法，那麼Lattice規劃算法的第一步就是採樣足夠多的軌跡，提供儘可能多的選擇。
Lattice規劃算法的第二步是計算每一條軌跡計算的cost。這個cost考慮了軌跡的可行性、安全性等因素。我會在後面爲大家詳細介紹。
那麼有了軌跡的cost以後，第三步就是一個循環檢測的過程。在這個過程中，我們每次會先挑選出cost最低的軌跡，對其進行物理限制檢測和碰撞檢測。如果挑出來的軌跡不能同時通過這兩個檢測，就將其篩除，考察下一條cost最低的軌跡。
以右圖爲例，假設我們首先挑選出cost最低的是深紅色較短的軌跡。但我們發現即便猛踩剎車也無法執行這條軌跡。也就是說，這條軌跡超出了汽車的減速度上限。那麼它就無法通過物理限制檢測，我們會將其篩除。
假設我們下一條選出來cost最低的軌跡是右圖中深紅色較長的軌跡。我們會發現若沿着這條軌跡前進，紅車會和藍色障礙車發生碰撞。也就是說，這條軌跡軌跡無法通過碰撞檢測。於是只能放棄這條軌跡，考慮下一條cost最低的。
這樣的過程循環繼續下去，假設我們現在挑選出右圖中靠左邊的深紅色軌跡，它既符合汽車的物理性狀，也不會有碰撞風險。
最終就將這條軌跡作爲規劃軌跡輸出。
三、Lattice Planner的採樣過程
首先是採樣過程。在正式介紹採樣過程之前，作爲鋪墊，先來介紹一下Frenet座標系。在二維平面中，通常採用X-Y座標系來描述問題。但在自動駕駛規劃問題中，我們的工作是基於道路的。這種情況下，X-Y座標系並不是最方便的。所以我們這裏需要使用基於車道線橫向和縱向的Frenet座標系。
那麼如何用Frenet座標系來表示一輛汽車的狀態呢？首先有一條光滑的參考線（右圖中紅線），可以按右圖所示將汽車的座標點投影到參考線上，得到一個參考線上的投影點（圖中藍色點）。從參考線起點到投影點的路徑長度就是汽車在Frenet座標系下的縱向偏移量，用S表示。而投影點到汽車位置的距離則是汽車在Frenet座標系下的橫向偏移量，用L表示。因爲參考線是足夠光滑的，也可通過汽車的朝向、速度、加速度來計算出Frenet座標系下，橫向和縱向偏移量的一階導和二階導。
這裏需要注意的是，我們將橫向偏移量L設計成縱向偏移量S的函數。這是因爲對於大多數的汽車而言，橫向運動是由縱向運動誘發的。
有了Frenet座標系的概念，下面來介紹一下如何生成一條軌跡。首先我們可以通過計算得到自動駕駛汽車在Frenet座標系下的在零時刻的起始狀態，也就是汽車的當前狀態。爲了生成一條軌跡，第一步就是在Frenet座標系下采樣一個在T1時刻的末狀態。
第二步就是將末狀態和起始狀態做多項式擬合。分別形成橫向和縱向的多項式軌跡。
有了橫向軌跡和縱向軌跡之後，第三步就是二維合成。給定一個時刻T*，可以計算出在T*時刻的縱向偏移量和橫向偏移量，再通過參考線，即可還原成一個二維平面中的軌跡點。通過一系列的時間點T0，T1，...，Tn，可以獲得一系列的軌跡點P0，P1，…，Pn，最終形成一條完整的軌跡。瞭解瞭如何生成一條軌跡之後，下面介紹一下如何採樣一系列軌跡。
四、如何採樣橫向和縱向軌跡
首先介紹如何採樣橫向軌跡。橫向軌跡的採樣需要涵蓋多種橫向運動狀態。現在Apollo的代碼中設計了三個末狀態橫向偏移量，-0.5，0.0和0.5，以及四個到達這些橫向偏移量的縱向位移，分別爲10，20，40，80。用兩層循環遍歷各種組合，再通過多項式擬合，即可獲得一系列的橫向軌跡。
對於縱向軌跡的採樣，需要考慮巡航、跟車或超車、停車這三種狀態。
對於巡航狀態，通過兩層循環來完成採樣。外層循環將速度從零到上限值按等間隔均勻遍歷。內層循環遍歷到達末狀態速度的時間，從1秒到8秒按1秒的間隔均勻遍歷。由於巡航狀態不需要指明到達末狀態的S值，所以這裏只需要用四次多項式擬合即可。
在停車狀態中，給定停車點，末狀態的速度和加速度都是零，所以末狀態是確定的。
那麼我們只需用一層循環來採樣到達停車點的時間即可。
在介紹跟車/超車的採樣邏輯之前，需要介紹一下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是碰撞cost。左圖中的兩條軌跡，反映在右圖S-T圖中，我們可以發現紅色的軌跡和藍色障礙車在S-T圖中的陰影區域有重疊，說明有碰撞風險，那麼它的碰撞cost就會相對較高。而綠色的軌跡在S-T圖中反映出來的碰撞風險較小，那麼它的碰撞cost就相對較低。
第四個cost是縱向加加速度的cost。加加速度（jerk）是加速度對時間的導數，表示加速度的變化率。我們用加加速度的最大值值來表示這個cost。
第五個cost是橫向加速度的cost。設計這個cost是爲了平穩地換道。那麼像左圖猛打方向盤的軌跡，它的橫向加速度cost就會相對較大。
最後一個cost是向心加速度cost。設計這個cost是爲了在轉彎或調頭的時候能夠減速慢行。在彎道處，車速慢的軌跡，其向心加速度cost就會相對較低，那麼就會更容易被率先挑選出來。
這六個cost的加權求和就是軌跡的總cost。開發者可以根據產品的需要，調試這六個權重。
這裏介紹一下限制檢測和碰撞檢測。限制檢測考察的內容有軌跡的加速度、加加速度、和曲率。碰撞檢測則是把自動駕駛汽車的軌跡和其他障礙物的預測軌跡進行比對，觀察是否有軌跡重疊。
對於換道場景，Lattice算法僅僅需要對目標車道對應的參考線做一次採樣+選擇的流程。本車道和目標車道均能產生一條最優軌跡。給換道軌跡的cost上增加額外的車道優先級的cost，再將兩條軌跡比較，選擇cost較小的那條即可。