這本文為Pony.ai Tech lead梁亞雄分享《自動(dòng)駕駛中軌跡規(guī)劃的探索與挑戰(zhàn)》，從軌跡規(guī)劃的概念、決策、橫向規(guī)劃、縱向規(guī)劃四個(gè)方面介紹了軌跡規(guī)劃的難題。

編輯整理：孫昊

內(nèi)容來(lái)源：Pony.ai & DataFun AI Talk

出品社區(qū)：DataFun

注：歡迎轉(zhuǎn)載，轉(zhuǎn)載請(qǐng)注明出處。

大家好，今天我們主要介紹一下軌跡規(guī)劃的探索和挑戰(zhàn)，我主要從四個(gè)方面介紹：

軌跡規(guī)劃的概念

決策

橫向規(guī)劃

縱向規(guī)劃

軌跡規(guī)劃的概念：

軌跡規(guī)劃的核心就是要解決車(chē)輛該怎么走的問(wèn)題。比如我們知道了附近有行人、騎自行車(chē)的人以及前方的卡車(chē)，如果我們現(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ē)輛在特定的位置上。在PPT上是t ->（x,y,z），把z標(biāo)灰是因?yàn)槟壳盀橹刮覀兊能?chē)不會(huì)飛。

軌跡規(guī)劃本質(zhì)上來(lái)說(shuō)是一個(gè)優(yōu)化問(wèn)題。談到優(yōu)化問(wèn)題，我們需要知道優(yōu)化的約束和優(yōu)化的目標(biāo)。首先看約束，第一個(gè)約束就是車(chē)輛要遵守交規(guī)，這是強(qiáng)制性的約束；第二個(gè)約束是要避免碰撞；第三個(gè)約束是要使規(guī)劃的軌跡在控制上可實(shí)現(xiàn)，不能出現(xiàn)規(guī)劃了一個(gè)急彎的軌跡，車(chē)根本拐不過(guò)來(lái)的狀況。而優(yōu)化的目標(biāo)總的來(lái)說(shuō)就是要跟人開(kāi)的像，具體表現(xiàn)就是讓人乘坐起來(lái)感到舒適。但像人也有不同的優(yōu)化目標(biāo)，比如我們可以開(kāi)的穩(wěn)一點(diǎn)也可以開(kāi)的急一點(diǎn)，像老司機(jī)一點(diǎn)。

這樣一個(gè)優(yōu)化問(wèn)題在數(shù)學(xué)上的性質(zhì)是什么呢？我們關(guān)心一個(gè)優(yōu)化問(wèn)題首先看這個(gè)優(yōu)化問(wèn)題是不是凸的，因?yàn)橥沟膯?wèn)題比較好解一些。什么是凸的？我們可以簡(jiǎn)單描述對(duì)于一個(gè)問(wèn)題它的兩個(gè)可行解，其線性組合也應(yīng)該是可行的，而且不比這兩個(gè)解都要差。那么軌跡規(guī)劃是不是一個(gè)凸優(yōu)化問(wèn)題呢，并不是,它是非凸的。為什么呢？我們?cè)趯?duì)t ->(x,y)問(wèn)題進(jìn)行求解時(shí)，復(fù)雜度比較高計(jì)算量比較大。所以通常的做法是把他分成橫向規(guī)劃和縱向規(guī)劃。橫向規(guī)劃，是s ->（x,y）,就是決定軌跡的形狀是什么?？v向規(guī)劃是t ->s,也就是指在此形狀上運(yùn)動(dòng)的速度狀態(tài)，也就是時(shí)間與位移的關(guān)系。橫向規(guī)劃和縱向規(guī)劃聯(lián)合起來(lái)就是t ->(x,y)。那么橫向規(guī)劃是凸的嗎？如下圖，圖上沒(méi)有時(shí)間信息，這就是橫向規(guī)劃。兩條黑線代表不同的變道軌跡都是可行解，然后我們看其線性組合也就是中間的紅線，但明顯紅線的軌跡是不能用的，是很差的，他變道太慢，騎線行駛的距離太長(zhǎng)了，所以橫向規(guī)劃是非凸的。

我們?cè)賮?lái)看縱向規(guī)劃，我們經(jīng)常用t - s圖表示。t表示時(shí)間，s表示我們走過(guò)的路程。當(dāng)一個(gè)人橫穿馬路時(shí)我們可以用白色矩形來(lái)在t - s圖上表示這個(gè)過(guò)程，左邊界表示人進(jìn)入我們規(guī)劃路徑的時(shí)刻，右邊界表示離開(kāi)規(guī)劃路徑的時(shí)刻。橫向上，上下邊界差可以理解為人在占用我們規(guī)劃路徑的寬度。對(duì)于這樣一個(gè)縱向規(guī)劃，有兩種選擇，一種是我們要讓人，既下邊的綠色路線，一種是車(chē)輛加速超過(guò)，既上邊的綠色路線。但它的線性組合可能是這條紅線，明顯這已經(jīng)撞上了，所以這并不是一個(gè)凸的問(wèn)題。

