1.軌跡規(guī)劃基本介紹

軌跡規(guī)劃的概念

自動(dòng)駕駛軌跡規(guī)劃的核心就是要解決車(chē)輛該怎么走的問(wèn)題。一輛自動(dòng)駕駛車(chē)輛處在周?chē)行腥恕ⅡT自行車(chē)的人以及前方有卡車(chē)的環(huán)境下，現(xiàn)在它需要左轉(zhuǎn)，該怎么做？這就是軌跡規(guī)劃該解決的問(wèn)題。



軌跡規(guī)劃的輸入包括拓?fù)涞貓D、障礙物及障礙物的預(yù)測(cè)軌跡、交通信號(hào)燈的狀態(tài)，還有定位、導(dǎo)航（因?yàn)橹滥康牡夭拍芤?guī)劃路徑）、車(chē)輛狀態(tài)等其他信息。而軌跡規(guī)劃的輸出則是一個(gè)軌跡，軌跡是一個(gè)時(shí)間到位置的函數(shù)，也就是特定的時(shí)刻要求車(chē)輛在特定的位置上。上圖中軌跡函數(shù)是t→(x,y,z)，把z標(biāo)灰是因?yàn)槟壳盀橹刮覀兊能?chē)還不會(huì)飛，所以z是(x, y)的函數(shù)。

非凸優(yōu)化問(wèn)題

軌跡規(guī)劃本質(zhì)上來(lái)說(shuō)是一個(gè)優(yōu)化問(wèn)題。談到優(yōu)化問(wèn)題，我們往往需要知道優(yōu)化的約束和優(yōu)化的目標(biāo)。

首先看軌跡規(guī)劃的約束，第一個(gè)約束就是車(chē)輛要遵守交規(guī)，這是強(qiáng)制性的約束；第二個(gè)約束是要避免碰撞；第三個(gè)約束是要使規(guī)劃的軌跡在車(chē)輛控制上可實(shí)現(xiàn)，例如不能出現(xiàn)規(guī)劃出一條車(chē)根本拐不過(guò)來(lái)的急彎的軌跡。

而優(yōu)化的目標(biāo)是使得自動(dòng)駕駛跟人類(lèi)司機(jī)駕駛相似，具體表現(xiàn)就是乘客乘坐時(shí)感到舒適。但“開(kāi)得像人”也有不同的優(yōu)化目標(biāo)，比如我們可以駕駛得穩(wěn)一點(diǎn)也可以開(kāi)得急一點(diǎn)，像老司機(jī)一般。

這樣一個(gè)優(yōu)化問(wèn)題在數(shù)學(xué)上的性質(zhì)是什么呢？數(shù)學(xué)上，解決一個(gè)優(yōu)化問(wèn)題，首先看這個(gè)優(yōu)化問(wèn)題是不是凸的，因?yàn)橥沟膯?wèn)題比較好解一些。

什么是凸優(yōu)化問(wèn)題？可以簡(jiǎn)單描述為，一個(gè)問(wèn)題如果有兩個(gè)可行解，則要滿足這兩個(gè)可行解的線性組合也是可行的，且不比這兩個(gè)可行解都差。

那么軌跡規(guī)劃是不是一個(gè)凸優(yōu)化問(wèn)題呢？可以拆解來(lái)看，我們求解軌跡規(guī)劃的t→(x,y)問(wèn)題時(shí)，發(fā)現(xiàn)其復(fù)雜度較高且計(jì)算量較大。所以常見(jiàn)的解法是把軌跡規(guī)劃分成橫向規(guī)劃和縱向規(guī)劃?？梢园堰@兩個(gè)子問(wèn)題看成t→(x,y)在空間上的投影，只要二者之一不是凸的，則軌跡規(guī)劃問(wèn)題就不會(huì)是凸的。

橫向規(guī)劃，即s→(x,y)，決定了軌跡的形狀。縱向規(guī)劃是t→s，是指在此形狀上運(yùn)動(dòng)的速度狀態(tài)，也就是時(shí)間與位移的關(guān)系。橫向規(guī)劃和縱向規(guī)劃聯(lián)合起來(lái)就是t→(x,y)。



那么橫向規(guī)劃是凸優(yōu)化問(wèn)題嗎？如上圖中的左側(cè)圖，沒(méi)有顯示時(shí)間信息，這就是橫向規(guī)劃。兩條黑線代表不同的變道軌跡都是可行解，然后我們看其線性組合也就是中間的紅線，這條紅線的變道軌跡明顯是不能用的——它變道太慢，騎線行駛距離過(guò)長(zhǎng)。所以，根據(jù)數(shù)學(xué)定義，橫向規(guī)劃是非凸優(yōu)化問(wèn)題。

