軌跡規划(Trajectories Planning)主要指考慮實際臨時或者移動障礙物，考慮速度，動力學約束的情況下，盡量按照規划路徑進行軌跡規划。
軌跡規划的核心就是要解決車輛該怎么走的問題。軌跡規划的輸入包括拓撲地圖，障礙物及障礙物的預測軌跡，交通信號燈的狀態，還有定位導航（因為要知道目的地是哪才能規划路徑）、車輛狀態等其他信息。而軌跡規划的輸出就是一個軌跡，軌跡是一個時間到位置的函數，就是在特定的時刻車輛在特定的位置上。
在本篇分享中，我們有請高級架構師 Zhang Yajia 為大家分享在百度 Apollo 平台上，軌跡規划算法的技術方案。
軌跡規划綜述
軌跡規划的目標是計算出安全、舒適的軌跡供無人駕駛車輛完成預定的行駛任務。安全意味着車輛在行駛過程中與障礙物保持適當的距離，避免碰撞；舒適意味着給乘客提供舒適的乘坐體驗，比如避免過急的加減速度，在彎道時適當減速避免過大的向心加速度等等；最后，完成行駛任務指規划出的軌跡要完成給定的行駛任務，不能因為過於保守的駕駛導致不可接受的行駛時間。
無人駕駛的位形與狀態
我們這里對軌跡規划問題作正式的定義。首先，我們介紹兩個機器人領域的概念：位形（Configuration）是在所研究的規划問題中，能夠唯一性的表達機器人狀態的最小一組變量。變量的數量稱為位形的維度。這里需要注意的是，位形空間的維度，即使對於同一個機器人來講，所研究的問題不同，維度也是不同的。比如，對一個人形機器人來講，如果規划問題是在三維空間中移動，位形需要由參照點的變換矩陣，關節的伸展角度組成；如果規划問題是作物體的操作（Manipulation Planning），則在前面問題位形空間的基礎上，還要增加機器人手指關節的伸展角度等。
對於無人駕駛車輛來講，最簡單的位形描述需要使用3個變量：車輛某個參照點的坐標(x, y)，以及車輛的朝向θ來表達車輛的構型，這也是很多參考文獻中，對於非和諧車輛系統的位形表達方式。對於我們專門為有人乘用的無人駕駛車輛作軌跡規划的問題來講，更好的位形組成在上面的3個變量基礎上加入車輛的即時轉向曲率κ：如果車輛的導向輪保持一定的角度，車輛會做圓周運動。這個圓周運動的半徑就是即時轉向曲率 κ。加入κ，有助於控制模塊獲得更准確的控制反饋，設計更為精細平穩的控制器。
軌跡規划的定義
軌跡規划的正式定義，計算一個以時間t為參數的函數S，對於定義域內（[0, t_max]）的每一個t，都有滿足條件的狀態 s：滿足目標車輛的運動學約束，碰撞約束，以及車輛的物理極限。
軌跡規划問題的難點
軌跡規划是一個復雜的問題，首先，規划上加入了速度與時間的信息，增加了規划的維度。其次，由於車輛是非和諧系統，具有特殊的運動學約束條件。舉個例子，車輛不能獨立的橫向移動，需要縱向移動的同時才能獲得橫向偏移。躲避障礙物，特別是高速動態障礙物，也是一個比較困難的問題。對於搭載乘客的無人駕駛車輛來說，非常重要的一個要求（在本人看來可能是最重要的）是舒適性，規划出的軌跡必須做到平滑，將影響乘客舒適度的因素，比如加速度，向心加速度等等，保持在能夠容忍的范圍。
另外，對於無人駕 駛車輛來講，為了處理多變的行車環境，軌跡規划算法需要以短周期高頻率運行，對算法的計算效率也提出了要求。
Apollo 中軌跡規划策略
在 Apollo 平台中，為了解決規划問題，尤其是從降低計算的復雜性上，我們采取了對軌跡的路徑與速度規划分離的方式。即先確定路徑，也就是軌跡的幾何形狀，然后在已知路徑的基礎上，計算速度分配。使用這種策略，我們將一個高維度的軌跡規划問題，轉換成了兩個順序計算的低維度規划問題：路徑規划，借助中間變量路徑的累計長度 s，先求解 s 映射到幾何形狀(x, y, θ, κ)的路徑函數；速度規划，再求解時間 t，映射到中間變量 s,與 v，a 的速度函數。
路徑規划的目標
前面提到 Apollo 軌跡規划的策略，我們先看路徑規划在這種策略下需要解決的問題：
-
路徑需要有足夠的靈活度，用來對復雜城市環境中的障礙物進行避讓。
-
路徑的幾何特性需要平順變化，這樣車輛在沿路徑行駛時才能夠保證舒適性。
-
路徑的規划需要遵守車輛的運動學限制條件，比如，路徑的曲率、曲率變化率需要限制在符合目標車輛屬性的范圍之內。
-
由於我們在 Apollo 平台中采用了路徑/速度分離的策略，在路徑規划時也需要考慮到當前車輛的運動狀態而適當的調整車輛的幾何形狀。比如在做較大的橫向運動的時候，低速和高速下，我們需要不同的幾何形狀來保證橫向運動的舒適性。
路徑規划方法
在路徑規划中，我們借助於弗萊納坐標系，將當前車輛的運動狀態(x, y, θ, κ, v, a)做分解。使用弗萊納坐標系的前提是具有一條光滑的指引線。一般情況下，我們可以將指引線理解為道路中心線，車輛在沒有障礙物情況下的理想運動路徑。我們這里的光滑的指引線是指指引線的幾何屬性可以到曲率級別的光滑（曲率可導）。指引線的光滑性至關重要，因為其直接決定了在弗萊納坐標系下求得軌跡的平順性。在 Apollo 平台上，我們實現了多種平滑指引線的計算方法，在今天的分享中就不展開介紹了，有興趣的同學請查閱相關代碼（或者我們再進行一次以指引線為主題的分享。
在給定一條光滑指引線上，我們按照車輛位置，將其投影到指引線上，以投射點為基礎，將地圖坐標系下當前車輛的運動狀態(x, y, θ, κ, v, a)進行分解，獲得沿着指引線方向的位置，速度，加速度，以及相對於指引線運動的位置， “速度”， “加速度”。這里打引號的原因是橫向的“速度”、“加速度”並非通常意義上位移對時間的一階/二階導數，而是橫向位移對縱向位移的一階/二階導數，它們描述了幾何形狀的變化趨勢。
在弗萊納坐標系下作軌跡規划的優點
在弗萊納坐標系下做運動規划的好處在於借助於指引線做運動分解，我們將高維度的規划問題，轉換成了多個低維度的規划問題，極大的降低了規划的難度。另外一個好處在於方便理解場景，無論道路在地圖坐標系下的形狀與否，我們都能夠很好的理解道路上物體的運動關系。相較直接使用地圖坐標系下的坐標做分析，使用弗萊納坐標，能夠直觀的體現道路上不同物體的運動關系。
路徑規划-分段加加速度優化算法
在弗萊納坐標系下的函數l(s)，表達了以指引線為參考，相對於指引線的幾何形狀；通過轉換到地圖坐標系下，就表達了在地圖坐標系的路徑。所以，本質上，路徑規划問題在弗萊納坐標系下，就是一個計算函數l(s)的過程。
為了解決這個問題，這里介紹由我提出的分段加加速度優化算法（Piecewise Jerk Path Optimization Algorithm）。我們先來看一下算法的流程：
第一步，我們將環境中的靜態障礙物從地圖坐標系映射到依據指引線的弗萊納坐標系下（紅色方框）。
第二步，沿指引線的s軸方向，使用Δs將s方向等距離離散化。根據障礙物的信息、道路的邊界，計算出每一個離散點所對應的可行區域(l_min^i,l_max^i )。為了方便描述，我們將無人駕駛車簡化成一個點。為了避免碰撞，在計算可行區域的時候可以留出適當的緩沖區間。
第三步，我們介紹優化算法使用的優化變量。對於每一個離散點i，我們將該點的橫向偏移 l_i，橫向偏移相對於s的一階和二階導數 l_i^′ 和 l_i^″作為優化變量。l_i^′ 和 l_i^″可以近似理解為橫向運動的“速度”與“加速度”。他們的大小決定了車輛靠近與偏離指引線的趨勢。我們假設每兩個離散點之間，有一個常三階導數項l_(i→i+1)^‴連接着相鄰的離散點， 這個“常”三階導數項可以由相鄰的二階導數通過差分的方式計算得到。在優化的迭代過程中，l(i→i+1)^‴ 會隨着相鄰點li^″ 與l_(i+1)^″ 的變化而變化，但l_(i→i+1)^‴在兩個離散點之間會保持不變。
這也是我們算法的得名，分段的加加速度算法。
第四步，設置優化目標。首先，對於路徑來講，在沒有環境因素影響的情況下，我們希望它盡可能的貼近指引線。對於一階、二階與三階導數，我們希望它們盡可能的小, 這樣可以控制橫向運動的向心加速度，保證路徑的舒適性。
算法完整描述
下面，我們回顧一下這個算法，算法的輸入由四個部分組成：
-
光滑的道路指引線，用於在弗萊納坐標系下的運動分解。
-
當前無人駕駛車輛的運動狀態，包括位置、朝向、即時轉向、速度與加速度。車輛運動狀態會依照光滑指引線進行分解。
-
環境中靜態障礙物的信息，投影到指引線上。
-
車輛自身的屬性，運動學模型等，用於計算優化過程中合理的變量限制條件。
算法的變量每一個離散點的橫向位移，橫向位移對指引線縱向長度 s 的一階、二階導數，對於有 n 個輸入點的問題，優化變量有 3∗n 個。對於限制條件，首先橫向位移li需要在相對應的安全區間之內(l_min^i,l_max^i )。對於 l_i^′, 〖l_i〗^″, l(i→i+1)^‴的取值范圍，需要根據當前車輛移動速度, 車輛的動力學模型計算，這涉及到具體的車輛模型，這里我們就不展開介紹了。
一個非常關鍵的約束條件是路徑的連續性。我們假設相鄰兩點之間有一個常三階導數量 l_(i→i+1)^‴來連接，我們需要確定經過這個常三階導數量l(i→i+1)^‴，相鄰兩個點的l, l^′, l″的數值能夠吻合。舉個例子，兩個點之間就好比可以拉伸的繩子，l(i→i+1)^‴決定了可以拉伸的程度，這兩個式約束決定了在繩子拉伸的過程中保證繩子不被拉斷。
算法的優化目標，是減小橫向位移的零階，一階，二階和三階導數，使路徑盡可能貼近指引線的同時保證舒適性。當然，我們也可以根據需求，選擇性的加入與邊界距離的優化目標，使車輛與障礙物保持適當距離。分段加加速度算法由於其大密度分段的特性，使得能夠表達的路徑具有很高的靈活性，能夠解決城市道路中（特別是國內擁擠的環境J）的路徑規划問題；並且，由於是以加加速度級別的控制變量來改變路徑的形狀，能夠做到曲率級別的連續性，保證了路徑的平滑舒適。
路徑規划的目標
下一步，我們來看怎樣為計算出的路徑分配速度。對於速度規划，我們有如下要求：
-
速度分配具有靈活性，能夠避讓復雜、擁擠的城市環境中的眾多移動障礙物。
-
速度分配需要遵守車輛運動學的限制條件， 比如最大速度、加速度、加加速度等，確保規划出的軌跡車輛實際可以執行。
-
規划的速度分配需要遵守考慮交通法規的限制，在限速范圍內規划。
-
規划的速度分配需要完成到達指定位置或者指定速度的任務，並且在保證舒適度的前提下，完成時間盡可能的短。
路徑規划的策略
在 Apollo 平台的實現中，我們采用了結合啟發式速度規划和分段加加速度算法相結合的方式來解決速度規划問題。啟發式速度規划提供考慮了動靜態障礙物、路徑幾何信息、道路信息、舒適度、目標速度和地點多種因素綜合下的速度規划粗略估計；分段加加速度算法對啟發是速度規划提供的粗略分析進行平滑，輸出安全舒適的速度分配。
在啟發式速度規划中，我們采用了一個非常好的分析工具，路徑-時間障礙物圖（Path-Time Obstacle graph）。路徑-時間障礙物圖非常適合在確定了路徑的情況下，考慮動靜態障礙物的規避問題。這個分析工具的核心思想就是將考慮了本車和周圍障礙物幾何形狀和預測軌跡的情況下，將障礙物的軌跡投影到已經確定的本車路徑上。障礙物所代表的含義是在何時(t)與何地(s)本車會與障礙物發生碰撞。
啟發式速度規划
在 Apollo 的實現中，路徑-時間障礙物圖根據時間軸t和沿路徑的累計距離s離散化，形成等距離、等時間的網格狀圖。然后根據我們的需求，將各種因素使用不同的權重，建模成一個單一熟知的代價函數，每一個網格的節點會賦予一個代價，相鄰網格之間的邊也視為代價的一部分，用來建模加速度。整個過程可以看成是一個圖搜索的過程，搜索出一個代價最優的路徑。搜索的結果就是一系列的路徑距離與時間的序列，代表何時（t），我們期望無人駕駛車到達何地(s).
分段加加速度平滑算法
啟發式速度規划的粗略結果只能提供位置，缺乏控制所需要的更高維信息，不適合控制模塊直接使用。產生的結果需要進一步的平滑才能滿足舒適度要求。我們進一步做平滑處理的方法使用的是分段常加加速度算法，其主要思想類似於前面介紹的路徑優化算法，在給定趨近啟發式速度規划結果的情況下，調整啟發式速度規划，提高速度分配的平滑性。
分段加加速度算法
下面我們具體介紹應用在速度平滑上的分段加加速度算法。由於與前面的路徑上使用的算法類似，相似的地方就不再贅述。該算法以時間作為離散參數（可以使用啟發式算法使用的時間間隔），以每個離散點的位置，速度，加速度作為優化變量，並且假設相鄰兩點之間，是一個由相鄰加速度變量差分得到的加加速度。
在優化函數的設置上，與前面算法相似的地方是懲罰加速度與加加速度的以獲得舒適的乘坐體驗。不同的一個優化目標是希望位置變量與啟發式規划所得到的位置信息盡可能貼近。啟發式規划所得到的位置信息蘊含了根據路徑幾何形狀、道路限速等等所做出的計算，這些計算綁定在相應的位置上，所以優化之后的軌跡需要貼近於相應的啟發式結果才能體現啟發式規划所做的選擇。
算法評價與改進
大家可能有的一個問題是，在速度規划的時候，為什么分成啟發式的速度規划與分段加加速度算法結合的形式？為何不直接使用加加速度算法進行求解？主要的問題在於在速度規划的時候，我們進行離散的維度、時間，也是我們優化目標的一部分。位置與時間同為優化變量，與位置相關的限制條件, 比如路徑曲率，道路限速信息等等，會影響到達位置的時間；而時間的變化又會引起優化速度的變化，進而導致位置發生變化。這就導致了一種變量間耦合變化的情況。啟發式速度規划使用粗略的方法，近似解決了位置s決定的限制條件與時間t相互耦合的問題，使時間成為了常量。
但是，這樣做也有很明顯的不利影響，啟發式速度規划的粒度影響了搜索的質量，在搜索過程不夠准確，不能反映車輛的動態變化。平滑時單純貼近啟發式速度規划，速度規划並非最優。