決策：

正是因?yàn)檐壽E規(guī)劃是非凸的，所以我們需要用決策的模塊來(lái)解決。什么是決策呢？從數(shù)學(xué)上來(lái)講，決策就是限定非凸問(wèn)題的解空間，將問(wèn)題轉(zhuǎn)化為凸的。我們可以看下圖的例子，如果兩個(gè)人在穿馬路的話，我們大體上可以有四種不同的決策，既讓兩個(gè)人，超兩個(gè)人，讓一超一和超一讓一。而一旦決策確定，那么其實(shí)就可轉(zhuǎn)換為凸的問(wèn)題，求解就會(huì)相對(duì)來(lái)說(shuō)比較容易。決策也會(huì)使解更加穩(wěn)定，因?yàn)橐粋€(gè)單純的數(shù)值優(yōu)化問(wèn)題的求解，會(huì)很難保證每一幀的解是相對(duì)穩(wěn)定的，不會(huì)出現(xiàn)第一幀要讓，而第二幀卻要沖的問(wèn)題。但需要注意決策的問(wèn)題是一個(gè)NP-hard 問(wèn)題。有一些需要決策的場(chǎng)景，比如搶行還是讓行，是否要沖黃燈，在哪兩輛車(chē)之間變道、并線，是否要主動(dòng)變道，是從左還是右繞行前方障礙物，只有做了決策才能使問(wèn)題變?yōu)橥箖?yōu)化問(wèn)題，便于求解。對(duì)于凸問(wèn)題，有很多比較快速的算法進(jìn)行計(jì)算了。線性規(guī)劃，二次規(guī)劃，序列二次規(guī)劃，都是數(shù)值求解優(yōu)化問(wèn)題的方式。我們可以看一下二次規(guī)劃中，如果Q正定，二次規(guī)劃就是凸的問(wèn)題，有很快的方式求解它。

下邊就是看如何把自動(dòng)駕駛中的問(wèn)題轉(zhuǎn)化為優(yōu)化問(wèn)題。

首先是縱向規(guī)劃的決策，決策的方法我們已經(jīng)談到了一些，比如動(dòng)態(tài)規(guī)劃。這是一個(gè)NP-hard問(wèn)題所以沒(méi)有一個(gè)簡(jiǎn)單的算法進(jìn)行精確求解?？聪聢D，如果有兩個(gè)人穿馬路，我們來(lái)研究一下如何走才是最優(yōu)的，我們可以將T-S進(jìn)行離散化，離散化之后，我們就可作出一個(gè)狀態(tài)轉(zhuǎn)移方程，而這個(gè)狀態(tài)轉(zhuǎn)移方程我們可以看作每一步的決策只與前邊的兩步有關(guān)系，所以有轉(zhuǎn)移方程。雖然這個(gè)方程在實(shí)際過(guò)程中比較難寫(xiě)但確實(shí)是一種做法。雖然離散化t和s降低了精度，但降低精度也能降低了運(yùn)行時(shí)間。注意，這種方法并不能保證最后的速度比較舒適，它只是大概的一個(gè)決策方法，既到底讓或者不讓。

我們?cè)賮?lái)看一下決策方面的挑戰(zhàn)。第一個(gè)挑戰(zhàn)上面已經(jīng)講過(guò)了，決策問(wèn)題是一個(gè)NP-hard問(wèn)題，不易直接求解，有多種多樣的近似算法。第二個(gè)挑戰(zhàn)是很難用規(guī)則去擬合人的經(jīng)驗(yàn)，包括上述的狀態(tài)轉(zhuǎn)移方程中的cost也很難去表示。而一些解決的辦法是我們可以根據(jù)各種不同的情況去建立一個(gè)數(shù)學(xué)模型。以及用機(jī)器學(xué)習(xí)的方法去做一些決策。比如說(shuō)下圖左邊的例子，我們已經(jīng)打算繞行，但前車(chē)突然起步了，我們?cè)撛趺崔k？我們是該變道還是跟在后邊走還是繼續(xù)繞行？這種情況確實(shí)很靠人的經(jīng)驗(yàn)。再看右邊的圖，我們要左轉(zhuǎn)，但是前邊這輛車(chē)停了很久也沒(méi)有走我們要不要變道呢？作為人類(lèi)司機(jī)也很難判斷。場(chǎng)景的多變而復(fù)雜使得決策面臨著很多挑戰(zhàn)。

橫向規(guī)劃：

