軌迹規劃(Trajectories Planning)主要指考慮實際臨時或者移動障礙物，考慮速度，動力學限制的情況下，盡量按照規劃路徑進行軌迹規劃。

軌迹規劃的核心就是要解決車輛該怎麼走的問題。軌迹規劃的輸入包括拓撲地圖，障礙物及障礙物的預測軌迹，交通信号燈的狀态，還有定位導航（因為要知道目的地是哪才能規劃路徑）、車輛狀态等其他資訊。而軌迹規劃的輸出就是一個軌迹，軌迹是一個時間到位置的函數，就是在特定的時刻車輛在特定的位置上。

軌迹規劃綜述

軌迹規劃的目标是計算出安全、舒适的軌迹供無人駕駛車輛完成預定的行駛任務。安全意味着車輛在行駛過程中與障礙物保持适當的距離，避免碰撞；舒适意味着給乘客提供舒适的乘坐體驗，比如避免過急的加減速度，在彎道時适當減速避免過大的向心加速度等等；最後，完成行駛任務指規劃出的軌迹要完成給定的行駛任務，不能因為過于保守的駕駛導緻不可接受的行駛時間。

無人駕駛的位形與狀态

我們這裡對軌迹規劃問題作正式的定義。首先，我們介紹兩個機器人領域的概念：位形（Configuration）是在所研究的規劃問題中，能夠唯一性的表達機器人狀态的最小一組變量。變量的數量稱為位形的次元。這裡需要注意的是，位形空間的次元，即使對于同一個機器人來講，所研究的問題不同，次元也是不同的。比如，對一個人形機器人來講，如果規劃問題是在三維空間中移動，位形需要由參照點的變換矩陣，關節的伸展角度組成；如果規劃問題是作物體的操作（Manipulation Planning），則在前面問題位形空間的基礎上，還要增加機器人手指關節的伸展角度等。

對于無人駕駛車輛來講，最簡單的位形描述需要使用3個變量：車輛某個參照點的坐标(x, y)，以及車輛的朝向θ來表達車輛的構型，這也是很多參考文獻中，對于非和諧車輛系統的位形表達方式。對于我們專門為有人乘用的無人駕駛車輛作軌迹規劃的問題來講，更好的位形組成在上面的3個變量基礎上加入車輛的即時轉向曲率κ：如果車輛的導向輪保持一定的角度，車輛會做圓周運動。這個圓周運動的半徑就是即時轉向曲率 κ。加入κ，有助于控制子產品獲得更準确的控制回報，設計更為精細平穩的控制器。

軌迹規劃的定義

軌迹規劃的正式定義，計算一個以時間t為參數的函數S，對于定義域内（[0, t_max]）的每一個t，都有滿足條件的狀态 s：滿足目标車輛的運動學限制，碰撞限制，以及車輛的實體極限。

軌迹規劃問題的難點

軌迹規劃是一個複雜的問題，首先，規劃上加入了速度與時間的資訊，增加了規劃的次元。其次，由于車輛是非和諧系統，具有特殊的運動學限制條件。舉個例子，車輛不能獨立的橫向移動，需要縱向移動的同時才能獲得橫向偏移。躲避障礙物，特别是高速動态障礙物，也是一個比較困難的問題。對于搭載乘客的無人駕駛車輛來說，非常重要的一個要求（在本人看來可能是最重要的）是舒适性，規劃出的軌迹必須做到平滑，将影響乘客舒适度的因素，比如加速度，向心加速度等等，保持在能夠容忍的範圍。

另外，對于無人駕 駛車輛來講，為了處理多變的行車環境，軌迹規劃算法需要以短周期高頻率運作，對算法的計算效率也提出了要求。

軌迹規劃政策

