二次规划（QP）样条路径优化

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

apollo 代码代码文档，转载从github
apollo文档
侵删

二次规划（QP）样条路径优化

二次规划（QP）+样条插值

1. 目标函数

1.1 获得路径长度

路径定义在station-lateral座标系中。s的变化区间为从车辆当前位置点到默认路径的长度。

1.2 获得样条段

将路径划分为n段，每段路径用一个多项式来表示。

1.3 定义样条段函数

每个样条段 i 都有沿着参考线的累加距离$d_i$。每段的路径默认用5介多项式表示。
$l = f_i(s) = a_{i0} + a_{i1} \cdot s + a_{i2} \cdot s^2 + a_{i3} \cdot s^3 + a_{i4} \cdot s^4 + a_{i5} \cdot s^5 (0 \leq s \leq d_{i})$

1.4 定义每个样条段优化目标函数

$cost = \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)$

1.5 将开销（cost）函数转换为QP公式

QP公式:

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

下面是将开销（cost）函数转换为QP公式的例子：

$f_i(s) ＝
\begin{vmatrix} 1 & s & s^2 & s^3 & s^4 & s^5 \end{vmatrix}
\cdot
\begin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} \end{vmatrix} $

且
$f_i’(s) =
\begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix}
\cdot
\begin{vmatrix} a_{i0} \ a_{i1} \ a_{i2} \ a_{i3} \ a_{i4} \ a_{i5} \end{vmatrix} $

且
$f_i'(s)^2 = \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

然后得到，
$\int\limits_{0}^{d_i} f_i'(s)^2 ds ＝ \int\limits_{0}^{d_i} \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} ds$

从聚合函数中提取出常量得到，

$\int\limits_{0}^{d_i} f'(s)^2 ds ＝ \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$
$＝\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 0 & 0 &0&0&0&0\\ 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4\\ 0 & 2s & 4s^2 & 6s^3 & 8s^4 & 10s^5\\ 0 & 3s^2 & 6s^3 & 9s^4 & 12s^5&15s^6 \\ 0 & 4s^3 & 8s^4 &12s^5 &16s^6&20s^7 \\ 0 & 5s^4 & 10s^5 & 15s^6 & 20s^7 & 25s^8 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

最后得到，

$\int\limits_{0}^{d_i} f'_i(s)^2 ds =\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 & 0 & 0 & 0 &0&0\\ 0 & d_i & d_i^2 & d_i^3 & d_i^4&d_i^5\\ 0& d_i^2 & \frac{4}{3}d_i^3& \frac{6}{4}d_i^4 & \frac{8}{5}d_i^5&\frac{10}{6}d_i^6\\ 0& d_i^3 & \frac{6}{4}d_i^4 & \frac{9}{5}d_i^5 & \frac{12}{6}d_i^6&\frac{15}{7}d_i^7\\ 0& d_i^4 & \frac{8}{5}d_i^5 & \frac{12}{6}d_i^6 & \frac{16}{7}d_i^7&\frac{20}{8}d_i^8\\ 0& d_i^5 & \frac{10}{6}d_i^6 & \frac{15}{7}d_i^7 & \frac{20}{8}d_i^8&\frac{25}{9}d_i^9 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

请注意我们最后得到一个6介的矩阵来表示5介样条插值的衍生开销。
应用同样的推理方法可以得到2介，3介样条插值的衍生开销。

2 约束条件

2.1 初始点约束

假设第一个点为 ($s_0$, $l_0$), ($s_0$, $l'_0$) and ($s_0$, $l''_0$)，其中$l_0$ , $l'_0$ and $l''_0$表示横向的偏移，并且规划路径的起始点的第一，第二个点的衍生开销可以从$f_i(s)$, $f'_i(s)$, $f_i(s)''$计算得到。

将上述约束转换为QP约束等式，使用等式：

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

下面是转换的具体步骤：

$f_i(s_0) = \begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5}\end{vmatrix} = l_0$

且

$f'_i(s_0) = \begin{vmatrix} 0& 1 & 2s_0 & 3s_0^2 & 4s_0^3 &5 s_0^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l'_0$

且
$f''_i(s_0) = \begin{vmatrix} 0&0& 2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l''_0$
其中，i是包含$s_0$的样条段的索引值。

2.2 终点约束

和起始点相同，终点$(s_e, l_e)$ 也应当按照起始点的计算方法生成约束条件。

将起始点和终点组合在一起，得出约束等式为：

$\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 0&1 & 2s_0 & 3s_0^2 & 4s_0^3 & 5s_0^4 \\ 0& 0&2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \\ 1 & s_e & s_e^2 & s_e^3 & s_e^4&s_e^5 \\ 0&1 & 2s_e & 3s_e^2 & 4s_e^3 & 5s_e^4 \\ 0& 0&2 & 3\times2s_e & 4\times3s_e^2 & 5\times4s_e^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = \begin{vmatrix} l_0\\ l'_0\\ l''_0\\ l_e\\ l'_e\\ l''_e\\ \end{vmatrix}$

2.3 平滑节点约束

该约束的目的是使样条的节点更加平滑。假设两个段$seg_k$ 和$seg_{k+1}$互相连接，且$seg_k$的累计值s为$s_k$。计算约束的等式为：

$f_k(s_k) = f_{k+1} (s_0)$

下面是计算的具体步骤：
$\begin{vmatrix} 1 & s_k & s_k^2 & s_k^ 3 & s_k^ 4 & s_k^ 5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \end{vmatrix} = \begin{vmatrix} 1 & s_{0} & s_{0}^2 & s_{0}^3 & s_{0}^4&s_{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 & s_k & s_k^2 & s_k^3 & s_k^4&s_k^5 & -1 & -s_{0} & -s_{0}^2 & -s_{0}^3 & -s_{0}^4&-s_{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$

将$s_0$ = 0代入等式。

同样地，可以为下述等式计算约束等式：

$f'_k(s_k) = f'_{k+1} (s_0) \\ f''_k(s_k) = f''_{k+1} (s_0) \\ f'''_k(s_k) = f'''_{k+1} (s_0)$

2.4 点采样边界约束

在路径上均匀的取样m个点，检查这些点上的障碍物边界。将这些约束转换为QP约束不等式，使用不等式：

$Ax \geq b$

首先基于道路宽度和周围的障碍物找到点 $(s_j, l_j)$的下边界$l_{lb,j}$，且$j\in[0, m]$。计算约束的不等式为：

$\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 1 & s_1 & s_1^2 & s_1^3 & s_1^4&s_1^5 \\ ...&...&...&...&...&... \\ 1 & s_m & s_m^2 & s_m^3 & s_m^4&s_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix}a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq \begin{vmatrix} l_{lb,0}\\ l_{lb,1}\\ ...\\ l_{lb,m}\\ \end{vmatrix}$

同样地，对上边界$l_{ub,j}$，计算约束的不等式为：

$\begin{vmatrix} -1 & -s_0 & -s_0^2 & -s_0^3 & -s_0^4&-s_0^5 \\ -1 & -s_1 & -s_1^2 & -s_1^3 & -s_1^4&-s_1^5 \\ ...&...-&...&...&...&... \\ -1 & -s_m & -s_m^2 & -s_m^3 & -s_m^4&-s_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq -1 \cdot \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ ...\\ l_{ub,m}\\ \end{vmatrix}$