剛剛我們已經(jīng)說(shuō)過(guò)了，軌跡規(guī)劃可以拆成橫向和縱向的規(guī)劃。橫向規(guī)劃是一個(gè)方向的規(guī)劃，它可以看成一個(gè)如何打方向盤(pán)的規(guī)劃，它只決定了軌跡的形狀。這個(gè)問(wèn)題通常的解法分兩種，一種是無(wú)車(chē)道的，比如說(shuō)freespace中的規(guī)劃，或者說(shuō)是停車(chē)之類(lèi)的問(wèn)題，它是沒(méi)有車(chē)道線信息或者說(shuō)先驗(yàn)信息的，大多都用OpenSpace路徑生成的方式去做，一般也是低速的。對(duì)于有車(chē)道的情況，雖然有車(chē)道線但是輸出s ->(x,y)這個(gè)函數(shù)難度也是比較大的，常見(jiàn)的做法是是離線生成參考線，之后我們就可以將s ->（x，y）這個(gè)問(wèn)題變?yōu)橐粋€(gè)求解s ->L的問(wèn)題，L是指車(chē)輛在這個(gè)參考線上的橫向偏移量。可以看到下圖右邊這張圖，本來(lái)是沿車(chē)道往前走，但由于有車(chē)輛的遮擋，我們就必須繞行它。即橫向偏移就可以了。

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

有了參考線之后我們就可以把參考線離散化，采一些點(diǎn)出來(lái)，那么就變成了解一個(gè)離參考線偏移距離的一個(gè)問(wèn)題。就變成了s ->L的問(wèn)題，約束是不跨越邊界，避免碰撞，而優(yōu)化的目標(biāo)是要離參考線近，要離障礙物遠(yuǎn)，曲率不大，曲率變化率不大等等。

我們看下這個(gè)例子，那么這個(gè)問(wèn)題就可以看成一個(gè)二次規(guī)劃QP的問(wèn)題。0，-1，-2是我們車(chē)過(guò)去的路徑，0號(hào)點(diǎn)是我們車(chē)當(dāng)前的位置，我們需要解的就是1，2，3，4，5，6這種點(diǎn)的相對(duì)于參考線的橫向偏移x，已知就是x[-2]，x[-1]，x[0]，求解x[1],x[2]等，而約束就是他不能超過(guò)左右邊界包括馬路牙，實(shí)線，障礙物等。優(yōu)化目標(biāo)就是我們要離中心參考線近，方向盤(pán)不能打太多太快。下圖中的公式的形式就是一個(gè)關(guān)于x的二次的形式，所以它們可以去用QP的方法來(lái)解。

之后我們?cè)賮?lái)看看橫向規(guī)劃的挑戰(zhàn)。雖然大部分時(shí)候車(chē)都在一個(gè)有車(chē)道線的地方開(kāi)，但比如像下圖左下部分的路口，這邊是三條直行的車(chē)道，而對(duì)面是四條條直行的車(chē)道，右邊的白車(chē)就不會(huì)按車(chē)道線開(kāi)，會(huì)橫跨車(chē)道線擠你。右上的圖，是有輔路的情況，他是沒(méi)有車(chē)道線的，這種進(jìn)輔路的問(wèn)題更像是一種沒(méi)有參考線的freespace的規(guī)劃，這種都是比較大的挑戰(zhàn)，如果說(shuō)沒(méi)有車(chē)道線或者說(shuō)沒(méi)有人按車(chē)道線開(kāi)，那么這樣的規(guī)劃就會(huì)比較難做一點(diǎn)。

另外一些挑戰(zhàn)就是環(huán)境的問(wèn)題，因?yàn)橹車(chē)沫h(huán)境是瞬息萬(wàn)變的周?chē)沫h(huán)境預(yù)測(cè)也是困難的，我們看下圖，本來(lái)車(chē)輛是要往左變道，而左下角黃色的塊表示摩托車(chē)，這個(gè)摩托車(chē)正在飛速的過(guò)來(lái)，所以車(chē)輛又進(jìn)行了取消變道的決策，也就是生成平滑的曲線回到原來(lái)的道路上去，這樣的情況我如何能保證路徑光滑且在控制上可實(shí)現(xiàn)，也是一件很難的事情。

縱向規(guī)劃：

縱向規(guī)劃其實(shí)是對(duì)于我們?cè)O(shè)定好的路徑上的速度上的規(guī)劃，決定了在這整個(gè)軌跡上的運(yùn)動(dòng)過(guò)程。求解這種優(yōu)化問(wèn)題，有的約束第一就是遵守交規(guī)（信號(hào)燈、限速、停車(chē)讓行等），第二就是避免碰撞。優(yōu)化的目標(biāo)，想都可以想的到就是舒適，舒適就是速度變化率不大，加速度變化率不大，行駛也要盡量快一點(diǎn)，等等。