在 Apollo 平台中，為了解決規劃問題，尤其是從降低計算的複雜性上，我們采取了對軌迹的路徑與速度規劃分離的方式。即先确定路徑，也就是軌迹的幾何形狀，然後在已知路徑的基礎上，計算速度配置設定。使用這種政策，我們将一個高次元的軌迹規劃問題，轉換成了兩個順序計算的低次元規劃問題：路徑規劃，借助中間變量路徑的累計長度 s，先求解 s 映射到幾何形狀(x, y, θ, κ)的路徑函數；速度規劃，再求解時間 t，映射到中間變量 s,與 v，a 的速度函數。

路徑規劃的目标

前面提到 Apollo 軌迹規劃的政策，我們先看路徑規劃在這種政策下需要解決的問題：

1. 路徑需要有足夠的靈活度，用來對複雜城市環境中的障礙物進行避讓。
2. 路徑的幾何特性需要平順變化，這樣車輛在沿路徑行駛時才能夠保證舒适性。
3. 路徑的規劃需要遵守車輛的運動學限制條件，比如，路徑的曲率、曲率變化率需要限制在符合目标車輛屬性的範圍之内。
4. 由于我們在 Apollo 平台中采用了路徑/速度分離的政策，在路徑規劃時也需要考慮到目前車輛的運動狀态而适當的調整車輛的幾何形狀。比如在做較大的橫向運動的時候，低速和高速下，我們需要不同的幾何形狀來保證橫向運動的舒适性。

路徑規劃方法

在路徑規劃中，我們借助于弗萊納（frenet）坐标系，将目前車輛的運動狀态(x, y, θ, κ, v, a)做分解。使用弗萊納坐标系的前提是具有一條光滑的指引線。一般情況下，我們可以将指引線了解為道路中心線，車輛在沒有障礙物情況下的理想運動路徑。我們這裡的光滑的指引線是指指引線的幾何屬性可以到曲率級别的光滑（曲率可導）。指引線的光滑性至關重要，因為其直接決定了在弗萊納坐标系下求得軌迹的平順性。在 Apollo 平台上，我們實作了多種平滑指引線的計算方法，在今天的分享中就不展開介紹了，有興趣的同學請查閱相關代碼（或者我們再進行一次以指引線為主題的分享。

在給定一條光滑指引線上，我們按照車輛位置，将其投影到指引線上，以投射點為基礎，将地圖坐标系下目前車輛的運動狀态(x, y, θ, κ, v, a)進行分解，獲得沿着指引線方向的位置，速度，加速度，以及相對于指引線運動的位置， “速度”， “加速度”。這裡打引号的原因是橫向的“速度”、“加速度”并非通常意義上位移對時間的一階/二階導數，而是橫向位移對縱向位移的一階/二階導數，它們描述了幾何形狀的變化趨勢。

在弗萊納坐标系下作軌迹規劃的優點

在弗萊納坐标系下做運動規劃的好處在于借助于指引線做運動分解，我們将高次元的規劃問題，轉換成了多個低次元的規劃問題，極大的降低了規劃的難度。另外一個好處在于友善了解場景，無論道路在地圖坐标系下的形狀與否，我們都能夠很好的了解道路上物體的運動關系。相較直接使用地圖坐标系下的坐标做分析，使用弗萊納坐标，能夠直覺的展現道路上不同物體的運動關系。

路徑規劃-分段加加速度優化算法

在弗萊納坐标系下的函數l(s)，表達了以指引線為參考，相對于指引線的幾何形狀；通過轉換到地圖坐标系下，就表達了在地圖坐标系的路徑。是以，本質上，路徑規劃問題在弗萊納坐标系下，就是一個計算函數l(s)的過程。

為了解決這個問題，這裡介紹由我提出的分段加加速度優化算法（Piecewise Jerk Path Optimization Algorithm）。我們先來看一下算法的流程：

第一步，我們将環境中的靜态障礙物從地圖坐标系映射到依據指引線的弗萊納坐标系下（紅色方框）。

第二步，沿指引線的s軸方向，使用Δs将s方向等距離離散化。根據障礙物的資訊、道路的邊界，計算出每一個離散點所對應的可行區域(l_mini,l_maxi )。為了友善描述，我們将無人駕駛車簡化成一個點。為了避免碰撞，在計算可行區域的時候可以留出适當的緩沖區間。

