qp_spline_st_speed_optimizer_cn

原文鏈接：https://github.com/ApolloAuto/apollo/tree/master/docs/specs

apollo 代碼代碼文檔，轉載從github
apollo文檔
侵刪

@[toc]# 二次規劃ST速度優化

1 定義

從二次規劃樣條路徑中選取一條路徑後，Apollo將路線上的所有障礙物和自動駕駛車輛（ADV）展現在一個時間-路徑圖上（path-time ST），該路徑圖表示了路徑上的站點變化。速度優化的任務是在ST圖上找到一條合理的，無障礙的路徑。

Apollo使用多個樣條來表示速度參數，在ST圖上表示爲一系列的ST點。Apollo會對二次規劃的結果做再次的平衡以獲得最佳的速度參數。QP問題的標準類型定義爲：

$minimize \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. LB \leq x \leq UB \\ A_{eq}x = b_{eq} \\ Ax \leq b$

2 目標函數

2.1 獲取樣條段

將路ST速度參數分爲 n 段，每段路徑用一個多項式來表示。

2.2 定義樣條段函數

每個樣條段 i 都有沿着參考線的累加距離$d_i$。每段的路徑默認用5介多項式表示。多項式介數可以通過配置參數進行調整。

$s = f_i(t) = a_{0i} + a_{1i} \cdot t + a_{2i} \cdot t^2 + a_{3i} \cdot t^3 + a_{4i} \cdot t^4 + a_{5i} \cdot t^5$

2.3 定義樣條段優化函數

Apollo首先定義$cost_1$以使路徑更加平滑：