我們舉一些例子，前邊提到了行人橫穿馬路，行人就是這樣一個(gè)矩形，我們對(duì)應(yīng)了兩種決策，決策完成后我們應(yīng)該怎么辦呢？如果決定要搶行，我們就可以將矩形的約束條件擴(kuò)展到最下部，這樣就是凸問(wèn)題求最優(yōu)解。如果是避讓我們就可從下邊過(guò)，但是如果是行人斜著穿過(guò)馬路呢？那么請(qǐng)大家想一下按s ->t圖是如何表示的呢？答案就是它是斜向上的平行四邊形。

再看一些類(lèi)似的例子,如下圖。比如黃燈，黃燈是一個(gè)特殊的例子，如果決策要沖的話，需要盡快通過(guò)路口，否則的話很容易被逼停在路中間出不去，這種情況我們可以用一個(gè)這樣的s - t圖表示，左邊界是表示黃燈亮的時(shí)刻，大家看這樣的白色矩形有一個(gè)缺角，當(dāng)黃燈亮起的時(shí)候，我們?nèi)绻M快通過(guò)路口，t在增大的過(guò)程中s也要盡快的增大，增大的速率要超過(guò)缺角的斜率。

我們?cè)倏匆恍└呒?jí)的想法，我們?cè)诟?chē)的時(shí)候，假如前車(chē)在勻減速，在s - t，如下圖所示，如果能夠精確描述前車(chē)的運(yùn)動(dòng)的狀態(tài)，那么白色部分會(huì)有各種各樣的形狀，這樣就可以在解優(yōu)化問(wèn)題是解出一個(gè)好的速度曲線。

之后我們?cè)倏匆幌驴v向規(guī)劃的挑戰(zhàn)。如下圖，比如我們綠燈左轉(zhuǎn)，而有一個(gè)電動(dòng)車(chē)，在左側(cè)很快速的橫穿馬路，正常來(lái)說(shuō)那么人應(yīng)該怎么開(kāi)呢？人會(huì)和電動(dòng)車(chē)司機(jī)對(duì)一下眼神，通過(guò)眼神比較氣勢(shì)誰(shuí)猛，然后誰(shuí)就會(huì)主動(dòng)的讓對(duì)方，開(kāi)個(gè)玩笑。但在決策上是這個(gè)事情并不好處理，這是一個(gè)博弈的過(guò)程，并不能一開(kāi)始就決定要無(wú)視他還是讓他，而要在一開(kāi)始對(duì)他有一點(diǎn)反應(yīng)，所以在很多時(shí)候要在激進(jìn)和保守之間掌握一個(gè)平衡點(diǎn)，很多時(shí)候要有不同的參數(shù)和不同的模式去處理這個(gè)事情。

另一方面，感知和預(yù)測(cè)帶來(lái)的困難也會(huì)帶來(lái)縱向規(guī)劃的挑戰(zhàn)，可以看下邊這連續(xù)兩張圖，突然就有人從車(chē)前沖出來(lái)了，這個(gè)我們俗稱叫做鬼探頭，你要檢測(cè)到有盲區(qū)，需要在在檢測(cè)到有盲區(qū)的時(shí)候進(jìn)行減速等等。還有問(wèn)題就是預(yù)測(cè)，我們可以看到左下角的圖，右車(chē)道有一個(gè)面包車(chē)，而前邊有速度慢的一輛自行車(chē)，人一看就知道，這個(gè)面包車(chē)極有可能變道，但這對(duì)預(yù)測(cè)也提出了很大的挑戰(zhàn)，如果沒(méi)有預(yù)測(cè)那么你的反應(yīng)就會(huì)比較慢。右下角的圖，你也無(wú)法判斷這輛左拐的車(chē)要拐到哪條車(chē)道上去。

還有一些極端的情況，需要考慮到橫縱向協(xié)調(diào)配合，比如下圖一個(gè)極限的車(chē)輛插入，你在高速行駛，而旁邊的車(chē)突然就要插進(jìn)來(lái)，如果是人類(lèi)司機(jī)他會(huì)打方向避讓，如果左邊沒(méi)有車(chē)，甚至?xí)兊?，左邊有?chē)，他也會(huì)扭一點(diǎn)方向去避讓。這種就需要橫縱向的配合去一起解決這種極端的情況，從縱向規(guī)劃來(lái)說(shuō)，這種情況，你已經(jīng)不可能保持安全車(chē)距了，你更多的只能說(shuō)保證不撞，然后盡快拉開(kāi)車(chē)距，而不是一腳剎到底。

作者介紹：

梁亞雄，Pony.aiTechlead，清華大學(xué)計(jì)算機(jī)碩士，ACM-ICPC亞洲區(qū)賽金牌得主，此前任職于Hulu，目前在Pony.ai負(fù)責(zé)自動(dòng)駕駛路徑規(guī)劃和控制模塊的技術(shù)研發(fā)。