自动驾驶 QP 路径规划基本原理

Given a reference line and road boundaries, we use QP (Quadratic Programming) to generate a path that satisfy some predefined constraints.

1. A standard form of QP

$$\arg \min \frac{1}{2}{X}^{T}HX+f^{T}x$$
$$s.t.A_{ieq}x\le b_{ieq}$$
$$A_{ep}x\le b_{ep}$$

2. Problem definition

In Path optimization, the object $x$ is $l$, $\dot{l}$ and $\ddot{l}$ under Frenet coordinate, which is denoted by:
$$[l_0,l_1,...,l_n,\dot{l_0},...,\dot{l_n},\ddot{l_0},...,\ddot{l_n}{]}^T$$

3. Cost function

We define a cost function consisting of four terms:

1. lateral offset term: $x_i^2$
2. lateral velocity term: ${\dot{x}}_i^2$
3. lateral acceleration term: ${\ddot{x}}_i^2$
4. lateral jerk term: ${\stackrel{\cdots }{x}}_i^2$

The cost function in discrete form is:
$$\mathrm{J}(\mathrm{x})=w_x\sum _{i=0}^{n-1}{x}_i^2+w_{\dot{x}}\sum _{i=0}^{n-1}{\dot{x}}_i^2+w_{\ddot{x}}\sum _{i=0}^{n-1}{\ddot{x}}_i^2+w_{\stackrel{\cdots }{x}}\sum _{i=0}^{n-2}{\stackrel{\cdots }{x}}_i^2$$

Rewrite discrete cost function form to matrix form.

(1) lateral offset term

The first term: $w_x{\sum }_{i=0}^{n-1}{x}_i^2$

$${\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_n\\ \dot{x_0}\\ ⋮\\ \dot{x_n}\\ \ddot{x_0}\\ ⋮\\ \ddot{x_n}\end{array}\right]}^T\left[\begin{array}{cccccc}2w_x& & & & & \\ & \ddots & & & & \\ & & 2w_x& & & \\ & & & 0& & \\ & & & & \ddots & \\ & & & & & 0\end{array}\right]\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_n\\ \dot{x_0}\\ ⋮\\ \dot{x_n}\\ \ddot{x_0}\\ ⋮\\ \ddot{x_n}\end{array}\right]$$

(2) lateral velocity term

The second term: $w_{\dot{x}}{\sum }_{i=0}^{n-1}{\dot{x}}_i^2$

$${\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_n\\ \dot{x_0}\\ ⋮\\ \dot{x_n}\\ \ddot{x_0}\\ ⋮\\ \ddot{x_n}\end{array}\right]}^T\left[\begin{array}{ccccccccc}0& & & & & & & & \\ & \ddots & & & & & & & \\ & & 0& & & & & & \\ & & & 2w_{\dot{x}}& & & & & \\ & & & & \ddots & & & & \\ & & & & & 2w_{\dot{x}}& & & \\ & & & & & & 0& & \\ & & & & & & & \ddots & \\ & & & & & & & & 0\end{array}\right]\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_n\\ \dot{x_0}\\ ⋮\\ \dot{x_n}\\ \ddot{x_0}\\ ⋮\\ \ddot{x_n}\end{array}\right]$$

(3) lateral acceleration term

The third term: $w_{\ddot{x}}{\sum }_{i=0}^{n-1}{\ddot{x}}_i^2$

$${\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_n\\ \dot{x_0}\\ ⋮\\ \dot{x_n}\\ \ddot{x_0}\\ ⋮\\ \ddot{x_n}\end{array}\right]}^T\left[\begin{array}{cccccc}0& & & & & \\ & \ddots & & & & \\ & & 0& & & \\ & & & 2w_{\ddot{x}}& & \\ & & & & \ddots & \\ & & & & & 2w_{\ddot{x}}\end{array}\right]\left[\begin{array}{c}{x}_0\\ x_1\\ ⋮\\ x_n\\ \dot{x_0}\\ ⋮\\ \dot{x_n}\\ \ddot{x_0}\\ ⋮\\ \ddot{x_n}\end{array}\right]$$