再來(lái)看看縱向規(guī)劃。縱向規(guī)劃常以上圖右側(cè)的t - s圖表示，t表示時(shí)間，s表示我們走過(guò)的路程。當(dāng)一個(gè)行人橫穿馬路時(shí)，我們可以用白色矩形在t - s圖上表示此過(guò)程，左邊界表示行人進(jìn)入規(guī)劃路徑（考慮車(chē)寬）的時(shí)刻，右邊界表示離開(kāi)規(guī)劃路徑的時(shí)刻。橫軸上的上下邊界差可以理解為行人占用的規(guī)劃路徑的寬度。

面對(duì)這樣場(chǎng)景，縱向規(guī)劃將有兩種選擇，一種是車(chē)要讓人，對(duì)應(yīng)圖中白色矩形下方的綠色路線；一種是車(chē)輛加速超過(guò)，即上方的綠色路線。但它的線性組合可能是圖中的紅線，而這條路線中行人與車(chē)已經(jīng)撞上，顯然是不可行的路線（解）。所以縱向規(guī)劃也不是一個(gè)凸的問(wèn)題。

2.為什么需要決策模塊？

決策使軌跡規(guī)劃化繁為簡(jiǎn)

既然軌跡規(guī)劃是非凸優(yōu)化問(wèn)題，我們需要利用決策模塊來(lái)解決這個(gè)問(wèn)題。

什么是自動(dòng)駕駛的決策模塊呢？從數(shù)學(xué)上來(lái)講，決策就是為了限定非凸問(wèn)題（軌跡規(guī)劃）的解空間，將問(wèn)題轉(zhuǎn)化為凸的。



我們可以看上圖的例子：兩個(gè)行人正在橫穿馬路，自動(dòng)駕駛大體上可以作出四種不同的決策，即讓兩個(gè)人；超兩個(gè)人；讓一超一；超一讓一。

而一旦決策確定，那么路徑規(guī)劃就可轉(zhuǎn)換為凸的問(wèn)題，求解就會(huì)相對(duì)來(lái)說(shuō)比較容易，也會(huì)使解更加穩(wěn)定。因?yàn)橐粋€(gè)單純的數(shù)值優(yōu)化問(wèn)題的求解，很難保證每一幀的解都是相對(duì)穩(wěn)定的，甚至可能出現(xiàn)第一幀要讓?zhuān)诙瑓s要沖的問(wèn)題，決策可以提前規(guī)避這種不穩(wěn)定。

我們需要注意，決策是一個(gè)NP-hard（非確定性多項(xiàng)式困難）問(wèn)題。自動(dòng)駕駛需要決策的場(chǎng)景涉及許多，包括搶行還是讓行，是否要沖黃燈，在哪兩輛車(chē)之間變道、并線，是否要主動(dòng)變道，是從左還是右繞行前方障礙物。只有做了決策才能使軌跡規(guī)劃問(wèn)題變?yōu)橥箖?yōu)化問(wèn)題。



處理凸問(wèn)題可以利用的快速算法有許多，線性規(guī)劃，二次規(guī)劃，序列二次規(guī)劃……這些都是數(shù)值求解優(yōu)化問(wèn)題的方式。例如在二次規(guī)劃中，如果Q正定，二次規(guī)劃就是凸的問(wèn)題，求解它會(huì)更加迅速和容易。

縱向規(guī)劃決策求解：動(dòng)態(tài)規(guī)劃

在縱向規(guī)劃決策上，我想介紹一種以動(dòng)態(tài)規(guī)劃算法作為決策的方法。

看看下圖的案例，假設(shè)有兩個(gè)人正在橫穿馬路，我們來(lái)研究一下如何決策才是最優(yōu)的選擇。



首先，將t - s圖進(jìn)行離散化，離散化之后，我們可以認(rèn)為每一步的決策只與前邊的兩步有關(guān)系。兩步是因?yàn)槲覀兛梢越魄蟪黾铀俣龋⒛軐?xiě)出一個(gè)狀態(tài)轉(zhuǎn)移方程。

盡管這個(gè)方程在實(shí)際過(guò)程中比較難寫(xiě)，但確實(shí)是一種做法。雖然離散化t和s降低了精度，但降低精度也幫助降低了運(yùn)行時(shí)間。

注意，這種方法并不能保證
最后的速度是否舒適，它給出的是一個(gè)大概的決策方法，即到底讓或者不讓。

決策面臨的挑戰(zhàn)

