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

軌跡規(guī)劃的概念

自動駕駛軌跡規(guī)劃的核心就是要解決車輛該怎么走的問題。一輛自動駕駛車輛處在周圍有行人、騎自行車的人以及前方有卡車的環(huán)境下，現(xiàn)在它需要左轉，該怎么做？這就是軌跡規(guī)劃該解決的問題。



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

非凸優(yōu)化問題

軌跡規(guī)劃本質上來說是一個優(yōu)化問題。談到優(yōu)化問題，我們往往需要知道優(yōu)化的約束和優(yōu)化的目標。

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

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

這樣一個優(yōu)化問題在數(shù)學上的性質是什么呢？數(shù)學上，解決一個優(yōu)化問題，首先看這個優(yōu)化問題是不是凸的，因為凸的問題比較好解一些。

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

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

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



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

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

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

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

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

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

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



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

而一旦決策確定，那么路徑規(guī)劃就可轉換為凸的問題，求解就會相對來說比較容易，也會使解更加穩(wěn)定。因為一個單純的數(shù)值優(yōu)化問題的求解，很難保證每一幀的解都是相對穩(wěn)定的，甚至可能出現(xiàn)第一幀要讓，而第二幀卻要沖的問題，決策可以提前規(guī)避這種不穩(wěn)定。

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



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

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

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

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



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

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

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

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

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

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



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

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

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

橫向規(guī)劃的解法

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

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

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



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



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



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



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

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

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

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

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



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



4.縱向規(guī)劃

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

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

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

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



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



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

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



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



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

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

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



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

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

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

除此之外，感知和預測帶來的困難也會使縱向規(guī)劃面臨挑戰(zhàn)。

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



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



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

這類處理就需要橫縱向規(guī)劃的配合，共同解決極端情況。比如從縱向規(guī)劃來說，當時已經無法保持安全車距，規(guī)劃需要做到的是保證不相撞，并盡快拉開車距，而不是一腳剎到底。