「Pony Tech」是小馬智行Pony.ai推出的深度技術科普專欄，由專業的工程師總結歸納出自動駕駛領域的技術實踐及研究成果。本文主題為自動駕駛中軌跡規劃的探索和挑戰，主要內容包括：

» 軌跡規劃基本介紹

» 為什麼需要決策模組？

» 橫向規劃

» 縱向規劃

1、軌跡規劃基本介紹

軌跡規劃的概念

自動駕駛軌跡規劃的核心就是要解決車輛該怎麼走的問題。一輛自動駕駛車輛處在周圍有行人、騎腳踏車的人以及前方有卡車的環境下，現在它需要左轉，該怎麼做？這就是軌跡規劃該解決的問題。

軌跡規劃的輸入包括拓撲地圖、障礙物及障礙物的預測軌跡、交通訊號燈的狀態，還有定位、導航（因為知道目的地才能規劃路徑）、車輛狀態等其他資訊。而軌跡規劃的輸出則是一個軌跡，軌跡是一個時間到位置的函式，也就是特定的時刻要求車輛在特定的位置上。上圖中軌跡函式是t→(x,y,z)，把z標灰是因為目前為止我們的車還不會飛，所以z是(x, y)的函式。

非凸最佳化問題

軌跡規劃本質上來說是一個最佳化問題。談到最佳化問題，我們往往需要知道最佳化的約束和最佳化的目標。

首先看軌跡規劃的約束，第一個約束就是車輛要遵守交規，這是強制性的約束；第二個約束是要避免碰撞；第三個約束是要使規劃的軌跡在車輛控制上可實現，例如不能出現規劃出一條車根本拐不過來的急彎的軌跡。

而最佳化的目標是使得自動駕駛跟人類司機駕駛相似，具體表現就是乘客乘坐時感到舒適。但“開得像人”也有不同的最佳化目標，比如我們可以駕駛得穩一點也可以開得急一點，像老司機一般。

這樣一個最佳化問題在數學上的性質是什麼呢？數學上，解決一個最佳化問題，首先看這個最佳化問題是不是凸的，因為凸的問題比較好解一些。

什麼是凸最佳化問題？可以簡單描述為，一個問題如果有兩個可行解，則要滿足這兩個可行解的線性組合也是可行的，且不比這兩個可行解都差。

那麼軌跡規劃是不是一個凸最佳化問題呢？可以拆解來看，我們求解軌跡規劃的t→(x,y)問題時，發現其複雜度較高且計算量較大。所以常見的解法是把軌跡規劃分成橫向規劃和縱向規劃。可以把這兩個子問題看成t→(x,y)在空間上的投影，只要二者之一不是凸的，則軌跡規劃問題就不會是凸的。

橫向規劃，即s→(x,y)，決定了軌跡的形狀。縱向規劃是t→s，是指在此形狀上運動的速度狀態，也就是時間與位移的關係。橫向規劃和縱向規劃聯合起來就是t→(x,y)。

那麼橫向規劃是凸最佳化問題嗎？如上圖中的左側圖，沒有顯示時間資訊，這就是橫向規劃。兩條黑線代表不同的變道軌跡都是可行解，然後我們看其線性組合也就是中間的紅線，這條紅線的變道軌跡明顯是不能用的——它變道太慢，騎線行駛距離過長。所以，根據數學定義，橫向規劃是非凸最佳化問題。

再來看看縱向規劃。縱向規劃常以上圖右側的t - s圖表示，t表示時間，s表示我們走過的路程。當一個行人橫穿馬路時，我們可以用白色矩形在t - s圖上表示此過程，左邊界表示行人進入規劃路徑（考慮車寬）的時刻，右邊界表示離開規劃路徑的時刻。橫軸上的上下邊界差可以理解為行人佔用的規劃路徑的寬度。

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

2、為什麼需要決策模組？

決策使軌跡規劃化繁為簡

既然軌跡規劃是非凸最佳化問題，我們需要利用決策模組來解決這個問題。

什麼是自動駕駛的決策模組呢？從數學上來講，決策就是為了限定非凸問題（軌跡規劃）的解空間，將問題轉化為凸的。

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

而一旦決策確定，那麼路徑規劃就可轉換為凸的問題，求解就會相對來說比較容易，也會使解更加穩定。因為一個單純的數值最佳化問題的求解，很難保證每一幀的解都是相對穩定的，甚至可能出現第一幀要讓，而第二幀卻要衝的問題，決策可以提前規避這種不穩定。

