人類(lèi)在駕駛時(shí)面對(duì)不同的路況會(huì)有不同的解決方案，自動(dòng)駕駛系統(tǒng)也是如此。而決策規(guī)劃就如大腦進(jìn)行狀況預(yù)測(cè)。這一預(yù)測(cè)過(guò)程涵蓋了路況分析、行為決策、動(dòng)作規(guī)劃和反饋控制等多重內(nèi)容，同時(shí)還要重視安全和舒適兩大要點(diǎn)，進(jìn)而構(gòu)成了自動(dòng)駕駛系統(tǒng)的核心部分。

想要同時(shí)兼顧這些條件并不容易，當(dāng)前在業(yè)界關(guān)于決策規(guī)劃也并沒(méi)有成熟統(tǒng)一的解決方案。但一些優(yōu)秀的規(guī)劃算法卻可以很好的完成這些任務(wù)。比如Lattice Planner，遵循先采樣后選擇的邏輯，依據(jù)從安全性、舒適性和交規(guī)抽象出的cost函數(shù)來(lái)保障軌跡的平滑。

上周，來(lái)自百度Apollo的高級(jí)研發(fā)工程師許珂誠(chéng)，在Apollo開(kāi)發(fā)者社群內(nèi)分享了有關(guān)Lattice Plannner的內(nèi)容，幫助開(kāi)發(fā)者更加了解Apollo核心算法的邏輯與細(xì)節(jié)。

Lattice Planner 總體概覽

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

這是Apollo中規(guī)劃模塊的工作流程。首先是依據(jù)Routing和定位，通過(guò)平滑算法，生成一條平滑的參考線（平滑的道路中心線）。再通過(guò)規(guī)劃算法，生成一條符合交規(guī)，安全舒適的規(guī)劃軌跡。那么Lattice算法就是Apollo開(kāi)源平臺(tái)中，其中的一種規(guī)劃算法。

一個(gè)合格規(guī)劃算法，必須滿(mǎn)足幾個(gè)條件。首先，必須能夠使自動(dòng)駕駛汽車(chē)到達(dá)目的地；其次，必須符合交規(guī)；第三，能夠避免碰撞；最后，也需要能保證一定的舒適性。在Apollo中，規(guī)劃算法的輸出是一系列軌跡點(diǎn)連成的軌跡。每一個(gè)軌跡點(diǎn)包含位置，速度，加速的等信息。

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

下面介紹一下Lattice規(guī)劃算法的工作流程。我們以右圖中的場(chǎng)景為例。其中紅車(chē)是我們的自動(dòng)駕駛汽車(chē)，藍(lán)車(chē)是其他障礙車(chē)，前面藍(lán)色帶尖頭的曲線是藍(lán)車(chē)的預(yù)測(cè)軌跡。那么這是一個(gè)前方即將有車(chē)輛并入的場(chǎng)景。

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

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

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

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

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

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

這樣的過(guò)程循環(huán)繼續(xù)下去，假設(shè)現(xiàn)在挑選出右圖中靠左邊的深紅色軌跡，它既符合汽車(chē)的物理性狀，也不會(huì)有碰撞風(fēng)險(xiǎn)。

最終就將這條軌跡作為規(guī)劃軌跡輸出。

那么下面將對(duì)每一個(gè)步驟，進(jìn)行詳細(xì)的說(shuō)介紹。

3Lattice Planner的采樣過(guò)程

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

那么如何用Frenet坐標(biāo)系來(lái)表示一輛汽車(chē)的狀態(tài)呢？首先有一條光滑的參考線（右圖中紅線），可以按右圖所示將汽車(chē)的坐標(biāo)點(diǎn)投影到參考線上，得到一個(gè)參考線上的投影點(diǎn)（圖中藍(lán)色點(diǎn)）。從參考線起點(diǎn)到投影點(diǎn)的路徑長(zhǎng)度就是汽車(chē)在Frenet坐標(biāo)系下的縱向偏移量，用S表示。而投影點(diǎn)到汽車(chē)位置的距離則是汽車(chē)在Frenet坐標(biāo)系下的橫向偏移量，用L表示。因?yàn)閰⒖季€是足夠光滑的，也可通過(guò)汽車(chē)的朝向、速度、加速度來(lái)計(jì)算出Frenet坐標(biāo)系下，橫向和縱向偏移量的一階導(dǎo)和二階導(dǎo)。

這里需要注意的是，我們將橫向偏移量L設(shè)計(jì)成縱向偏移量S的函數(shù)。這是因?yàn)閷?duì)于大多數(shù)的汽車(chē)而言，橫向運(yùn)動(dòng)是由縱向運(yùn)動(dòng)誘發(fā)的。

如何生成一條軌跡

有了Frenet坐標(biāo)系的概念，下面來(lái)介紹一下如何生成一條軌跡。首先我們可以通過(guò)計(jì)算得到自動(dòng)駕駛汽車(chē)在Frenet坐標(biāo)系下的在零時(shí)刻的起始狀態(tài)，也就是汽車(chē)的當(dāng)前狀態(tài)。為了生成一條軌跡，第一步就是在Frenet坐標(biāo)系下采樣一個(gè)在T1時(shí)刻的末狀態(tài)。

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

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

4如何采樣橫向和縱向軌跡

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

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

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

在停車(chē)狀態(tài)中，給定停車(chē)點(diǎn)，末狀態(tài)的速度和加速度都是零，所以末狀態(tài)是確定的。

那么我們只需用一層循環(huán)來(lái)采樣到達(dá)停車(chē)點(diǎn)的時(shí)間即可。

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

如果上述場(chǎng)景變成這樣，障礙車(chē)從T_in時(shí)刻進(jìn)入車(chē)道，然后在T_out時(shí)刻離開(kāi)車(chē)道。那么這個(gè)場(chǎng)景對(duì)應(yīng)的S-T圖就會(huì)縮短。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

限制檢測(cè)和碰撞檢測(cè)

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

換道場(chǎng)景

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