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

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

• 決策

• 橫向規(guī)劃

• 縱向規(guī)劃

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

軌跡規(guī)劃的核心就是要解決車輛該怎么走的問題。比如我們知道了附近有行人、騎自行車的人以及前方的卡車，如果我們現(xiàn)在將要左轉(zhuǎn)，該怎么辦？這就是軌跡規(guī)劃該解決的問題。

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

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

這樣一個(gè)優(yōu)化問題在數(shù)學(xué)上的性質(zhì)是什么呢？我們關(guān)心一個(gè)優(yōu)化問題首先看這個(gè)優(yōu)化問題是不是凸的，因?yàn)橥沟膯栴}比較好解一些。什么是凸的？我們可以簡(jiǎn)單描述對(duì)于一個(gè)問題它的兩個(gè)可行解，其線性組合也應(yīng)該是可行的，而且不比這兩個(gè)解都要差。那么軌跡規(guī)劃是不是一個(gè)凸優(yōu)化問題呢，并不是,它是非凸的。為什么呢？我們?cè)趯?duì)t ->(x,y)問題進(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)合起來就是t ->(x,y)。那么橫向規(guī)劃是凸的嗎？如下圖，圖上沒有時(shí)間信息，這就是橫向規(guī)劃。兩條黑線代表不同的變道軌跡都是可行解，然后我們看其線性組合也就是中間的紅線，但明顯紅線的軌跡是不能用的，是很差的，他變道太慢，騎線行駛的距離太長(zhǎng)了，所以橫向規(guī)劃是非凸的。

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

決策：

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

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

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

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

橫向規(guī)劃：

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

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

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

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

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

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

縱向規(guī)劃：

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

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

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

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

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

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

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

作者介紹：

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