[Paper Notes] Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constrain

The article "Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraints for In-lane Driving Scenarios" is the source of Apollo's speed planning idea based on quadratic planning .

I. INTRODUCTION

It mainly introduces the two-stage algorithm: the first stage generates a reference line; the second stage uses the collision-free area and reference line obtained by path-time obstacle graph (ST boundary) analysis as input to generate longitudinal acceleration. , centripetal acceleration and collision-free comfortable trajectory with minimum acceleration.

This method of path-time obstacle graph (ST graph) determines whether a collision with the own vehicle will occur in the future by projecting the predicted trajectories of surrounding obstacles onto the ST graph. This method is suitable for structured roads and can make better use of road information (domain knowledge).

Optimization methods are more suitable for low-dimensional spaces. For high-dimensional spaces, the solution speed is slower and it is easy to fall into local minima.

II. RELATED WORK

Basically consistent with this article "Optimal Vehicle Path Planning Using Quadratic Optimization for Baidu Apollo Open Platform".

Notes: https://blog.csdn.net/sinat_52032317/article/details/132523043?spm=1001.2014.3001.5501

III. PROBLEM DEFINITION

$The\; role\; of\; \kappa$ : the real-time steering curvature of the vehicle. If the steering wheel of the vehicle maintains a certain angle, the vehicle will make a circular motion. The radius of the circular motion is the radius of curvature, and the reciprocal is the curvature κ \$\kappa$ . Join$\kappa$ , helps the control module obtain more accurate control feedback and design a more stable controller.

In this article, by default the vehicle trajectory will follow the reference line and possible lateral errors will be eliminated by an ideal controller.

IV. PHASE I: SMOOTH DRIVING GUIDE LINEGENERATION

The reference is first to the premise of planning in the Frenet coordinate system. The smoothness of the reference line will affect the quality of the planned trajectory. According to the document "Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame", the reference line needs to have continuous differential curvature to ensure continuous acceleration and steering angle.

Apollo uses a nonlinear optimization algorithm to generate reference lines.

• Objective: The swing of the reference line geometric characteristics will cause unsmooth acceleration and the swing of the vehicle steering angle, so the reference line length ss needs to be minimized$s$ , reference line curvature$\kappa$ , and the first-order differential of the reference line curvature$\dot{K}$
• Representation of the reference line : using a discrete-continuous combination method. $n$ input points,$n-A$ closed curve connects the points.
• Constraints : The reference line does not necessarily need to completely pass through the input point in the Cartesian coordinate system, because there may be errors; but it needs to be constrained within a certain range.

In this paper, the authors use a fifth-order polynomial spiral curve as a smooth representation of the road to facilitate optimization when generating trajectories for autonomous vehicles. The advantage of such a curve is that its geometric properties, such as curve direction, curvature, and rate of change of curvature, can be easily derived by directly solving for its derivatives. At the same time, the objective function of the spiral curve can be greatly simplified compared with other curve representations (such as parametric curves in Cartesian space).

Schematic diagram of spiral
$\theta$ is the tangential angle,$K=\frac{d}{ds}$, $\dot{K}=\frac{d^2}{{ds}^2}$

Reference examples given in the article

Optimization variables :

$\Delta sis$ the length of the curve connecting two points

$$x=\{i_0,i_1,\dots ,i_{n-1},\dot{i_0},\dot{i_1},\dots ,\dot{i_{n-1}},\ddot{i_0},\ddot{i_1},\dots ,\ddot{i_{n-1}}\}$$

What are the characteristic changes $i_i，{\dot{i}}_i，{\ddot{i}}_i,i_{i+1}，{\dot{i}}_{i+1}，{\ddot{i}}_{i+1}$And the curve length $\Delta s{\_}_i$The coefficients that uniquely determine the fifth-order spiral

$$i_i(s)=\sum _{j=0}^5{c}_j\ast s^j$$

The paper does not give a specific objective function form and corresponding constraint equations. I found a possible objective function form from the Internet:

$$cost=w_{length}\sum _{i=1}^{n-1}\Delta s{\_}_i+w_{kappa}\sum _{i=1}^{n-1}\sum _{i=0}^{m-1}({\dot{i}}_i(s_j){)}^2+w_{dkapp}\sum _{i=1}^{n-1}\sum _{i=0}^{m-1}({\ddot{i}}_i(s_j){)}^2$$

V. PHASE II: TRAJECTORY GENERATION

A. Optimality Modeling

Several evaluation indicators to measure trajectory quality:

1. Task achievement: Evaluated by comparing the specified target state with the state of the vehicle at the end of the trajectory. For in-lane scenarios, this part mainly includes two tasks:

• The Cruise vehicle can cruise at a target speed and make corresponding decisions for obstacles that appear.
• StopThe vehicle can stop at a designated target point or encounter an obstacle that requires a stopping decision.

2. Safety Safety:
   The main idea of ​​this part is to introduce the concept of ST diagram. Use the ST chart to determine when a collision with an obstacle will occur.
   Through the analysis of the ST diagram, it can be obtained that at a given time ttArea to avoid collision under$t$$[s_{min},s_{max}]$ , and smincan also be increased by passing$s_{min}$Or reduce $s_{max}$To improve the safety margin of self-vehicle driving. (When encountering more complex scenes, apollo uses simple heuristics to determine specific areas, which also means that more advanced related reasoning techniques can be used to improve)
   