$cost_1 = \sum_{i=1}^{n} \Big( w_1 \cdot \int\limits_{0}^{d_i} (f_i')^2(s) ds + w_2 \cdot \int\limits_{0}^{d_i} (f_i'')^2(s) ds + w_3 \cdot \int\limits_{0}^{d_i} (f_i^{\prime\prime\prime})^2(s) ds \Big)$

然後，Apollo定義$cost_2$表示最後的S-T路徑和S-T巡航路徑（有速度限制且m個點）的差值：

$cost_2 = \sum_{i=1}^{n}\sum_{j=1}^{m}\Big(f_i(t_j)- s_j\Big)^2$

同樣地，Apollo定義了$cost_3$表示第一個S-T路徑和隨後的S-T路徑（o個點）的差值：

$cost_3 = \sum_{i=1}^{n}\sum_{j=1}^{o}\Big(f_i(t_j)- s_j\Big)^2$

最後得出的目標函數爲：

$cost = cost_1 + cost_2 + cost_3$

3 約束條件

3.1 初始點約束

假設第一個點是($t0$, $s0$)，且$s0$在路徑$f_i(t)$, $f'i(t)$, 和$f_i(t)''$上（位置、速率、加速度）。Apollo將這些約束轉換爲QP約束的等式爲：

$A_{eq}x = b_{eq}$

3.2 單調約束

路線必須是單調的，比如車輛只能往前開。

在路徑上採樣 m 個點，對每一個 $j$和$j-1$ 的點對，且($j\in[1,...,m]$)，如果兩個點都處在同一個樣條$k$上，則：

$\begin{vmatrix} 1 & t_j & t_j^2 & t_j^3 & t_j^4&t_j^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_k \\ b_k \\ c_k \\ d_k \\ e_k \\ f_k \end{vmatrix} > \begin{vmatrix} 1 & t_{j-1} & t_{j-1}^2 & t_{j-1}^3 & t_{j-1}^4&t_{j-1}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k} \\ b_{k} \\ c_{k} \\ d_{k} \\ e_{k} \\ f_{k} \end{vmatrix}$

如兩個點分別處在不同的樣條$k$和$l$上，則：

$\begin{vmatrix} 1 & t_j & t_j^2 & t_j^3 & t_j^4&t_j^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_k \\ b_k \\ c_k \\ d_k \\ e_k \\ f_k \end{vmatrix} > \begin{vmatrix} 1 & t_{j-1} & t_{j-1}^2 & t_{j-1}^3 & t_{j-1}^4&t_{j-1}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{l} \\ b_{l} \\ c_{l} \\ d_{l} \\ e_{l} \\ f_{l} \end{vmatrix}$

3.3 平滑節點約束

該約束的目的是使樣條的節點更加平滑。假設兩個段$seg_k$ 和$seg_{k+1}$互相連接，且$seg_k$的累計值 s 爲$s_k$。計算約束的等式爲：

$f_k(t_k) = f_{k+1} (t_0)$

即：

$\begin{vmatrix} 1 & t_k & t_k^2 & t_k^3 & t_k^4&t_k^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \end{vmatrix} = \begin{vmatrix} 1 & t_{0} & t_{0}^2 & t_{0}^3 & t_{0}^4&t_{0}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \end{vmatrix}$

然後，

$\begin{vmatrix} 1 & t_k & t_k^2 & t_k^3 & t_k^4&t_k^5 & -1 & -t_{0} & -t_{0}^2 & -t_{0}^3 & -t_{0}^4&-t_{0}^5\\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \\ a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \end{vmatrix} = 0$

等式中得出的結果爲$t_0$ = 0。

同樣地，爲下述等式計算約束等式：

$f'_k(t_k) = f'_{k+1} (t_0) \\ f''_k(t_k) = f''_{k+1} (t_0) \\ f'''_k(t_k) = f'''_{k+1} (t_0)$

3.4 點採樣邊界約束

在路徑上均勻的取樣 m 個點，檢查這些點上的障礙物邊界。將這些約束轉換爲QP約束不等式，使用不等式：

$Ax \leq b$

首先基於道路寬度和周圍的障礙物找到點 $(s_j, l_j)$的下邊界$l_{lb,j}$，且$j\in[0, m]$。計算約束的不等式爲：

$\begin{vmatrix} 1 & t_0 & t_0^2 & t_0^3 & t_0^4&t_0^5 \\ 1 & t_1 & t_1^2 & t_1^3 & t_1^4&t_1^5 \\ ...&...&...&...&...&... \\ 1 & t_m & t_m^2 & t_m^3 & t_m^4&t_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \leq \begin{vmatrix} l_{lb,0}\\ l_{lb,1}\\ ...\\ l_{lb,m}\\ \end{vmatrix}$

同樣地，對上邊界$l_{ub,j}$，計算約束的不等式爲：

$\begin{vmatrix} 1 & t_0 & t_0^2 & t_0^3 & t_0^4&t_0^5 \\ 1 & t_1 & t_1^2 & t_1^3 & t_1^4&t_1^5 \\ ...&...&...&...&...&... \\ 1 & t_m & t_m^2 & t_m^3 & t_m^4&t_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \leq -1 \cdot \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ ...\\ l_{ub,m}\\ \end{vmatrix}$

3.5 速度邊界優化

Apollo同樣需要建立速度限制邊界。

在st曲線上取樣 m 個點，爲每個點$j$獲取速度限制的上邊界和下邊界，例如$v{ub,j}$ 和 $v{lb,j}$，約束定義爲：

$f'(t_j) \geq v_{lb,j}$

即：

$\begin{vmatrix} 0& 1 & t_0 & t_0^2 & t_0^3 & t_0^4 \\ 0 & 1 & t_1 & t_1^2 & t_1^3 & t_1^4 \\ ...&...&...&...&...&... \\ 0& 1 & t_m & t_m^2 & t_m^3 & t_m^4 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \geq \begin{vmatrix} v_{lb,0}\\ v_{lb,1}\\ ...\\ v_{lb,m}\\ \end{vmatrix}$

且，

$f'(t_j) \leq v_{ub,j}$

即：

$\begin{vmatrix} 0& 1 & t_0 & t_0^2 & t_0^3 & t_0^4 \\ 0 & 1 & t_1 & t_1^2 & t_1^3 & t_1^4 \\ ...&...&...&...&...&... \\ 0 &1 & t_m & t_m^2 & t_m^3 & t_m^4 \\ \end{vmatrix} \cdot \begin{vmatrix} a_i \\ b_i \\ c_i \\ d_i \\ e_i \\ f_i \end{vmatrix} \leq \begin{vmatrix} v_{ub,0}\\ v_{ub,1}\\ ...\\ v_{ub,m}\\ \end{vmatrix}$