(4) lateral jerk term

The forth term: $w_{\stackrel{\cdots }{x}}{\sum }_{i=0}^{n-2}{\stackrel{\cdots }{x}}_i^2$
Since we do not have $\stackrel{\cdots }{x}$ form in our cost function, we need to use $\ddot{x}$ to represent the $\stackrel{\cdots }{x}$ term:

$${\stackrel{\cdots }{x}}_i=\frac{{\ddot{x}}_{i+1}-{\ddot{x}}_i}{\Delta s}$$
$$\sum _{i=0}^{n-2}{\stackrel{\cdots }{x}}_i^2=\sum _{i=0}^{n-2}({\ddot{x}}_{i+1}-{\ddot{x}}_i{)}^2=\sum _{i=0}^{n-2}({\ddot{x}}_{i+1}-{\ddot{x}}_i{)}^T({\ddot{x}}_{i+1}-{\ddot{x}}_i)$$

in matrix term:

$$\frac{2}{\Delta s^2}{\left[\begin{array}{c}{\ddot{x}}_0\\ {\ddot{x}}_1\\ ⋮\\ {\ddot{x}}_n\end{array}\right]}^T\underset{{\widetilde{Q_{\stackrel{\cdots }{x}}}}^T}{\underbrace{\left[\begin{array}{ccccc}-1& & & & \\ 1& -1& & & \\ & 1& \ddots & & \\ & & \ddots & -1& \\ & & & 1& 0\end{array}\right]}}\underset{\widetilde{Q_{\stackrel{\cdots }{x}}}}{\underbrace{\left[\begin{array}{ccccc}-1& 1& & & \\ & -1& 1& & \\ & & \ddots & \ddots & \\ & & & -1& 1\\ & & & & 0\end{array}\right]}}\left[\begin{array}{c}{\ddot{x}}_0\\ {\ddot{x}}_1\\ ⋮\\ {\ddot{x}}_n\end{array}\right]$$

Let’s define the respective Q matrix for ${\stackrel{\cdots }{x}}_i$ term as $Q_{\stackrel{\cdots }{x}}$ then

$$Q_{\stackrel{\cdots }{x}}={\widetilde{Q_{\stackrel{\cdots }{x}}}}^T\widetilde{Q_{\stackrel{\cdots }{x}}}$$

We have

$$Q_{\stackrel{\cdots }{x}}=\left[\begin{array}{ccccccc}1& -1& & & & & \\ -1& 2& -1& & & & \\ & -1& 2& \ddots & & & \\ & & \ddots & \ddots & \ddots & & \\ & & & \ddots & 2& -1& \\ & & & & -1& 2& -1\\ & & & & & -1& 1\end{array}\right]$$

verify

Let’s use a specific case to verify the $Q_{\stackrel{\cdots }{x}}$, let’s say we have four points $\left[\begin{array}{c}{x}_0,x_1,x_2,x_3\end{array}\right]$, now define the ${\stackrel{\cdots }{x}}_i^2$ term:

$$\sum _{i=0}^2{\stackrel{\cdots }{x}}_i^2=\sum _{i=0}^2({\ddot{x}}_{i+1}-{\ddot{x}}_i{)}^2=({\ddot{x}}_1-{\ddot{x}}_0{)}^2+({\ddot{x}}_2-{\ddot{x}}_1{)}^2+({\ddot{x}}_3-{\ddot{x}}_2{)}^2$$

Use the matrix form:

$$\begin{array}{c}\left[\begin{array}{cccc}{\ddot{x}}_0& {\ddot{x}}_1& {\ddot{x}}_2& {\ddot{x}}_3\end{array}\right]\left[\begin{array}{cccc}\frac{1}{\Delta s^2}& -\frac{1}{\Delta s^2}& 0& 0\\ -\frac{1}{\Delta s^2}& \frac{2}{\Delta s^2}& -\frac{1}{\Delta s^2}& 0\\ 0& -\frac{1}{\Delta s^2}& \frac{2}{\Delta s^2}& -\frac{1}{\Delta s^2}\\ 0& 0& -\frac{1}{\Delta s^2}& \frac{1}{\Delta s^2}\end{array}\right]\left[\begin{array}{c}{\ddot{x}}_0\\ {\ddot{x}}_1\\ {\ddot{x}}_2\\ {\ddot{x}}_3\end{array}\right]\\ =\left[\begin{array}{c}\frac{{\ddot{x}}_0-{\ddot{x}}_1}{\Delta s^2},\frac{2{\ddot{x}}_1-{\ddot{x}}_0-{\ddot{x}}_2}{\Delta s^2},\frac{2{\ddot{x}}_2-{\ddot{x}}_1-{\ddot{x}}_3}{\Delta s^2},\frac{{\ddot{x}}_3-{\ddot{x}}_2}{\Delta s^2}\end{array}\right]\left[\begin{array}{c}{\ddot{x}}_0\\ {\ddot{x}}_1\\ {\ddot{x}}_2\\ {\ddot{x}}_3\end{array}\right]\\ =\frac{({\ddot{x}}_0-{\ddot{x}}_1){\ddot{x}}_0}{\Delta s^2}+\frac{[({\ddot{x}}_1-{\ddot{x}}_0)+({\ddot{x}}_1-{\ddot{x}}_2)]{\ddot{x}}_1}{\Delta s^2}+\frac{[({\ddot{x}}_2-{\ddot{x}}_1)+({\ddot{x}}_2-{\ddot{x}}_3)]{\ddot{x}}_2}{\Delta s^2}+\frac{({\ddot{x}}_3-{\ddot{x}}_2){\ddot{x}}_3}{\Delta s^2}\\ ={\ddot{x}}_0^2-2{\ddot{x}}_1{\ddot{x}}_0+{\ddot{x}}_1^2+{\ddot{x}}_1^2-2{\ddot{x}}_1{\ddot{x}}_2+{\ddot{x}}_2^2+{\ddot{x}}_2^2-2{\ddot{x}}_2{\ddot{x}}_3+{\ddot{x}}_3^2\\ =({\ddot{x}}_0-{\ddot{x}}_1{)}^2+({\ddot{x}}_1-{\ddot{x}}_2{)}^2+({\ddot{x}}_2-{\ddot{x}}_3{)}^2\\ =\sum _{i=0}^{n-2}{\ddot{x}}_i^2\end{array}$$

Q Matrix

The Q matrix is summation of the Q matrix of each cost function term:

$$\mathbb{Q}=\left[\begin{array}{ccccccc}2w_x& & & & & & \\ & \ddots & & & & & \\ & & 2w_x& & & & \\ & & & 2w_{\dot{x}}& & & \\ & & & & \ddots & & \\ & & & & & 2w_{\dot{x}}& \\ & & & & & & Q_{\ddot{x}}+Q_{\stackrel{\cdots }{x}}\end{array}\right]$$

Where

$$Q_{\ddot{x}}+Q_{\stackrel{\cdots }{x}}=\left[\begin{array}{cccc}2w_{\ddot{x}}+\frac{2w_{\stackrel{\cdots }{x}}}{\Delta s^2}& -\frac{2w_{\stackrel{\cdots }{x}}}{\Delta s^2}& & \\ -\frac{2w_{\stackrel{\cdots }{x}}}{\Delta s^2}& 2w_{\ddot{x}}+\frac{4w_{\stackrel{\cdots }{x}}}{\Delta s^2}& \ddots & \\ & \ddots & \ddots & -\frac{2w_{\stackrel{\cdots }{x}}}{\Delta s^2}\\ & & -\frac{2w_{\stackrel{\cdots }{x}}}{\Delta s^2}& 2w_{\ddot{x}}+\frac{2w_{\stackrel{\cdots }{x}}}{\Delta s^2}\end{array}\right]$$

4. Constraints

We have several constraints as follows:

a. $x_{lb}[i]\le x_i\le x_{ub}[i]$, the lateral offset $x_i$ should be constrained between the road upper and lower boundaries.