我們需要注意，決策是一個NP-hard（非確定性多項式困難）問題。自動駕駛需要決策的場景涉及許多，包括搶行還是讓行，是否要衝黃燈，在哪兩輛車之間變道、併線，是否要主動變道，是從左還是右繞行前方障礙物。只有做了決策才能使軌跡規劃問題變為凸最佳化問題。

處理凸問題可以利用的快速演算法有許多，線性規劃，二次規劃，序列二次規劃……這些都是數值求解最佳化問題的方式。例如在二次規劃中，如果Q正定，二次規劃就是凸的問題，求解它會更加迅速和容易。

縱向規劃決策求解：動態規劃

在縱向規劃決策上，我想介紹一種以動態規劃演算法作為決策的方法。

看看下圖的案例，假設有兩個人正在橫穿馬路，我們來研究一下如何決策才是最優的選擇。

首先，將t - s圖進行離散化，離散化之後，我們可以認為每一步的決策只與前邊的兩步有關係。兩步是因為我們可以近似求出加速度，並能寫出一個狀態轉移方程。

儘管這個方程在實際過程中比較難寫，但確實是一種做法。雖然離散化t和s降低了精度，但降低精度也幫助降低了執行時間。

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

決策面臨的挑戰

第一個挑戰上面已經提到，由於決策問題是一個NP-hard問題，不易直接求解，存在多種多樣的近似演算法。

第二個挑戰是很難用規則去擬合人的經驗，包括上述的狀態轉移方程中的cost也很難去表示。目前解決這個挑戰可用的部分辦法是根據各種不同的情況建立數學模型，以及採用機器學習的方法進行決策。

以上圖左側的場景為例，一輛正在行駛的自動駕駛車輛打算繞行前面白車，但此時前車突然起步了，我們該如何繼續行駛？是變道？還是跟在後邊行駛，又或是繼續繞行？處理這種情況確實依靠人類駕駛的經驗。

再看上圖右側的例子：自動駕駛車準備在前方左轉，但是前車停了很久也沒有啟動（可能前車駕駛員沒反應過來），我們這時該不該變道呢？對這種情況，人類司機有時也很難判斷。由此可見，場景的多變而複雜使得決策面臨很多挑戰。

3、橫向規劃

橫向規劃的解法

前面提到軌跡規劃可以拆成橫向和縱向的規劃，現在我來具體介紹橫向規劃的解法。橫向規劃就是行車方向上的規劃，可以看成是如何打方向盤的規劃，它決定了軌跡的形狀。

這個問題通常的解法分兩種，一種是無車道的場景，比如在freespace（自由空間）中規劃或者泊車之類的場景，車輛一般處在低速行駛狀態，缺乏車道線等先驗資訊。業界大多都用搜索等路徑生成的方式去處理無車道場景。

另一種是有車道的情況。雖然可以參考車道線資訊，但是規劃上想輸出s→(x,y)函式，難度並不小。常見的做法是離線生成參考線，隨後就可以將求解s→(x,y)的問題變為求解s→l的問題，l是指車輛在這個參考線上的橫向偏移量。

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

而參考線的生成，其實類似於開卡丁車時別人教你的過彎的最優路線，它也是一個最佳化問題，當然也要保證安全性和舒適性。方便的是，有了高精地圖輔助後，參考線可以透過離線去進行，所以可以用一些開銷比較大的演算法做到最優。參考線的約束在於其需要在車道線內，並且在控制上可實現。最佳化目標則是參考線需接近車道中心，曲率不能太大，曲率變化率也不大。

確定參考線後，透過把參考線離散化，採一些點出來，那麼橫向規劃問題就轉化為求解一個離參考線偏移距離的一個問題，即轉化成s→l的問題。其約束是車輛行駛不跨越邊界，避免碰撞，而最佳化的目標是要離參考線近，要離障礙物遠，曲率不大，曲率變化率不大等等。

藉助上圖右側的例子，你會發現橫向規劃可看成一個二次規劃（QP）的問題。其中“0”，“-1”，“-2”是自動駕駛車行駛過的路徑，0號點是車當前的位置，現在我們需要解的就是1，2，3，4，5，6這些點相對於參考線的橫向偏移x。換句話說，已知x[-2]，x[-1]，x[0]，求解x[1]，x[2]等。

該函式約束是車輛行駛不能超過左右邊界包括馬路牙，實線，障礙物等。最佳化目標則是車輛要離參考線近，方向盤不能打太多太快，保證乘坐的舒適。上圖中的公式其實是一個關於x的二次的形式，所以可以使用二次規劃的方法來解決。

橫向規劃的挑戰

