After finding a path in QP-Spline-Path, we convert all obstacles on the path and ADV (autonomous driving vehicle) itself into a ST graph, which represents the stations changes over time along the path. Our speed optimization task is to find a path on ST graph that is collision-free and comfort.
We use spline to define the path, and find the best path leveraging Quadratic programming with a set of conditions. QP formulation is defined as
$$
\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 Objective function
### 1.1 Get spline segments
Split the path into n segments. each segment trajectory is defined by a polynomial.
### 1.2 Define function for each spline segment
Each segment *i* has accumulated distance $d_i$ along reference line. And the trajactory for the segment is defined as a polynomial of degree five by default
Let's assume the the first point is ($s_0$, $l_0$), and $l_0$ should be on the planned path $f_i(s)$, $f'_i(s)$, and $f_i(s)''$ (position, velocity, acceleration). we convert those constraint into QP equality constraints
$$
A_{eq}x = b_{eq}
$$
### 2.2 Monotone constraint
The path need to be monotone, e.g., vehicle can only drive forward.
### 2.3 Joint smoothness constraints
This constraint is to make the spline joint smooth. Let's assume two segment $seg_k$ and $seg_{k+1}$ are connected, and the accumulated s of segment $seg_k$ is $s_k$. The we can get the constraint euqation as
Evenly sample m points along the path, and check the obstacle boundary at those points. convert the constraint into QP inequality constraints
$$
Ax \leq b
$$
We first find lower boundary $l_{lb,j}$ at those points ($s_j$, $l_j$) and $j\in[0, m]$ based on road width and surrounding obstacles. The calculate the inequality constraints as