Apollo速度規(guī)劃都有什么？詳解阿波羅動態(tài)規(guī)劃&軌跡優(yōu)化！

作者：無印燁 2023-10-11 10:13:45

智能汽車新聞

基于處理好的ST圖，通過采樣構(gòu)建一個二維的cost_table，并對每一格計算cost值，從而通過動態(tài)規(guī)劃算法，得到一條粗的最優(yōu)ST軌跡。

本文經(jīng)自動駕駛之心公眾號授權(quán)轉(zhuǎn)載，轉(zhuǎn)載請聯(lián)系出處。

Apollo速度規(guī)劃分為：DP（動態(tài)規(guī)劃）+ QP（軌跡優(yōu)化）兩部分。

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

1. ST坐標(biāo)系中的S：

路徑規(guī)劃中的SL坐標(biāo)系：S是參考線上的s；
速度規(guī)劃中的ST坐標(biāo)系：S是路徑規(guī)劃輸出的路徑上的s；

2. 基于動態(tài)規(guī)劃的速度規(guī)劃

task：SPEED_HEURISTIC_OPTIMIZER

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

Planning 基于動態(tài)規(guī)劃的速度規(guī)劃 - 知乎 (zhihu.com)

基于處理好的ST圖，通過采樣構(gòu)建一個二維的cost_table，并對每一格計算cost值，從而通過動態(tài)規(guī)劃算法，得到一條粗的最優(yōu)ST軌跡。

一般動態(tài)規(guī)劃問題包括以下幾個步驟：

確定狀態(tài)變量
設(shè)計狀態(tài)轉(zhuǎn)移方程
初始化邊界
返回結(jié)果

狀態(tài)變量

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

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

狀態(tài)轉(zhuǎn)移方程

總體狀態(tài)轉(zhuǎn)移方程如下：

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

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

3. 基于osqp的二次速度規(guī)劃

task：PIECEWISE_JERK_SPEED_OPTIMIZER

apollo PiecewiseJerkSpeedProblem 速度優(yōu)化 QP 數(shù)學(xué)推導(dǎo) - 知乎 (zhihu.com)

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

優(yōu)化變量

目標(biāo)函數(shù)

第4項的：根據(jù)決策制定的速度分配確定的 i 時刻的曲率確定的權(quán)重；是曲率關(guān)于時間t的函數(shù)

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

4. 基于ipopt的非線性速度規(guī)劃

task：PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER

osqp二次規(guī)劃存在的問題

二次規(guī)劃的曲率規(guī)劃的懲罰pi是曲率關(guān)于時間t的函數(shù)，但實際上路徑的曲率是和s相關(guān)的。

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

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

ipopt非線性規(guī)劃求解

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

非線性優(yōu)化器求解主要是考慮路徑曲率對速度規(guī)劃的影響，因為曲率約束是一個非線性的函數(shù)。

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

速度點集的osqp優(yōu)化平滑
道路曲率的osqp優(yōu)化平滑
道路限速的osqp優(yōu)化平滑

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

額外加入了橫向加速度和松弛變量的優(yōu)化。

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

曲率之前是怎么來的呢？

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