本文主要參考Apollo開發(fā)者社區(qū)，以及一些大神的博客，在此膜拜，文末會奉上相關(guān)鏈接。

Lattice Planner 規(guī)劃算法簡介

之前的相關(guān)的規(guī)劃的算法都是放在無人駕駛專欄下的，Lattice Planner 是Apollo中實際使用的算法，因此，放在Apollo專欄下，這里簡單做一下說明。

Lattice Planner 算法屬于一種局部軌跡規(guī)劃，其輸入為定位信息，感知障礙物信息，以及靜態(tài)參考線信息，輸出為局部規(guī)劃軌跡，輸入到控制器，由控制器完成局部軌跡的跟蹤控制。因此，Lattice Planner 輸出的軌跡是一條光滑無碰撞滿足車輛運(yùn)動學(xué)約束和速度約束的平穩(wěn)安全的局部軌跡。

本文主要分為以下幾個模塊：

Lattice Planner 總體概覽

Lattice 規(guī)劃算法的工作流程

Lattice Planner的采樣過程

如何采樣橫向和縱向軌跡

軌跡COST的實現(xiàn)方法

Lattice Planner 總體概覽

Lattice 算法隸屬于規(guī)劃模塊。

規(guī)劃模塊以預(yù)測模塊、Routing模塊、高精地圖和定位的結(jié)果作為輸入，通過算法，輸出一條平穩(wěn)、舒適、安全的軌跡，交給控制模塊去執(zhí)行。可以看出，規(guī)劃模塊在Apollo中是一個承上啟下的重要模塊。

上圖是Apollo中規(guī)劃模塊的工作流程：

首先是依據(jù)Routing和定位，通過平滑算法，生成一條平滑的參考線（平滑的道路中心線）；

再通過規(guī)劃算法，生成一條符合交規(guī)，安全舒適的規(guī)劃軌跡。那么Lattice算法就是Apollo開源平臺中，其中的一種規(guī)劃算法。

一個合格規(guī)劃算法，必須滿足幾個條件：

首先，必須能夠使自動駕駛汽車到達(dá)目的地；

其次，必須符合交規(guī)；第三，能夠避免碰撞；

最后，也需要能保證一定的舒適性。

在Apollo中，規(guī)劃算法的輸出是一系列軌跡點(diǎn)連成的軌跡。每一個軌跡點(diǎn)包含位置，速度，加速的等信息。

Lattice Planner 規(guī)劃算法的工作流程

下面介紹一下Lattice規(guī)劃算法的工作流程。我們以下圖中的場景為例。

其中紅車是我們的自動駕駛汽車，藍(lán)車是其他障礙車，前面藍(lán)色帶尖頭的曲線是藍(lán)車的預(yù)測軌跡。那么這是一個前方即將有車輛并入的場景。

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

既然對于同一個場景，人類司機(jī)會有多種處理方法，那么Lattice規(guī)劃算法的第一步就是采樣足夠多的軌跡，提供盡可能多的選擇。

Lattice規(guī)劃算法的第二步是計算每一條軌跡計算的cost。這個cost考慮了軌跡的可行性、安全性等因素。cost的計算方式會在后面詳細(xì)介紹。

那么有了軌跡的cost以后，第三步就是一個循環(huán)檢測的過程。在這個過程中，每次會先挑選出cost最低的軌跡，對其進(jìn)行物理限制檢測和碰撞檢測。如果挑出來的軌跡不能同時通過這兩個檢測，就將其篩除，考察下一條cost最低的軌跡。

以下圖為例，假設(shè)首先挑選出cost最低的是深紅色較短的軌跡。但我們發(fā)現(xiàn)即便猛踩剎車也無法執(zhí)行這條軌跡。也就是說，這條軌跡超出了汽車的減速度上限。那么它就無法通過物理限制檢測，我們會將其篩除。

假設(shè)下一條選出來cost最低的軌跡是下圖中深紅色較長的軌跡。會發(fā)現(xiàn)若沿著這條軌跡前進(jìn)，紅車會和藍(lán)色障礙車發(fā)生碰撞。也就是說，這條軌跡軌跡無法通過碰撞檢測。于是只能放棄這條軌跡，考慮下一條cost最低的。

這樣的過程循環(huán)繼續(xù)下去，假設(shè)現(xiàn)在挑選出下圖中靠左邊的深紅色軌跡，它既符合汽車的物理性狀，也不會有碰撞風(fēng)險。
最終，就將這條軌跡作為規(guī)劃軌跡輸出給控制模塊。

Lattice Planner的采樣過程