ST picture in the scene where the side car cuts into the scene. Green: own car; red: side car. ST chart ordinate $s$ represents the distance traveled by the vehicle along the central reference line (this article assumes that the vehicle moves along the reference line, rather than moving along the path planned in the code);$t$ is time. The red quadrilateral is the projection of the obstacle vehicle on the ST diagram. The boundary formed by the two curves is formed after considering the vehicle speed and acceleration boundaries.

Kant and Zucker first introduced path time decoupling to solve the dynamic obstacle avoidance problem along a given path under speed constraints.

论文：Kant, Kamal, and Steven W. Zucker. “Toward Efficient Trajectory Planning: The Path-Velocity Decomposition.” The International Journal of Robotics Research, Aug. 1986, pp. 72–89, https://doi.org/10.1177/027836498600500304.

3. Comfort:
   In this regard, apollo mainly considers the following factors:

• Longitudinal acceleration: avoid frequent braking or accelerator operations;
• Longitudinal jerk: Reduce the amount of change in braking or accelerator;
• Centripetal acceleration: Centripetal acceleration needs to be reduced when cornering;
• Time: Complete the task as quickly as possible.

B. Constraints

Constraints:
In the algorithm, it is assumed that the vehicle will only move forward monotonically. Based on the vehicle kinematic characteristics, at any time $t$ needs to meet the following constraints to achieve safety, comfort and other goals:

1. Longitudinal velocity boundary: $\dot{s}\in [0,{\dot{s}}_{max}]$；
2. Longitudinal acceleration boundary: $\ddot{s}\in [{\ddot{s}}_{min},{\ddot{s}}_{max}]$；
3. vertical $jerk$边界: $s^{{}^{\prime \prime \prime }}\in [s_{min}^{{}^{\prime \prime \prime }},s_{max}^{{}^{\prime \prime \prime }}]$；
4. Centripetal acceleration boundary: $a_c\in [-a_{c_{max}},a_{c_{max}}]$
5. Safe driving boundary: $s\in s_{free}$

C. Trajectory Formulation and Discretization

This part introduces two discretization methods, both of which are discretized to $The\; second\; derivative\; of\; s$ to meet the relevant dynamic requirements, while using a constant$s^{{}^{\text{'}\text{'}\text{'}\text{'}}}$ to connect two discrete points.

Spatial parameter discretization:

Using a fixed spatial distance $\Delta s$ , optimization variables include$\dot{s},\ddot{s}$，$\Delta tis$ the time between two discrete points.

Advantages :

1. ss for each point$s$ is fixed, which also means that the curvature is also fixed. The maximum speed limit limited by centripetal acceleration can be calculated in advance, reducing the amount of calculation.

Disadvantages :
2. It is difficult to construct the objective function. Because $jerk$ is through two discrete points$\stackrel{¨The}{s}$ difference is calculated, but$\Delta tis$ unknown, and the objective function needs to be divided, which will cause the problem to become a non-convex problem.
3. It is difficult to use the information of ST diagram. Because ST boundary is time-related information.

Time parameter discretization:

Use a fixed time interval $\Delta t$ , optimization variables include$s,\dot{s},\ddot{s}$

advantage:

1. No need to $The\; calculation\; of\; jerk$ divides the objective function.
2. The information of ST boundary can be easily exploited.

shortcoming:

1. Difficulty calculating centripetal acceleration and related constraints. Under this method, the speed limit of the discrete point becomes one with $\kappa$ related function. Assume that the maximum centripetal acceleration is$a_{c_{max}}$, then the centripetal acceleration limit is as follows: $$-a_{cmax}\le {\dot{s}}^2\ast k\left(s\right)\le a_{cmax}$$To apply this constraint to the optimization process, ${\dot{s}}^2\ast \kappa \left(s\right)$ needs to be differentiable. In this article, a quintic spiral is used to solve this problem.

2. Another problem is that it is difficult to measure the minimum time to reach the target state. To solve this problem, the author introduced the reference speed into the objective function.

D. Complete Optimization Formulation

objective function

(4) enables the vehicle to reach a predefined speed as quickly as possible;
(5), (6), and (7) enable the vehicle to reach a predefined state at the end of the trajectory.

discretization

constraint

VI. IMPLEMENTATION AND EXPERIMENTS

Mainly using the IPOPT library for solution.

Conduct simulation tests on several scenarios of cruising (comfortable style and sporty aggressive style), parking, and car following.

VII. CONCLUSION AND FUTURE WORK

Mainly discuss the algorithm improvement direction extended to multi-lane scenarios:

1. Reference lines can dynamically adapt to surrounding static obstacles. It is guaranteed that the speed planning part needs almost no changes, but the calculation is time-consuming.
2. Another direction is to use optimization to generate lateral trajectories/paths $d\left(s\right)$ , which directly takes the projected static obstacles into consideration on the Frenet frame. This direction is more consistent with the framework planned by Frenet. The benefit is that smooth guide lines can be generated and stored offline, and dynamically loaded for online use. Additionally, lane change maneuvers will be handled more cleanly as lane changes can be planned by simply replacing the guide lines.

The latter is more similar to Apollo's current solution.