本文經自動駕駛之心公眾號授權轉載，轉載請聯系出處。

Apollo速度規劃分為：DP（動態規劃）+ QP（軌跡優化）兩部分。

DP給出一個粗解，為了開辟一個凸空間，然后再由osqp/ipopt來做優化平滑。

1. ST坐標系中的S：

1. 路徑規劃中的SL坐標系：S是參考線上的s；
2. 速度規劃中的ST坐標系：S是路徑規劃輸出的路徑上的s；

2. 基于動態規劃的速度規劃

task：SPEED_HEURISTIC_OPTIMIZER

Apollo自動駕駛planning縱向速度規劃之DP詳解 - 知乎 (zhihu.com)

Planning 基于動態規劃的速度規劃 - 知乎 (zhihu.com)

基于處理好的ST圖，通過采樣構建一個二維的cost_table，并對每一格計算cost值，從而通過動態規劃算法，得到一條粗的最優ST軌跡。

一般動態規劃問題包括以下幾個步驟：

• 確定狀態變量
• 設計狀態轉移方程
• 初始化邊界
• 返回結果

1. 狀態變量

狀態變量的定義就是StGraphPoint, 從代碼的定義我們可以看到，每個狀態點保存了：

• 當前柵格的位置信息point_，
• 優化后的速度（optimal_speed_），
• 參考速度cost （reference_cost_），
• 障礙物cost(obstacle_cost_)，
• 空間距離cost （spatial_potential_cost_），
• 當前點總的cost（total_cost_）

2. 狀態轉移方程

總體狀態轉移方程如下：

其中k表示了在最大加減速度約束下，從i時間j位置向i+1時間轉移可達范圍內的所有點

遍歷從（0,0）點開始，向（1，k）逐步遍歷。對于遍歷點C（i+1，j+k）會比較其本身cost和從C（i,j）轉移到C(i+1，j+k)的cost中最小cost作為其新的cost。

3. 基于osqp的二次速度規劃

task：PIECEWISE_JERK_SPEED_OPTIMIZER

apollo PiecewiseJerkSpeedProblem 速度優化 QP 數學推導 - 知乎 (zhihu.com)

Planning 基于二次規劃的速度規劃 - 知乎 (zhihu.com)

1. 優化變量

2. 目標函數

第4項的  ：根據決策制定的速度分配確定的 i 時刻的曲率確定的權重；是曲率關于時間t的函數

但是這里如果曲線優化后，在對應時間的s就不是參考位置的s了，曲率的懲罰也不是優化后對應位置的曲率，所以apollo還提供了一直非線性規劃方法。

4. 基于ipopt的非線性速度規劃

task：PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER

osqp二次規劃存在的問題

二次規劃的曲率規劃的懲罰pi是曲率關于時間t的函數，但實際上路徑的曲率是和s相關的。

二次規劃的速度規劃通過對動態規劃粗糙的st曲線，將關于s的區域懲罰轉化為關于t的區域的懲罰，此時如果平滑后的曲線和原來動態規劃的st曲線相差不大的時候，其懲罰位置是正確的，如果平滑后的曲線和原始的動態規劃的st曲線相差較大，實際的懲罰區間就和設定的不一樣。

比如上圖中應該懲罰的區間為橙色區間，而二次規劃算法實際對于曲率懲罰的區域是在綠色的區域。還有比如地圖的限速約束也是和s相關的，所以這樣的問題就在于，限速或者曲率的函數是關于s的函數，而s又是待求的量(優化量)，就無法對目標函數施加速度約束或者曲率的約束，只能通過動態規劃的st曲線進行轉化，轉化成t的函數再施加約束，但這樣就會導致二次規劃算法的st曲線的速度約束不精確的問題。

ipopt非線性規劃求解

f(x)為代價函數，g(x)為限制條件，f(x)和g(x)都可以為非線性或者是非凸的函數，但必須為二次連續可微（及二階導數是連續的）；

非線性優化器求解主要是考慮路徑曲率對速度規劃的影響，因為曲率約束是一個非線性的函數。

Planning 基于非線性規劃的速度規劃 - 知乎 (zhihu.com)

1. 速度點集的osqp優化平滑
2. 道路曲率的osqp優化平滑
3. 道路限速的osqp優化平滑

等間隔的時間采樣，s，s一階導數，s二階導數，以及s的兩組松弛變量，用于避免求解失敗

額外加入了橫向加速度和松弛變量的優化。

橫向加速度和曲率有關，由于曲率是關于s的關系式，這里還要對該曲率進行進一步的平滑 ，因為對于非線性規劃求解器，無論是目標函數還是約束都是要求函數能夠二階可導：

曲率之前是怎么來的呢？

上節課講到了，規劃路徑是通過分段多項式的二次規劃算法求得的，每一段的s和l都是保持的三階導為定值的一個多項式的曲線關系，它可以保證sl是二階可導，但是求得s的曲率實際上并不是二階可導的，所以這里需要對曲率曲線重新做一個擬合，生成s關于曲率二階可導的一個關系式。這里apollo同樣采用分段多項式的方法獲得s關于曲率kappa的關系式。首先對s進行采樣，然后求解s關于曲率的分段多項式的關系式。

限速函數既不連續也不可導，所以要對限速曲線進行平滑；

對于速度規劃問題，我們如何計算一個初始解：

分段多項式二次求解作為非線性規的初值

原文鏈接：https://mp.weixin.qq.com/s/GJO8_DwyJHI2rQ0j05dGYw