第一個(gè)挑戰(zhàn)上面已經(jīng)提到，由于決策問(wèn)題是一個(gè)NP-hard問(wèn)題，不易直接求解，存在多種多樣的近似算法。

第二個(gè)挑戰(zhàn)是很難用規(guī)則去擬合人的經(jīng)驗(yàn)，包括上述的狀態(tài)轉(zhuǎn)移方程中的cost也很難去表示。目前解決這個(gè)挑戰(zhàn)可用的部分辦法是根據(jù)各種不同的情況建立數(shù)學(xué)模型，以及采用機(jī)器學(xué)習(xí)的方法進(jìn)行決策。



以上圖左側(cè)的場(chǎng)景為例，一輛正在行駛的自動(dòng)駕駛車(chē)輛打算繞行前面白車(chē)，但此時(shí)前車(chē)突然起步了，我們?cè)撊绾卫^續(xù)行駛？是變道？還是跟在后邊行駛，又或是繼續(xù)繞行？處理這種情況確實(shí)依靠人類(lèi)駕駛的經(jīng)驗(yàn)。

再看上圖右側(cè)的例子：自動(dòng)駕駛車(chē)準(zhǔn)備在前方左轉(zhuǎn)，但是前車(chē)停了很久也沒(méi)有啟動(dòng)（可能前車(chē)駕駛員沒(méi)反應(yīng)過(guò)來(lái)），我們這時(shí)該不該變道呢？對(duì)這種情況，人類(lèi)司機(jī)有時(shí)也很難判斷。由此可見(jiàn)，場(chǎng)景的多變而復(fù)雜使得決策面臨很多挑戰(zhàn)。

3.橫向規(guī)劃的解法

橫向規(guī)劃的解法

前面提到軌跡規(guī)劃可以拆成橫向和縱向的規(guī)劃，現(xiàn)在我來(lái)具體介紹橫向規(guī)劃的解法。橫向規(guī)劃就是行車(chē)方向上的規(guī)劃，可以看成是如何打方向盤(pán)的規(guī)劃，它決定了軌跡的形狀。

這個(gè)問(wèn)題通常的解法分兩種，一種是無(wú)車(chē)道的場(chǎng)景，比如在freespace（自由空間）中規(guī)劃或者泊車(chē)之類(lèi)的場(chǎng)景，車(chē)輛一般處在低速行駛狀態(tài)，缺乏車(chē)道線等先驗(yàn)信息。業(yè)界大多都用搜索等路徑生成的方式去處理無(wú)車(chē)道場(chǎng)景。

另一種是有車(chē)道的情況。雖然可以參考車(chē)道線信息，但是規(guī)劃上想輸出s→(x,y)函數(shù)，難度并不小。常見(jiàn)的做法是離線生成參考線，隨后就可以將求解s→(x,y)的問(wèn)題變?yōu)榍蠼鈙→l的問(wèn)題，l是指車(chē)輛在這個(gè)參考線上的橫向偏移量。



以上圖右側(cè)場(chǎng)景為例，原本車(chē)是沿車(chē)道向前走，但由于有前方車(chē)輛的遮擋，我們就必須繞行它，即向右橫向偏移。



而參考線的生成，其實(shí)類(lèi)似于開(kāi)卡丁車(chē)時(shí)別人教你的過(guò)彎的最優(yōu)路線，它也是一個(gè)優(yōu)化問(wèn)題，當(dāng)然也要保證安全性和舒適性。方便的是，有了高精地圖輔助后，參考線可以通過(guò)離線去進(jìn)行，所以可以用一些開(kāi)銷(xiāo)比較大的算法做到最優(yōu)。參考線的約束在于其需要在車(chē)道線內(nèi)，并且在控制上可實(shí)現(xiàn)。優(yōu)化目標(biāo)則是參考線需接近車(chē)道中心，曲率不能太大，曲率變化率也不大。



確定參考線后，通過(guò)把參考線離散化，采一些點(diǎn)出來(lái)，那么橫向規(guī)劃問(wèn)題就轉(zhuǎn)化為求解一個(gè)離參考線偏移距離的一個(gè)問(wèn)題，即轉(zhuǎn)化成s→l的問(wèn)題。其約束是車(chē)輛行駛不跨越邊界，避免碰撞，而優(yōu)化的目標(biāo)是要離參考線近，要離障礙物遠(yuǎn)，曲率不大，曲率變化率不大等等。



