Apollo_ADS_路徑規劃- QP-Spline-ST-Speed Optimizer

QP-Spline-ST-Speed Optimizer

https://github.com/ApolloAuto/apollo/blob/master/docs/specs/qp_spline_st_speed_optimizer.md

1 Definition

After finding a path in QP-Spline-Path, Apollo converts all obstacles on the path and the ADV (autonomous driving vehicle) into an path-time (S-T) graph, which represents that the station changes over time along the path. The speed optimization task is to find a path on the S-T graph that is collision-free and comfortable.

Apollo uses splines to represent speed profiles, which are lists of S-T points in S-T graph. Apollo leverages Quadratic programming to find the best profile. The standard form of QP problem is defined as:

在QP樣條路徑中找到路徑後，Apollo將路徑上的所有障礙物和ADV(自動駕駛車輛)轉換爲路徑時間(S-T)圖，表示站點在路徑上隨時間的變化。速度優化任務是在S-T圖上找到一條無碰撞且舒適的路徑。

Apollo使用樣條來表示速度profile，這是S-T圖中S-T點的列表。阿波羅利用二次規劃來尋找最佳profile。QP問題的標準形式定義爲:

 

 

2 Objective function

2.1 Get spline segments

Split the S-T profile into n segments. Each segment trajectory is defined by a polynomial.

2.2 Define function for each spline segment

Each segment i has an accumulated (積分) distance d_i along a reference line. And the trajectory for the segment is defined as a polynomial of degree five by default. The degree of the polynomials are adjustable by configuration parameters.

位置公式--時間t

如果求導，一階導數得到v,二階a,三階 jerk. ...

2.3 Define objective function of optimization for each segment

Apollo first defines cost_1 to make the trajectory smooth:

Then Apollo defines cost_2 as the difference between the final S-T trajectory and the cruise S-T trajectory (with given speed limits — m points):

Similarly, Apollo defines cost_3 that is the difference between the first S-T path and the follow S-T path (o points):

Finally, the objective function is defined as:

cost = cost_1 + cost_2 + cost_3

3 Constraints

3.1 The init point constraints

Given已知  the assumption that the first point is ($t0$, $s0$), and $s0$ is on the planned path f_i(t) , f'i(t), and f_i(t)'' (position, velocity, acceleration). Apollo converts those constraint into QP equality constraints:

3.2 Monotone 單調性 constraint

The path must be monotone, e.g., the vehicle can only drive forward.--汽車向前開，s一直變大

Sample m points on the path, for each j and j-1 point pairs  :

If the two points on the same spline $k$:

If the two points on the different spline $k$ and $l$:

3.3 Joint smoothness constraints

This constraint is designed to smooth the spline joint. Given the assumption that two segments, $seg_k$ and $seg_{k+1}$, are connected, and the accumulated s of segment $seg_k$ is $s_k$, Apollo calculates the constraint equation as:

Namely:

Then

The result is $t_0$ = 0 in the equation.

Similarly calculate the equality constraints for

3.4 Sampled points for boundary constraint

Evenly sample m points along the path, and check the obstacle boundary at those points. Convert the constraint into QP inequality constraints, using:

Apollo first finds 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. Then it calculates the inequality constraints as:

Similarly, for upper boundary $l_{ub,j}$, Apollo calculates the inequality constraints as:

3.5 Speed Boundary constraint

Apollo establishes a speed limit boundary as well.

Sample m points on the st curve, and get speed limits defined as an upper boundary and a lower boundary for each point $j$, e.g., $v{ub,j}$ and $v{lb,j}$ . The constraints are defined as:

Namely

And

Namely