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

編輯整理：孫昊

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

出品社區(qū)：DataFun

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

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

軌跡規(guī)劃的概念

決策

橫向規(guī)劃

縱向規(guī)劃

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

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

軌跡規(guī)劃的輸入包括拓撲地圖，障礙物及障礙物的預(yù)測軌跡，交通信號燈的狀態(tài)，還有定位導(dǎo)航（因為要知道目的地是哪才能規(guī)劃路徑）、車輛狀態(tài)等其他信息。而軌跡規(guī)劃的輸出就是一個軌跡，軌跡是一個時間到位置的函數(shù)，就是在特定的時刻車輛在特定的位置上。在PPT上是t ->（x,y,z），把z標(biāo)灰是因為目前為止我們的車不會飛。

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

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

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

決策：

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

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

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

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

橫向規(guī)劃：

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

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

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

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

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

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

縱向規(guī)劃：

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

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

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

我們再看一些更高級的想法，我們在跟車的時候，假如前車在勻減速，在s - t，如下圖所示，如果能夠精確描述前車的運動的狀態(tài)，那么白色部分會有各種各樣的形狀，這樣就可以在解優(yōu)化問題是解出一個好的速度曲線。

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

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

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

作者介紹：

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