現(xiàn)在對每一個步驟，進(jìn)行詳細(xì)的說明介紹。

Frenet 坐標(biāo)系

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

那么如何用Frenet坐標(biāo)系來表示一輛汽車的狀態(tài)呢？

首先有一條光滑的參考線（上圖中紅線），可以按上圖所示將汽車的坐標(biāo)點(diǎn)投影到參考線上，得到一個參考線上的投影點(diǎn)（圖中藍(lán)色點(diǎn)）。

從參考線起點(diǎn)到投影點(diǎn)的路徑長度就是汽車在Frenet坐標(biāo)系下的縱向偏移量，用$s$表示；而投影點(diǎn)到汽車位置的距離則是汽車在Frenet坐標(biāo)系下的橫向偏移量，用$l$表示。因為參考線是足夠光滑的，也可通過汽車的朝向、速度、加速度，來計算出Frenet坐標(biāo)系下車輛橫向和縱向偏移量的一階導(dǎo)和二階導(dǎo)。（具體的坐標(biāo)變換，在另外一篇中有詳細(xì)的說明）

這里需要注意的是，我們將橫向偏移量$l$設(shè)計成縱向偏移量$s$的函數(shù)。這是因為對于大多數(shù)的汽車而言，橫向運(yùn)動是由縱向運(yùn)動誘發(fā)的。

軌跡的生成

有了Frenet坐標(biāo)系的概念，下面來介紹一下如何生成一條軌跡。

首先我們可以通過計算得到自動駕駛汽車在Frenet坐標(biāo)系下的在零時刻的起始狀態(tài)，也就是汽車的當(dāng)前狀態(tài)。

為了生成一條軌跡，第一步就是在Frenet坐標(biāo)系下采樣一個在T1時刻的末狀態(tài)；

第二步就是將末狀態(tài)和起始狀態(tài)做多項式擬合，分別形成橫向和縱向的多項式軌跡；

有了橫向軌跡和縱向軌跡之后，第三步就是二維合成。給定一個時刻T*，可以計算出在T*時刻的縱向偏移量和橫向偏移量，再通過參考線，即可還原成一個二維平面中的軌跡點(diǎn)。通過一系列的時間點(diǎn)T0，T1，…，Tn，可以獲得一系列的軌跡點(diǎn)P0，P1，…，Pn，最終形成一條完整的軌跡。

如何采樣橫向和縱向軌跡

在了解了如何生成一條軌跡之后，下面介紹一下如何采樣一系列軌跡。

首先介紹如何采樣橫向軌跡。橫向軌跡的采樣需要涵蓋多種橫向運(yùn)動狀態(tài)。現(xiàn)在Apollo的代碼中設(shè)計了三個末狀態(tài)橫向偏移量，-0.5，0.0和0.5，以及四個到達(dá)這些橫向偏移量的縱向位移，分別為10，20，40，80。用兩層循環(huán)遍歷各種組合，再通過多項式擬合，即可獲得一系列的橫向軌跡。

對于縱向軌跡的采樣，需要考慮巡航、跟車或超車、停車這三種狀態(tài)。

對于巡航狀態(tài)，通過兩層循環(huán)來完成采樣。外層循環(huán)將速度從零到上限值按等間隔均勻遍歷。內(nèi)層循環(huán)遍歷到達(dá)末狀態(tài)速度的時間，從1秒到8秒按1秒的間隔均勻遍歷。由于巡航狀態(tài)不需要指明到達(dá)末狀態(tài)的S值，所以這里只需要用四次多項式擬合即可。

在停車狀態(tài)中，給定停車點(diǎn)，末狀態(tài)的速度和加速度都是零，所以末狀態(tài)是確定的。那么只需用一層循環(huán)來采樣到達(dá)停車點(diǎn)的時間即可。（到此為止，有沒有感覺規(guī)劃很像最優(yōu)控制里面的泛函分析，其實理論都是一樣的）

在介紹跟車/超車的采樣邏輯之前，需要介紹一下S-T圖的概念。
以下圖中的場景為例，藍(lán)色障礙車從車道右側(cè)切入，在T_in時刻開始進(jìn)入當(dāng)前車道。那么這個場景對應(yīng)的S-T圖就如下圖所示。從T_in時刻開始出現(xiàn)一塊斜向上的陰影區(qū)域。這塊陰影區(qū)域的高度就是藍(lán)色障礙車的車身長，上邊界表示車頭，下邊界表示車尾，斜率表示車速。

如果上述場景變成這樣，障礙車從T_in時刻進(jìn)入車道，然后在T_out時刻離開車道。那么這個場景對應(yīng)的S-T圖就會縮短，如下圖。