b. $\dot{x_{lb}[i]}\le {\dot{x}}_i\le \dot{x_{ub}[i]}$, the lateral velocity should be constrained between the maximum and minimum velocity constraints.

c. $\ddot{x_{lb}[i]}\le {\ddot{x}}_i\le \ddot{x_{ub}[i]}$, the lateral acceleration should be constrained between the maximum and minimum acceleration constraints.

d. $\stackrel{\cdots }{x_{lb}[i]}\le \frac{{\ddot{x}}_{i+1}-{\ddot{x}}_i}{\Delta s}\le \stackrel{\cdots }{x_{ub}[i]}$, the lateral jerk should be constrained between the maximum and minimum jerk constraints.

e. continuity constraints, We have physical relations as follows:

$$\begin{array}{c}\ddot{l_{i+1}}=\ddot{l_i}+{\int }_0^{\Delta s}\stackrel{\cdots }{l_{i\to i+1}}ds=\ddot{l_i}+\stackrel{\cdots }{l_{i\to i+1}}\Delta s\\ \dot{l_{i+1}}=\dot{l_i}+{\int }_0^{\Delta s}\ddot{l_{i\to i+1}}ds=\dot{l_i}+\ddot{l_i}\Delta s+\frac{1}{2}\stackrel{\cdots }{l_{i\to i+1}}\Delta s^2\\ l_{i+1}=l_i+{\int }_0^{\Delta s}{l}_{i\to i+1}ds=l_i+{\dot{l}}_i\Delta s+\frac{1}{2}{\ddot{l}}_i\Delta s^2+\frac{1}{6}{\stackrel{\cdots }{l}}_i\Delta s^3\end{array}$$

We also have ${\stackrel{\cdots }{l}}_i=\frac{{\ddot{l}}_{i+1}-{\ddot{l}}_i}{\Delta s}$ with

$$\begin{array}{c}{l}_{i+1}=l_i+{\dot{l}}_i\Delta s+\frac{1}{2}{\ddot{l}}_i\Delta s^2+\frac{1}{6}{\stackrel{\cdots }{l}}_i\Delta s^3\\ =l_i+{\dot{l}}_i\Delta s+\frac{1}{2}{\ddot{l}}_i\Delta s^2+\frac{1}{6}(\frac{{\ddot{l}}_{i+1}-{\ddot{l}}_i}{\Delta s})\Delta s^3\\ =l_i+{\dot{l}}_i\Delta s+\frac{1}{2}{\ddot{l}}_i\Delta s^2+\frac{1}{6}(\ddot{l_{i+1}}-{\ddot{l}}_i)\Delta s^2\\ =l_i+{\dot{l}}_i\Delta s+\frac{1}{2}{\ddot{l}}_i\Delta s^2+\frac{1}{6}{\ddot{l}}_{i+1}\Delta s^2-\frac{1}{6}{\ddot{l}}_i\Delta s^2\\ =l_i+{\dot{l}}_i\Delta s+\frac{1}{3}{\ddot{l}}_i\Delta s^2+\frac{1}{6}{\ddot{l}}_{i+1}\Delta s^2\end{array}$$

f. similarly,
$${\dot{l}}_{i+1}={\dot{l}}_i+\frac{1}{2}{\ddot{l}}_i\Delta s+\frac{1}{2}{\ddot{l}}_{i+1}\Delta s$$

Now we generate constraints in matrix form. We use $b_{lb}\le Ax\le b_{ub}$

constraints a

constraints b

constraints c

constraints d

constraints e

constraints f

Equality constraints

We fix initial lateral offset, lateral velocity and lateral acceleration at some values.

Bounds

Finally, the constraints are summations of all constraints above, the lower and upper bounds are:

The A matrix form:

Summary

Now, the standard form is generated:

$$\begin{array}{c}\arg \min \frac{1}{2}{X}^{T}HX+f^{T}x\\ b_{lb}\le Ax\le b_{ub}\end{array}$$

The H matrix is Q, and $f^T$ is $O\in R^{3n}$.