雖然大部分時候車都行駛在有車道線的馬路上，但也會面臨特殊的挑戰。例如下圖裡左側顯示的路口，我們的車行駛的方向上有三條直行車道，但過路口後變成四條直行車道，並且對得很不齊。現實中，右側的行駛的車（白車）往往不依據車道線行駛，可能會橫跨車道線擠佔自動駕駛車輛所在車道的空間。

而下圖右上角則展示了沒有車道線的主輔路場景。在這種主輔路之間切換，就像解決一種沒有參考線的freespace的規劃，挑戰也不小。總的來說，要想解決沒有車道線或者說普通車輛不按車道線行駛的路徑規劃問題，難度都不小。

另外的挑戰就是環境的問題，因為車外行駛環境瞬息萬變，要對周圍進行環境預測也很困難。以下圖為例，我們的自動駕駛車準備往左變道，而左下角橙色塊代表的摩托車正飛速地向前行駛，於是我們的車輛迅速給出取消變道的決策，生成平滑的曲線回到原來行駛的道路上。因此，面對這類的情況，軌跡規劃模組需要保證規劃的路徑光滑且在控制上可實現。

4、縱向規劃

縱向規劃的定義和場景

縱向規劃本質是對車輛在設定好的路徑上的速度規劃，決定了車輛在整個軌跡上的運動過程。

求解這類最佳化問題，第一個約束是遵守交規（訊號燈、限速、停車讓行等），第二個約束是避免碰撞。而縱向規劃的最佳化的目標是乘坐舒適，也意味著車輛的速度變化率不大，加速度變化率不大，行駛速度也要儘量快一點（限速內）等。

前邊我提到了行人橫穿馬路的場景，在t - s圖中，行人的運動過程可以轉化一個矩形，最終給出了兩種車輛的對應決策——加速超過行人或減速讓行。那麼決策之後該怎麼做呢？

如果決定要搶行，我們可以將矩形的約束條件擴充套件到最下部，便能轉化為凸問題求最優解。如果採取避讓，車輛的路線則從t - s圖中的矩形下邊穿過。

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

黃燈也是我們要應對的場景之一。黃燈即將到來，如果決策要衝，那麼車輛須儘快透過路口，否則很容易被逼停在路中間出不去。

這種情況我們也可以用一樣的t - s圖表示（上圖），左邊界是表示黃燈亮的時刻，這個白色矩形存在一個缺角。當黃燈亮起的時候，車輛如果要儘快透過路口，那麼隨著t在增大的過程中，s也要迅速增大，並且增大的速率要超過缺角的斜率。

再看一些更有趣的場景案例（上圖）。當自動駕駛車跟車時，假設所跟的前車在減速，如果能夠精確預測前車的運動的狀態，那麼展現在t - s圖中的白色部分會出現各種各樣的形狀，這樣解最佳化問題就能解出一條好的速度曲線。

縱向規劃的挑戰

縱向規劃會面臨什麼挑戰呢？

首先存在博弈的挑戰。以上圖為例，自動駕駛車前方的左轉綠燈亮起並準備左轉，這時，一輛電動車突然出現在左前方，準備快速橫穿馬路。

這種情況，人類駕駛員會怎麼開呢？人類司機會和電動車司機迅速對一下眼神，透過眼神比較氣勢誰猛，另一方就會主動地讓對方。當然這有開玩笑的成分。

但在決策上這個場景並不好處理，它是一個博弈的過程，自動駕駛車不能一開始就決定無視還是讓步，所以在很多時候要在激進和保守之間掌握一個平衡點，這需要不同的引數和不同的模式去處理不同場景。

除此之外，感知和預測帶來的困難也會使縱向規劃面臨挑戰。

可以看下圖中右上方連續兩張相似的圖，在第二張圖裡你會發現有人突然從車前衝出來，俗稱叫做“鬼探頭”，也就是盲區。對於這種情況，感知需要提前檢測到盲區，車輛進行減速，規避可能的安全隱患。

預測給規劃帶來的挑戰出現在左下角（上圖）的場景裡。此時，自動駕駛車的右前方行駛著一輛麵包車，麵包車前邊有一輛速度很慢的腳踏車，一般人類司機會主動預測麵包車極有可能向左變道。但這類場景對預測模組提出了很大的挑戰，如果缺乏這類預測，自動駕駛車輛的後續應對同樣挑戰不小。

自動駕駛還有一些極端的情況，需要考慮到橫縱向協調配合。上圖是一個非常極限的車輛加塞案例：自動駕駛車正在高速行駛時，右側的一輛慢車突然加塞，一般人類司機會選擇打方向避讓，如果當時左車道沒有車，甚至會向左變道，如果左車道有車，他也會扭一點方向進行避讓。