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

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

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

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

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

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

task：SPEED_HEURISTIC_OPTIMIZER

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

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

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

一般動(dòng)態(tài)規(guī)劃問(wèn)題包括以下幾個(gè)步驟：

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

1. 狀態(tài)變量

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

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

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

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

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

遍歷從（0,0）點(diǎn)開(kāi)始，向（1，k）逐步遍歷。對(duì)于遍歷點(diǎn)C（i+1，j+k）會(huì)比較其本身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)

1. 優(yōu)化變量

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

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

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

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

task：PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER

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

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

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

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

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

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

非線性優(yōu)化器求解主要是考慮路徑曲率對(duì)速度規(guī)劃的影響，因?yàn)榍始s束是一個(gè)非線性的函數(shù)。

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

1. 速度點(diǎn)集的osqp優(yōu)化平滑
2. 道路曲率的osqp優(yōu)化平滑
3. 道路限速的osqp優(yōu)化平滑

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

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

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

曲率之前是怎么來(lái)的呢？

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

限速函數(shù)既不連續(xù)也不可導(dǎo)，所以要對(duì)限速曲線進(jìn)行平滑；

對(duì)于速度規(guī)劃問(wèn)題，我們?nèi)绾斡?jì)算一個(gè)初始解：

分段多項(xiàng)式二次求解作為非線性規(guī)的初值

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