第三步，我們介紹優化算法使用的優化變量。對于每一個離散點i，我們将該點的橫向偏移 l_i，橫向偏移相對于s的一階和二階導數 l_i^′ 和 l_i″作為優化變量。l_i′ 和 l_i″可以近似了解為橫向運動的“速度”與“加速度”。他們的大小決定了車輛靠近與偏離指引線的趨勢。我們假設每兩個離散點之間，有一個常三階導數項l_(i→i+1)‴連接配接着相鄰的離散點， 這個“常”三階導數項可以由相鄰的二階導數通過差分的方式計算得到。在優化的疊代過程中，l(i→i+1)^‴ 會随着相鄰點l*i^″ 與l_(i+1)^″ 的變化而變化，但l_(i→i+1)^‴在兩個離散點之間會保持不變。*

這也是我們算法的得名，分段的加加速度算法。

第四步，設定優化目标。首先，對于路徑來講，在沒有環境因素影響的情況下，我們希望它盡可能的貼近指引線。對于一階、二階與三階導數，我們希望它們盡可能的小, 這樣可以控制橫向運動的向心加速度，保證路徑的舒适性。

算法完整描述

下面，我們回顧一下這個算法，算法的輸入由四個部分組成：

1. 光滑的道路指引線，用于在弗萊納坐标系下的運動分解。
2. 目前無人駕駛車輛的運動狀态，包括位置、朝向、即時轉向、速度與加速度。車輛運動狀态會依照光滑指引線進行分解。
3. 環境中靜态障礙物的資訊，投影到指引線上。
4. 車輛自身的屬性，運動學模型等，用于計算優化過程中合理的變量限制條件。

   

   算法的變量每一個離散點的橫向位移，橫向位移對指引線縱向長度 s 的一階、二階導數，對于有 n 個輸入點的問題，優化變量有 3∗n 個。對于限制條件，首先橫向位移li需要在相對應的安全區間之内(l_mini,l_maxi )。對于 l_i^′, 〖l_i〗^″, l(i→i+1)^‴的取值範圍，需要根據目前車輛移動速度, 車輛的動力學模型計算，這涉及到具體的車輛模型，這裡我們就不展開介紹了。

   一個非常關鍵的限制條件是路徑的連續性。我們假設相鄰兩點之間有一個常三階導數量 l_(i→i+1)‴來連接配接，我們需要确定經過這個常三階導數量l*(i→i+1)‴，相鄰兩個點的l, l^′, l″的數值能夠吻合。舉個例子，兩個點之間就好比可以拉伸的繩子，l*(i→i+1)^‴決定了可以拉伸的程度，這兩個式限制決定了在繩子拉伸的過程中保證繩子不被拉斷。

算法的優化目标，是減小橫向位移的零階，一階，二階和三階導數，使路徑盡可能貼近指引線的同時保證舒适性。當然，我們也可以根據需求，選擇性的加入與邊界距離的優化目标，使車輛與障礙物保持适當距離。分段加加速度算法由于其大密度分段的特性，使得能夠表達的路徑具有很高的靈活性，能夠解決城市道路中（特别是國内擁擠的環境J）的路徑規劃問題；并且，由于是以加加速度級别的控制變量來改變路徑的形狀，能夠做到曲率級别的連續性，保證了路徑的平滑舒适。

路徑規劃的目标

下一步，我們來看怎樣為計算出的路徑配置設定速度。對于速度規劃，我們有如下要求：

1. 速度配置設定具有靈活性，能夠避讓複雜、擁擠的城市環境中的衆多移動障礙物。
2. 速度配置設定需要遵守車輛運動學的限制條件， 比如最大速度、加速度、加加速度等，確定規劃出的軌迹車輛實際可以執行。
3. 規劃的速度配置設定需要遵守考慮交通法規的限制，在限速範圍内規劃。
4. 規劃的速度配置設定需要完成到達指定位置或者指定速度的任務，并且在保證舒适度的前提下，完成時間盡可能的短。

   