借助上圖右側(cè)的例子，你會(huì)發(fā)現(xiàn)橫向規(guī)劃可看成一個(gè)二次規(guī)劃（QP）的問(wèn)題。其中“0”，“-1”，“-2”是自動(dòng)駕駛車(chē)行駛過(guò)的路徑，0號(hào)點(diǎn)是車(chē)當(dāng)前的位置，現(xiàn)在我們需要解的就是1，2，3，4，5，6這些點(diǎn)相對(duì)于參考線的橫向偏移x。換句話說(shuō)，已知x[-2]，x[-1]，x[0]，求解x[1]，x[2]等。

該函數(shù)約束是車(chē)輛行駛不能超過(guò)左右邊界包括馬路牙，實(shí)線，障礙物等。優(yōu)化目標(biāo)則是車(chē)輛要離參考線近，方向盤(pán)不能打太多太快，保證乘坐的舒適。上圖中的公式其實(shí)是一個(gè)關(guān)于x的二次的形式，所以可以使用二次規(guī)劃的方法來(lái)解決。

橫向規(guī)劃的挑戰(zhàn)

雖然大部分時(shí)候車(chē)都行駛在有車(chē)道線的馬路上，但也會(huì)面臨特殊的挑戰(zhàn)。例如下圖里左側(cè)顯示的路口，我們的車(chē)行駛的方向上有三條直行車(chē)道，但過(guò)路口后變成四條直行車(chē)道，并且對(duì)得很不齊?，F(xiàn)實(shí)中，右側(cè)的行駛的車(chē)（白車(chē)）往往不依據(jù)車(chē)道線行駛，可能會(huì)橫跨車(chē)道線擠占自動(dòng)駕駛車(chē)輛所在車(chē)道的空間。

而下圖右上角則展示了沒(méi)有車(chē)道線的主輔路場(chǎng)景。在這種主輔路之間切換，就像解決一種沒(méi)有參考線的freespace的規(guī)劃，挑戰(zhàn)也不小?？偟膩?lái)說(shuō)，要想解決沒(méi)有車(chē)道線或者說(shuō)普通車(chē)輛不按車(chē)道線行駛的路徑規(guī)劃問(wèn)題，難度都不小。



另外的挑戰(zhàn)就是環(huán)境的問(wèn)題，因?yàn)檐?chē)外行駛環(huán)境瞬息萬(wàn)變，要對(duì)周?chē)M(jìn)行環(huán)境預(yù)測(cè)也很困難。以下圖為例，我們的自動(dòng)駕駛車(chē)準(zhǔn)備往左變道，而左下角橙色塊代表的摩托車(chē)正飛速地向前行駛，于是我們的車(chē)輛迅速給出取消變道的決策，生成平滑的曲線回到原來(lái)行駛的道路上。因此，面對(duì)這類(lèi)的情況，軌跡規(guī)劃模塊需要保證規(guī)劃的路徑光滑且在控制上可實(shí)現(xiàn)。



4.縱向規(guī)劃

縱向規(guī)劃的定義和場(chǎng)景

縱向規(guī)劃本質(zhì)是對(duì)車(chē)輛在設(shè)定好的路徑上的速度規(guī)劃，決定了車(chē)輛在整個(gè)軌跡上的運(yùn)動(dòng)過(guò)程。

求解這類(lèi)優(yōu)化問(wèn)題，第一個(gè)約束是遵守交規(guī)（信號(hào)燈、限速、停車(chē)讓行等），第二個(gè)約束是避免碰撞。而縱向規(guī)劃的優(yōu)化的目標(biāo)是乘坐舒適，也意味著車(chē)輛的速度變化率不大，加速度變化率不大，行駛速度也要盡量快一點(diǎn)（限速內(nèi)）等。

前邊我提到了行人橫穿馬路的場(chǎng)景，在t - s圖中，行人的運(yùn)動(dòng)過(guò)程可以轉(zhuǎn)化一個(gè)矩形，最終給出了兩種車(chē)輛的對(duì)應(yīng)決策——加速超過(guò)行人或減速讓行。那么決策之后該怎么做呢？



如果決定要搶行，我們可以將矩形的約束條件擴(kuò)展到最下部，便能轉(zhuǎn)化為凸問(wèn)題求最優(yōu)解。如果采取避讓?zhuān)?chē)輛的路線則從t - s圖中的矩形下邊穿過(guò)。



但是如果是行人斜著穿過(guò)馬路呢？在t - s圖中，行人的運(yùn)動(dòng)過(guò)程又該如何表示？答案就是一個(gè)斜向上的平行四邊形（如上圖）。

黃燈也是我們要應(yīng)對(duì)的場(chǎng)景之一。黃燈即將到來(lái)，如果決策要沖，那么車(chē)輛須盡快通過(guò)路口，否則很容易被逼停在路中間出不去。



