軌跡規劃是自動駕駛技術中最重要的部分，之前的文章有一些這方面的介紹，但是並不全面和系統：
然而這些文章不夠全面和系統，不免產生盲人摸象的感覺，這裏結合百度Apollo和pony.ai較爲系統和全面的介紹次軌跡規劃。
1. 概念
軌跡規劃的輸入包括拓撲地圖，障礙物及障礙物的預測軌跡，交通信號燈的狀態，還有定位導航（因爲要知道目的地是哪才能規劃路徑）、車輛狀態等其他信息。而軌跡規劃的輸出就是一個軌跡，軌跡是一個時間到位置的函數，就是在特定的時刻車輛在特定的位置上。計算出安全、舒適的軌跡供無人駕駛車輛完成預定的行駛任務。安全意味着車輛在行駛過程中與障礙物保持適當的距離，避免碰撞；舒適意味着給乘客提供舒適的乘坐體驗，比如避免過急的加減速度，在彎道時適當減速避免過大的向心加速度等等；最後，完成行駛任務指規劃出的軌跡要完成給定的行駛任務，不能因爲過於保守的駕駛導致不可接受的行駛時間。
2. 約束
軌跡規劃本質上來說是一個優化問題。談到優化問題，我們需要知道優化的約束和優化的目標。首先看約束，第一個約束就是車輛要遵守交規，這是強制性的約束；第二個約束是要避免碰撞；第三個約束是要使規劃的軌跡在控制上可實現，不能出現規劃了一個急彎的軌跡，車根本拐不過來的狀況。而優化的目標總的來說就是要跟人開的像，具體表現就是讓人乘坐起來感到舒適。但像人也有不同的優化目標，比如我們可以開的穩一點也可以開的急一點，像老司機一點。
這樣一個優化問題在數學上的性質是什麼呢？我們關心一個優化問題首先看這個優化問題是不是凸的，因爲凸的問題比較好解一些。那麼軌跡規劃是不是一個凸優化問題呢，並不是,它是非凸的。爲什麼呢？
我們在對t ->(x,y)問題進行求解時，複雜度比較高計算量比較大。所以通常的做法是把他分成橫向規劃和縱向規劃。橫向規劃，是s ->（x,y）,就是決定軌跡的形狀是什麼。縱向規劃是t ->s,也就是指在此形狀上運動的速度狀態，也就是時間與位移的關係。橫向規劃和縱向規劃聯合起來就是t ->(x,y)。那麼橫向規劃是凸的嗎？如下圖，圖上沒有時間信息，這就是橫向規劃。兩條黑線代表不同的變道軌跡都是可行解，然後我們看其線性組合也就是中間的紅線，但明顯紅線的軌跡是不能用的，是很差的，他變道太慢，騎線行駛的距離太長了，所以橫向規劃是非凸的。
我們再來看縱向規劃，我們經常用t - s圖表示。t表示時間，s表示我們走過的路程。當一個人橫穿馬路時我們可以用白色矩形來在t - s圖上表示這個過程，左邊界表示人進入我們規劃路徑的時刻，右邊界表示離開規劃路徑的時刻。橫向上，上下邊界差可以理解爲人在佔用我們規劃路徑的寬度。對於這樣一個縱向規劃，有兩種選擇，一種是我們要讓人，既下邊的綠色路線，一種是車輛加速超過，既上邊的綠色路線。但它的線性組合可能是這條紅線，明顯這已經撞上了，所以這並不是一個凸的問題。
3. 規劃
正是因爲軌跡規劃是非凸的，所以我們需要用決策的模塊來解決。什麼是決策呢？從數學上來講，決策就是限定非凸問題的解空間，將問題轉化爲凸的。我們可以看下圖的例子，如果兩個人在穿馬路的話，我們大體上可以有四種不同的決策，既讓兩個人，超兩個人，讓一超一和超一讓一。而一旦決策確定，那麼其實就可轉換爲凸的問題，求解就會相對來說比較容易。決策也會使解更加穩定，因爲一個單純的數值優化問題的求解，會很難保證每一幀的解是相對穩定的，不會出現第一幀要讓，而第二幀卻要衝的問題。但需要注意決策的問題是一個NP-hard 問題。有一些需要決策的場景，比如搶行還是讓行，是否要衝黃燈，在哪兩輛車之間變道、併線，是否要主動變道，是從左還是右繞行前方障礙物，只有做了決策才能使問題變爲凸優化問題，便於求解。對於凸問題，有很多比較快速的算法進行計算了。線性規劃，二次規劃，序列二次規劃，都是數值求解優化問題的方式。我們可以看一下二次規劃中，如果Q正定，二次規劃就是凸的問題，有很快的方式求解它。
下邊就是看如何把自動駕駛 中的問題轉化爲優化問題。
首先是縱向規劃的決策，決策的方法我們已經談到了一些，比如動態規劃。這是一個NP-hard問題所以沒有一個簡單的算法進行精確求解。看下圖，如果有兩個人穿馬路，我們來研究一下如何走纔是最優的，我們可以將T-S進行離散化，離散化之後，我們就可作出一個狀態轉移方程，而這個狀態轉移方程我們可以看作每一步的決策只與前邊的兩步有關係，所以有轉移方程。雖然這個方程在實際過程中比較難寫但確實是一種做法。雖然離散化t和s降低了精度，但降低精度也能降低了運行時間。注意，這種方法並不能保證最後的速度比較舒適，它只是大概的一個決策方法，既到底讓或者不讓。
我們再來看一下決策方面的挑戰。第一個挑戰上面已經講過了，決策問題是一個NP-hard問題，不易直接求解，有多種多樣的近似算法。第二個挑戰是很難用規則去擬合人的經驗，包括上述的狀態轉移方程中的cost也很難去表示。而一些解決的辦法是我們可以根據各種不同的情況去建立一個數學模型。以及用機器學習的方法去做一些決策。比如說下圖左邊的例子，我們已經打算繞行，但前車突然起步了，我們該怎麼辦？我們是該變道還是跟在後邊走還是繼續繞行？這種情況確實很靠人的經驗。再看右邊的圖，我們要左轉，但是前邊這輛車停了很久也沒有走我們要不要變道呢？作爲人類司機也很難判斷。場景的多變而複雜使得決策面臨着很多挑戰。
4. 橫向規劃：
剛剛我們已經說過了，軌跡規劃可以拆成橫向和縱向的規劃。橫向規劃是一個方向的規劃，它可以看成一個如何打方向盤的規劃，它只決定了軌跡的形狀。這個問題通常的解法分兩種，一種是無車道的，比如說freespace中的規劃，或者說是停車之類的問題，它是沒有車道線信息或者說先驗信息的，大多都用OpenSpace路徑生成的方式去做，一般也是低速的。對於有車道的情況，雖然有車道線但是輸出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的二次的形式，所以它們可以去用QP的方法來解。
之後我們再來看看橫向規劃的挑戰。雖然大部分時候車都在一個有車道線的地方開，但比如像下圖左下部分的路口，這邊是三條直行的車道，而對面是四條條直行的車道，右邊的白車就不會按車道線開，會橫跨車道線擠你。右上的圖，是有輔路的情況，他是沒有車道線的，這種進輔路的問題更像是一種沒有參考線的freespace的規劃，這種都是比較大的挑戰，如果說沒有車道線或者說沒有人按車道線開，那麼這樣的規劃就會比較難做一點。
另外一些挑戰就是環境的問題，因爲周圍的環境是瞬息萬變的周圍的環境預測也是困難的，我們看下圖，本來車輛是要往左變道，而左下角黃色的塊表示摩托車，這個摩托車正在飛速的過來，所以車輛又進行了取消變道的決策，也就是生成平滑的曲線回到原來的道路上去，這樣的情況我如何能保證路徑光滑且在控制上可實現，也是一件很難的事情。
5. 縱向規劃：
縱向規劃其實是對於我們設定好的路徑上的速度上的規劃，決定了在這整個軌跡上的運動過程。求解這種優化問題，有的約束第一就是遵守交規（信號燈、限速、停車讓行等），第二就是避免碰撞。優化的目標，想都可以想的到就是舒適，舒適就是速度變化率不大，加速度變化率不大，行駛也要儘量快一點，等等。
我們舉一些例子，前邊提到了行人橫穿馬路，行人就是這樣一個矩形，我們對應了兩種決策，決策完成後我們應該怎麼辦呢？如果決定要搶行，我們就可以將矩形的約束條件擴展到最下部，這樣就是凸問題求最優解。如果是避讓我們就可從下邊過，但是如果是行人斜着穿過馬路呢？那麼請大家想一下按s ->t圖是如何表示的呢？答案就是它是斜向上的平行四邊形。
再看一些類似的例子,如下圖。比如黃燈，黃燈是一個特殊的例子，如果決策要衝的話，需要儘快通過路口，否則的話很容易被逼停在路中間出不去，這種情況我們可以用一個這樣的s - t圖表示，左邊界是表示黃燈亮的時刻，大家看這樣的白色矩形有一個缺角，當黃燈亮起的時候，我們如果要儘快通過路口，t在增大的過程中s也要儘快的增大，增大的速率要超過缺角的斜率。
我們再看一些更高級的想法，我們在跟車的時候，假如前車在勻減速，在s - t，如下圖所示，如果能夠精確描述前車的運動的狀態，那麼白色部分會有各種各樣的形狀，這樣就可以在解優化問題是解出一個好的速度曲線。
之後我們再看一下縱向規劃的挑戰。如下圖，比如我們綠燈左轉，而有一個電動車，在左側很快速的橫穿馬路，正常來說那麼人應該怎麼開呢？人會和電動車司機對一下眼神，通過眼神比較氣勢誰猛，然後誰就會主動的讓對方，開個玩笑。但在決策上是這個事情並不好處理，這是一個博弈的過程，並不能一開始就決定要無視他還是讓他，而要在一開始對他有一點反應，所以在很多時候要在激進和保守之間掌握一個平衡點，很多時候要有不同的參數和不同的模式去處理這個事情。
另一方面，感知和預測帶來的困難也會帶來縱向規劃的挑戰，可以看下邊這連續兩張圖，突然就有人從車前衝出來了，這個我們俗稱叫做鬼探頭，你要檢測到有盲區，需要在在檢測到有盲區的時候進行減速等等。還有問題就是預測，我們可以看到左下角的圖，右車道有一個麪包車，而前邊有速度慢的一輛自行車，人一看就知道，這個麪包車極有可能變道，但這對預測也提出了很大的挑戰，如果沒有預測那麼你的反應就會比較慢。右下角的圖，你也無法判斷這輛左拐的車要拐到哪條車道上去。
還有一些極端的情況，需要考慮到橫縱向協調配合，比如下圖一個極限的車輛插入，你在高速行駛，而旁邊的車突然就要插進來，如果是人類司機他會打方向避讓，如果左邊沒有車，甚至會變道，左邊有車，他也會扭一點方向去避讓。這種就需要橫縱向的配合去一起解決這種極端的情況，從縱向規劃來說，這種情況，你已經不可能保持安全車距了，你更多的只能說保證不撞，然後儘快拉開車距，而不是一腳剎到底。