在 Apollo 平台的實作中，我們采用了結合啟發式速度規劃和分段加加速度算法相結合的方式來解決速度規劃問題。啟發式速度規劃提供考慮了動靜态障礙物、路徑幾何資訊、道路資訊、舒适度、目标速度和地點多種因素綜合下的速度規劃粗略估計；分段加加速度算法對啟發是速度規劃提供的粗略分析進行平滑，輸出安全舒适的速度配置設定。

在啟發式速度規劃中，我們采用了一個非常好的分析工具，路徑-時間障礙物圖（Path-Time Obstacle graph）。路徑-時間障礙物圖非常适合在确定了路徑的情況下，考慮動靜态障礙物的規避問題。這個分析工具的核心思想就是将考慮了本車和周圍障礙物幾何形狀和預測軌迹的情況下，将障礙物的軌迹投影到已經确定的本車路徑上。障礙物所代表的含義是在何時(t)與何地(s)本車會與障礙物發生碰撞。

在 Apollo 的實作中，路徑-時間障礙物圖根據時間軸t和沿路徑的累計距離s離散化，形成等距離、等時間的網格狀圖。然後根據我們的需求，将各種因素使用不同的權重，模組化成一個單一熟知的代價函數，每一個網格的節點會賦予一個代價，相鄰網格之間的邊也視為代價的一部分，用來模組化加速度。整個過程可以看成是一個圖搜尋的過程，搜尋出一個代價最優的路徑。搜尋的結果就是一系列的路徑距離與時間的序列，代表何時（t），我們期望無人駕駛車到達何地(s).

啟發式速度規劃的粗略結果隻能提供位置，缺乏控制所需要的更高維資訊，不适合控制子產品直接使用。産生的結果需要進一步的平滑才能滿足舒适度要求。我們進一步做平滑處理的方法使用的是分段常加加速度算法，其主要思想類似于前面介紹的路徑優化算法，在給定趨近啟發式速度規劃結果的情況下，調整啟發式速度規劃，提高速度配置設定的平滑性。

分段加加速度算法

下面我們具體介紹應用在速度平滑上的分段加加速度算法。由于與前面的路徑上使用的算法類似，相似的地方就不再贅述。該算法以時間作為離散參數（可以使用啟發式算法使用的時間間隔），以每個離散點的位置，速度，加速度作為優化變量，并且假設相鄰兩點之間，是一個由相鄰加速度變量差分得到的加加速度。

在優化函數的設定上，與前面算法相似的地方是懲罰加速度與加加速度的以獲得舒适的乘坐體驗。不同的一個優化目标是希望位置變量與啟發式規劃所得到的位置資訊盡可能貼近。啟發式規劃所得到的位置資訊蘊含了根據路徑幾何形狀、道路限速等等所做出的計算，這些計算綁定在相應的位置上，是以優化之後的軌迹需要貼近于相應的啟發式結果才能展現啟發式規劃所做的選擇。

大家可能有的一個問題是，在速度規劃的時候，為什麼分成啟發式的速度規劃與分段加加速度算法結合的形式？為何不直接使用加加速度算法進行求解？主要的問題在于在速度規劃的時候，我們進行離散的次元、時間，也是我們優化目标的一部分。位置與時間同為優化變量，與位置相關的限制條件, 比如路徑曲率，道路限速資訊等等，會影響到達位置的時間；而時間的變化又會引起優化速度的變化，進而導緻位置發生變化。這就導緻了一種變量間耦合變化的情況。啟發式速度規劃使用粗略的方法，近似解決了位置s決定的限制條件與時間t互相耦合的問題，使時間成為了常量。

但是，這樣做也有很明顯的不利影響，啟發式速度規劃的粒度影響了搜尋的品質，在搜尋過程不夠準确，不能反映車輛的動态變化。平滑時單純貼近啟發式速度規劃，速度規劃并非最優。