有了S-T圖的概念，觀察下圖中的兩條規(guī)劃軌跡。
紅色的是一條跟車軌跡，綠色的是超車軌跡。這兩條軌跡反映在S-T圖中，就如下圖所示。紅色的跟車軌跡在藍(lán)色陰影區(qū)域下方，綠色的超車軌跡在藍(lán)色陰影區(qū)域上方。

采樣末狀態(tài)時，就可以分別在S-T圖中障礙物對應(yīng)的陰影區(qū)域的上方和下方分別采樣。
上方的末狀態(tài)對應(yīng)超車，下方的末狀態(tài)對應(yīng)跟車。

如果有多個障礙物，就對這些障礙物分別采樣超車和跟車所對應(yīng)的末狀態(tài)。

那么總結(jié)下來就是遍歷所有和車道有關(guān)聯(lián)的障礙物，對他們分別采樣超車和跟車的末狀態(tài)，然后用多項式擬合即可獲得一系列縱向軌跡。

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

軌跡COST的實現(xiàn)方法

現(xiàn)在介紹一下軌跡的cost的實現(xiàn)方法。

前面提到，軌跡規(guī)劃所需要滿足的四點(diǎn)要求，分別是到達(dá)目的、符合交規(guī)，避免碰撞、平穩(wěn)舒適。針對這四點(diǎn)要求，我們設(shè)計了六個cost，cost越高就表示越不滿足要求。下面一一介紹這六個cost的設(shè)計思路。

首先是到達(dá)目的的cost。分成兩種情況，一個是存在停車指令（比如紅燈）的情況，另一個是沒有停車指令的。如果存在停車指令，相對大的車速，其對應(yīng)的軌跡cost就越大；如果沒有停車指令，那么低速軌跡的cost就會越大。

怎么實現(xiàn)這樣的效果呢？針對這兩種情況分別設(shè)計了參考速度。
下圖藍(lán)線表示沒有停車指令時的參考速度。可以看到這種情況下，綠色的加速軌跡會獲得一個較小的cost，而紅色的減速軌跡會獲得一個相對較大的cost。那么如果存在停車指令，參考速度就會想右圖中的藍(lán)色曲線一樣呈下降趨勢。那么這種情況下，同樣的兩條軌跡，他們的cost大小關(guān)系就會正好相反。

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

第三個cost是碰撞cost。下圖中的兩條軌跡，反映在右圖S-T圖中，我們可以發(fā)現(xiàn)紅色的軌跡和藍(lán)色障礙車在S-T圖中的陰影區(qū)域有重疊，說明有碰撞風(fēng)險，那么它的碰撞cost就會相對較高。而綠色的軌跡在S-T圖中反映出來的碰撞風(fēng)險較小，那么它的碰撞cost就相對較低。

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

第五個cost是橫向加速度的cost。
設(shè)計這個cost是為了平穩(wěn)地?fù)Q道。那么像下圖猛打方向盤的軌跡，它的橫向加速度cost就會相對較大。

最后一個cost是向心加速度cost。
設(shè)計這個cost是為了在轉(zhuǎn)彎或調(diào)頭的時候能夠減速慢行。在彎道處，車速慢的軌跡，其向心加速度cost就會相對較低，那么就會更容易被率先挑選出來。

這六個cost的加權(quán)求和就是軌跡的總cost。開發(fā)者可以根據(jù)產(chǎn)品的需要，調(diào)試這六個權(quán)重。

限制檢測和碰撞檢測

這里介紹一下限制檢測和碰撞檢測。

限制檢測考察的內(nèi)容有軌跡的加速度、加加速度、和曲率。碰撞檢測則是把自動駕駛汽車的軌跡和其他障礙物的預(yù)測軌跡進(jìn)行比對，觀察是否有軌跡重疊。

換道場景

對于換道場景，Lattice算法僅僅需要對目標(biāo)車道對應(yīng)的參考線做一次采樣+選擇的流程。本車道和目標(biāo)車道均能產(chǎn)生一條最優(yōu)軌跡。給換道軌跡的cost上增加額外的車道優(yōu)先級的cost，再將兩條軌跡比較，選擇cost較小的那條即可。

最后，奉上資料的鏈接：

https://mp.weixin.qq.com/s/YDIoVf20kybu8JEUY3GZWg

https://zhuanlan.zhihu.com/p/399545248

總結(jié)

以上是生活随笔為你收集整理的Apollo学习笔记（12）Lattice Planner规划算法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。