同步操作将从 ApolloAuto/apollo 强制同步,此操作会覆盖自 Fork 仓库以来所做的任何修改,且无法恢复!!!
确定后同步将在后台操作,完成时将刷新页面,请耐心等待。
Tip: to read the equations in the document, you are recommended to use Chrome with a plugin or copy the latex equation to an online editor
Quadratic programming + Spline interpolation
Path is defined in station-lateral coordination system. The s range from vehicle's current position to default planning path length.
Split the path into n segments. each segment trajectory is defined by a polynomial.
Each segment i has accumulated distance $d_i$ along reference line. The trajectory for the segment is defined as a polynomial of degree five by default.
$$
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})
$$
$$
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)
$$
QP formulation:
$$
\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}
$$
Below is the example for converting the cost function into the QP formulation.
$$
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}
$$
And
$$
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}
$$
And
$$
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}
$$
then we have,
$$
\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
$$
extract the const outside the integration, we have,
$$
\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}
$$
Finally, we have
$$
\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}
$$
Please notice that we got a 6 x 6 matrix to represent the derivative cost of 5th order spline.
Similar deduction can also be used to calculate the cost of second and third order derivatives.
Assume that the first point is ($s_0$, $l_0$), ($s_0$, $l'_0$) and ($s_0$, $l''_0$), where $l_0$ , $l'_0$ and $l''_0$ is the lateral offset and its first and second derivatives on the init point of planned path, and are calculated from $f_i(s)$, $f'_i(s)$, and $f_i(s)''$.
Convert those constraints into QP equality constraints, using:
$$
A_{eq}x = b_{eq}
$$
Below are the steps of conversion.
$$
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
$$
And
$$
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
$$
And
$$
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
$$
where i is the index of the segment that contains the $s_0$.
Similar to the init point, the end point $(s_e, l_e)$ is known and should produce the same constraint as described in the init point calculations.
Combine the init point and end point, and show the equality constraint as:
$$
\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}
$$
This constraint is designed to smooth the spline joint. Assume two segments $seg_k$ and $seg_{k+1}$ are connected, and the accumulated s of segment $seg_k$ is $s_k$. Calculate the constraint equation as:
$$
f_k(s_k) = f_{k+1} (s_0)
$$
Below are the steps of the calculation.
$$
\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}
$$
Then
$$
\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
$$
Use $s_0$ = 0 in the equation.
Similarly calculate the equality constraints for:
$$
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)
$$
Evenly sample m points along the path, and check the obstacle boundary at those points. Convert the constraint into QP inequality constraints, using:
$$
Ax \geq b
$$
First find the lower boundary $l_{lb,j}$ at those points $(s_j, l_j)$ and $j\in[0, m]$ based on the road width and surrounding obstacles. Calculate the inequality constraints as:
$$
\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}
$$
Similarly, for the upper boundary $l_{ub,j}$, calculate the inequality constraints as:
$$
\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}
$$
此处可能存在不合适展示的内容,页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容,可点击提交进行申诉,我们将尽快为您处理。