這種情況我們也可以用一樣的t - s圖表示（上圖），左邊界是表示黃燈亮的時(shí)刻，這個(gè)白色矩形存在一個(gè)缺角。當(dāng)黃燈亮起的時(shí)候，車(chē)輛如果要盡快通過(guò)路口，那么隨著t在增大的過(guò)程中，s也要迅速增大，并且增大的速率要超過(guò)缺角的斜率。



再看一些更有趣的場(chǎng)景案例（上圖）。當(dāng)自動(dòng)駕駛車(chē)跟車(chē)時(shí)，假設(shè)所跟的前車(chē)在減速，如果能夠精確預(yù)測(cè)前車(chē)的運(yùn)動(dòng)的狀態(tài)，那么展現(xiàn)在t - s圖中的白色部分會(huì)出現(xiàn)各種各樣的形狀，這樣解優(yōu)化問(wèn)題就能解出一條好的速度曲線。

縱向規(guī)劃的挑戰(zhàn)

縱向規(guī)劃會(huì)面臨什么挑戰(zhàn)呢？



首先存在博弈的挑戰(zhàn)。以上圖為例，自動(dòng)駕駛車(chē)前方的左轉(zhuǎn)綠燈亮起并準(zhǔn)備左轉(zhuǎn)，這時(shí)，一輛電動(dòng)車(chē)突然出現(xiàn)在左前方，準(zhǔn)備快速橫穿馬路。

這種情況，人類(lèi)駕駛員會(huì)怎么開(kāi)呢？人類(lèi)司機(jī)會(huì)和電動(dòng)車(chē)司機(jī)迅速對(duì)一下眼神，通過(guò)眼神比較氣勢(shì)誰(shuí)猛，另一方就會(huì)主動(dòng)地讓對(duì)方。當(dāng)然這有開(kāi)玩笑的成分。

但在決策上這個(gè)場(chǎng)景并不好處理，它是一個(gè)博弈的過(guò)程，自動(dòng)駕駛車(chē)不能一開(kāi)始就決定無(wú)視還是讓步，所以在很多時(shí)候要在激進(jìn)和保守之間掌握一個(gè)平衡點(diǎn)，這需要不同的參數(shù)和不同的模式去處理不同場(chǎng)景。

除此之外，感知和預(yù)測(cè)帶來(lái)的困難也會(huì)使縱向規(guī)劃面臨挑戰(zhàn)。

可以看下圖中右上方連續(xù)兩張相似的圖，在第二張圖里你會(huì)發(fā)現(xiàn)有人突然從車(chē)前沖出來(lái)，俗稱(chēng)叫做“鬼探頭”，也就是盲區(qū)。對(duì)于這種情況，感知需要提前檢測(cè)到盲區(qū)，車(chē)輛進(jìn)行減速，規(guī)避可能的安全隱患。



預(yù)測(cè)給規(guī)劃帶來(lái)的挑戰(zhàn)出現(xiàn)在左下角（上圖）的場(chǎng)景里。此時(shí)，自動(dòng)駕駛車(chē)的右前方行駛著一輛面包車(chē)，面包車(chē)前邊有一輛速度很慢的自行車(chē)，一般人類(lèi)司機(jī)會(huì)主動(dòng)預(yù)測(cè)面包車(chē)極有可能向左變道。但這類(lèi)場(chǎng)景對(duì)預(yù)測(cè)模塊提出了很大的挑戰(zhàn)，如果缺乏這類(lèi)預(yù)測(cè)，自動(dòng)駕駛車(chē)輛的后續(xù)應(yīng)對(duì)同樣挑戰(zhàn)不小。



自動(dòng)駕駛還有一些極端的情況，需要考慮到橫縱向協(xié)調(diào)配合。上圖是一個(gè)非常極限的車(chē)輛加塞案例：自動(dòng)駕駛車(chē)正在高速行駛時(shí)，右側(cè)的一輛慢車(chē)突然加塞，一般人類(lèi)司機(jī)會(huì)選擇打方向避讓?zhuān)绻?dāng)時(shí)左車(chē)道沒(méi)有車(chē)，甚至?xí)蜃笞兊?，如果左?chē)道有車(chē)，他也會(huì)扭一點(diǎn)方向進(jìn)行避讓。

這類(lèi)處理就需要橫縱向規(guī)劃的配合，共同解決極端情況。比如從縱向規(guī)劃來(lái)說(shuō)，當(dāng)時(shí)已經(jīng)無(wú)法保持安全車(chē)距，規(guī)劃需要做到的是保證不相撞，并盡快拉開(kāi)車(chē)距，而不是一腳剎到底。