l———S澄者，S———T
規(guī)劃模塊在apollo中承上啟下笆呆，Lattice算法隸屬于規(guī)劃模塊。規(guī)劃模塊以預(yù)測(cè)模塊闷哆、routing模塊腰奋、高精地圖和定位的結(jié)果作為輸入，通過算法抱怔，輸出一條平穩(wěn)劣坊、舒適、安全的軌跡屈留，交給控制模塊去執(zhí)行局冰。
圖片.png
1.規(guī)劃處理流程：根據(jù)定位和路由模塊测蘑，經(jīng)過平滑生成一條平滑的參考線（道路中心線），再經(jīng)過規(guī)劃算法生成一條平穩(wěn)康二、舒適碳胳、安全的軌跡。
圖片.png
規(guī)劃算法需要沫勿，能到達(dá)目的地挨约、遵守交規(guī)、避障产雹、舒適诫惭。生成的軌跡包含位置、速度蔓挖、加速度信息夕土。
圖片.png
2.lattice規(guī)劃算法流程
圖片.png
以上圖中的場(chǎng)景為例。其中紅車是我們的自動(dòng)駕駛汽車瘟判，藍(lán)車是其他障礙車怨绣，前面藍(lán)色帶尖頭的曲線是藍(lán)車的預(yù)測(cè)軌跡。那么這是一個(gè)前方即將有車輛并入的場(chǎng)景拷获。面對(duì)這樣的場(chǎng)景篮撑，有些司機(jī)會(huì)按照右圖中淺紅色的軌跡，選擇繞開藍(lán)色的障礙車刀诬。另外有一些司機(jī)開車相對(duì)保守咽扇，會(huì)沿著右圖中深紅色較短的軌跡做一個(gè)減速邪财，給藍(lán)色障礙車讓路陕壹。
既然對(duì)于同一個(gè)場(chǎng)景，人類司機(jī)會(huì)有多種處理方法树埠，那么Lattice規(guī)劃算法的第一步就是采樣足夠多的軌跡糠馆，提供盡可能多的選擇。
圖片.png
Lattice規(guī)劃算法的第二步是計(jì)算每一條軌跡計(jì)算的cost怎憋。這個(gè)cost考慮了軌跡的可行性又碌、安全性等因素。
3.采樣過程
圖片.png
那么如何用Frenet坐標(biāo)系來表示一輛汽車的狀態(tài)呢绊袋？首先有一條光滑的參考線（右圖中紅線）毕匀，可以按右圖所示將汽車的坐標(biāo)點(diǎn)投影到參考線上，得到一個(gè)參考線上的投影點(diǎn)（圖中藍(lán)色點(diǎn)）癌别。從參考線起點(diǎn)到投影點(diǎn)的路徑長(zhǎng)度就是汽車在Frenet坐標(biāo)系下的縱向偏移量皂岔，用S表示。而投影點(diǎn)到汽車位置的距離則是汽車在Frenet坐標(biāo)系下的橫向偏移量展姐，用L表示躁垛。因?yàn)閰⒖季€是足夠光滑的剖毯，也可通過汽車的朝向、速度教馆、加速度來計(jì)算出Frenet坐標(biāo)系下逊谋，橫向和縱向偏移量的一階導(dǎo)和二階導(dǎo)。
將橫向偏移量L設(shè)計(jì)成縱向偏移量S的函數(shù)土铺。這是因?yàn)閷?duì)于大多數(shù)的汽車而言胶滋，橫向運(yùn)動(dòng)是由縱向運(yùn)動(dòng)誘發(fā)的。
有了Frenet坐標(biāo)系的概念悲敷，下面來介紹一下如何生成一條軌跡镀钓。首先我們可以通過計(jì)算得到自動(dòng)駕駛汽車在Frenet坐標(biāo)系下的在零時(shí)刻的起始狀態(tài)，也就是汽車的當(dāng)前狀態(tài)镀迂。為了生成一條軌跡丁溅，第一步就是在Frenet坐標(biāo)系下采樣一個(gè)在T1時(shí)刻的末狀態(tài)。
圖片.png
第二步就是將末狀態(tài)和起始狀態(tài)做多項(xiàng)式擬合探遵。分別形成橫向和縱向的多項(xiàng)式軌跡窟赏。
圖片.png
有了橫向軌跡和縱向軌跡之后，第三步就是二維合成箱季。給定一個(gè)時(shí)刻T涯穷，可以計(jì)算出在T時(shí)刻的縱向偏移量和橫向偏移量，再通過參考線藏雏，即可還原成一個(gè)二維平面中的軌跡點(diǎn)拷况。通過一系列的時(shí)間點(diǎn)T0，T1掘殴，...赚瘦，Tn，可以獲得一系列的軌跡點(diǎn)P0奏寨，P1起意，…，Pn病瞳，最終形成一條完整的軌跡揽咕。了解了如何生成一條軌跡之后，下面介紹一下如何采樣一系列軌跡套菜。
圖片.png
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)遍歷各種組合知染，再通過多項(xiàng)式擬合，即可獲得一系列的橫向軌跡斑胜。
圖片.png
對(duì)于縱向軌跡的采樣控淡，需要考慮巡航、跟車或超車止潘、停車這三種狀態(tài)
圖片.png
對(duì)于巡航狀態(tài)掺炭，通過兩層循環(huán)來完成采樣。外層循環(huán)將速度從零到上限值按等間隔均勻遍歷凭戴。內(nèi)層循環(huán)遍歷到達(dá)末狀態(tài)速度的時(shí)間涧狮，從1秒到8秒按1秒的間隔均勻遍歷。由于巡航狀態(tài)不需要指明到達(dá)末狀態(tài)的S值么夫，所以這里只需要用四次多項(xiàng)式擬合即可者冤。
圖片.png
在停車狀態(tài)中，給定停車點(diǎn)档痪，末狀態(tài)的速度和加速度都是零涉枫，所以末狀態(tài)是確定的。那么我們只需用一層循環(huán)來采樣到達(dá)停車點(diǎn)的時(shí)間即可腐螟。
在介紹跟車/超車的采樣邏輯之前愿汰，需要介紹一下S-T圖的概念。以左圖中的場(chǎng)景為例乐纸，藍(lán)色障礙車從車道右側(cè)切入衬廷，在T_in時(shí)刻開始進(jìn)入當(dāng)前車道。那么這個(gè)場(chǎng)景對(duì)應(yīng)的S-T圖就如右圖所示锯仪。從T_in時(shí)刻開始出現(xiàn)一塊斜向上的陰影區(qū)域泵督。這塊陰影區(qū)域的高度就是藍(lán)色障礙車的車身長(zhǎng)，上邊界表示車頭庶喜，下邊界表示車尾，斜率表示車速救鲤。
圖片.png
如果上述場(chǎng)景變成這樣久窟，障礙車從T_in時(shí)刻進(jìn)入車道，然后在T_out時(shí)刻離開車道本缠。那么這個(gè)場(chǎng)景對(duì)應(yīng)的S-T圖就會(huì)縮短斥扛。
圖片.png
有了S-T圖的概念，觀察左圖中的兩條規(guī)劃軌跡。紅色的是一條跟車軌跡稀颁，綠色的是超車軌跡芬失。這兩條軌跡反映在S-T圖中，就如右圖所示匾灶。紅色的跟車軌跡在藍(lán)色陰影區(qū)域下方棱烂，綠色的超車軌跡在藍(lán)色陰影區(qū)域上方。
圖片.png
采樣末狀態(tài)時(shí)阶女，就可以分別在S-T圖中障礙物對(duì)應(yīng)的陰影區(qū)域的上方和下方分別采樣颊糜。上方的末狀態(tài)對(duì)應(yīng)超車，下方的末狀態(tài)對(duì)應(yīng)跟車秃踩。
圖片.png
如果有多個(gè)障礙物衬鱼，就對(duì)這些障礙物分別采樣超車和跟車所對(duì)應(yīng)的末狀態(tài)
圖片.png
是遍歷所有和車道有關(guān)聯(lián)的障礙物，對(duì)他們分別采樣超車和跟車的末狀態(tài)憔杨，然后用多項(xiàng)式擬合即可獲得一系列縱向軌跡鸟赫。
圖片.png
將三組縱向軌跡組合起來，就可以獲得所有縱向軌跡消别。再將所有縱向軌跡和所有橫向軌跡兩兩配對(duì)二維合成惯疙，就可以完成軌跡采樣的工作。
圖片.png
5.軌跡cost實(shí)現(xiàn)方法
軌跡規(guī)劃所需要滿足的四點(diǎn)要求妖啥，分別是到達(dá)目的霉颠、符合交規(guī)，避免碰撞荆虱、平穩(wěn)舒適蒿偎。針對(duì)這四點(diǎn)要求，我們?cè)O(shè)計(jì)了六個(gè)cost怀读，cost越高就表示越不滿足要求诉位。下面一一介紹這六個(gè)cost的設(shè)計(jì)思路。
圖片.png
首先是到達(dá)目的的cost菜枷。分成兩種情況苍糠，一個(gè)是存在停車指令（比如紅燈）的情況，另一個(gè)是沒有停車指令的啤誊。如果存在停車指令岳瞭，相對(duì)大的車速，其對(duì)應(yīng)的軌跡cost就越大蚊锹；如果沒有停車指令瞳筏，那么低速軌跡的cost就會(huì)越大。
圖片.png
怎么實(shí)現(xiàn)這樣的效果呢牡昆？針對(duì)這兩種情況分別設(shè)計(jì)了參考速度姚炕。左圖藍(lán)線表示沒有停車指令時(shí)的參考速度。可以看到這種情況下柱宦，綠色的加速軌跡會(huì)獲得一個(gè)較小的cost些椒，而紅色的減速軌跡會(huì)獲得一個(gè)相對(duì)較大的cost。那么如果存在停車指令掸刊，參考速度就會(huì)想右圖中的藍(lán)色曲線一樣呈下降趨勢(shì)免糕。那么這種情況下，同樣的兩條軌跡痒给，他們的cost大小關(guān)系就會(huì)正好相反说墨。
圖片.png
第二個(gè)cost是橫向偏移cost。設(shè)計(jì)這個(gè)cost是為了讓自動(dòng)駕駛汽車能盡量沿著道路中心行駛苍柏。那么像左圖汽車靠道路一邊行駛尼斧，和中圖畫龍的行駛軌跡，他們的cost都相對(duì)較高试吁。
圖片.png
第三個(gè)cost是碰撞cost棺棵。左圖中的兩條軌跡，反映在右圖S-T圖中熄捍，我們可以發(fā)現(xiàn)紅色的軌跡和藍(lán)色障礙車在S-T圖中的陰影區(qū)域有重疊烛恤，說明有碰撞風(fēng)險(xiǎn)，那么它的碰撞cost就會(huì)相對(duì)較高余耽。而綠色的軌跡在S-T圖中反映出來的碰撞風(fēng)險(xiǎn)較小缚柏，那么它的碰撞cost就相對(duì)較低。
圖片.png
第四個(gè)cost是縱向加加速度的cost碟贾。加加速度（jerk）是加速度對(duì)時(shí)間的導(dǎo)數(shù)币喧，表示加速度的變化率。我們用加加速度的最大值值來表示這個(gè)cost袱耽。
圖片.png
第五個(gè)cost是橫向加速度的cost杀餐。設(shè)計(jì)這個(gè)cost是為了平穩(wěn)地?fù)Q道。那么像左圖猛打方向盤的軌跡朱巨，它的橫向加速度cost就會(huì)相對(duì)較大史翘。
圖片.png
最后一個(gè)cost是向心加速度cost。設(shè)計(jì)這個(gè)cost是為了在轉(zhuǎn)彎或調(diào)頭的時(shí)候能夠減速慢行冀续。在彎道處琼讽，車速慢的軌跡，其向心加速度cost就會(huì)相對(duì)較低沥阳，那么就會(huì)更容易被率先挑選出來跨琳。
圖片.png
這六個(gè)cost的加權(quán)求和就是軌跡的總cost。開發(fā)者可以根據(jù)產(chǎn)品的需要桐罕，調(diào)試這六個(gè)權(quán)重。
圖片.png
介紹一下限制檢測(cè)和碰撞檢測(cè)。限制檢測(cè)考察的內(nèi)容有軌跡的加速度功炮、加加速度溅潜、和曲率。碰撞檢測(cè)則是把自動(dòng)駕駛汽車的軌跡和其他障礙物的預(yù)測(cè)軌跡進(jìn)行比對(duì)薪伏，觀察是否有軌跡重疊滚澜。
圖片.png
對(duì)于換道場(chǎng)景，Lattice算法僅僅需要對(duì)目標(biāo)車道對(duì)應(yīng)的參考線做一次采樣+選擇的流程嫁怀。本車道和目標(biāo)車道均能產(chǎn)生一條最優(yōu)軌跡设捐。給換道軌跡的cost上增加額外的車道優(yōu)先級(jí)的cost，再將兩條軌跡比較塘淑，選擇cost較小的那條即可萝招。
圖片.png
