6. Planning control - action planning (2)

Summary: This article mainly studies the action planner based on Lattice Planner.

---

Preface

This article will understand and study action planning based on Lattice Planner.

---

1. Lattice Planner

1.1 Concept

1. Sampling-based local motion planner;
2. Plan in Frenet coordinate system.

Planner input:

1. Localization (high-precision map positioning information)
2. Perception (perception of surrounding environment information and obstacles)
3. Reference (reference line information of high-precision map)
   planner output:
   Since the planner is sent directly to the underlying controller, the trajectory output by the planner is a smooth, safe trajectory without collision risk, which meets the requirements of the vehicle. Kinematics and velocity constraints.

1.2 Basic process

Lattice trajectory sampling points add kinematic model constraints and have continuity.
Basic principle: The vehicle coordinate system is converted to the Frenet coordinate system, and the horizontal and vertical axes are planned to form a planned trajectory under this coordinate system. Then restore the trajectory to the world coordinate system.
The process is as follows:

1. Reference line discretization;
2. Convert the found reference line information to the Frenet coordinate system to obtain the initial state of the vehicle in the Frenet coordinate system;
3. Based on obstacle information, speed constraints and other information, determine the vehicle's driving target point;
4. Through polynomial connection, horizontal and vertical one-dimensional trajectory trees are generated, and the trajectory is confirmed to comply with the kinematic trajectory and constraints. The trajectory that needs to be driven is obtained by merging the good trajectories;
5. Convert the obtained trajectory points from the Frenet coordinate system to the Cartesian coordinate system to obtain a smooth trajectory in the world coordinate system that satisfies kinematic constraints;
6. Collision detection and optimal trajectory scoring are performed in the world coordinate system, and the optimal, collision-free, smooth trajectory is obtained and input into the controller for tracking.

2. Frenet coordinate

2.1 Concept

The Frenet coordinate system is a coordinate system on the reference line and is a dynamic coordinate system (T axis, N axis).

2.2 Contact between Frenet and Cartesian coordinate systems

2.2.1 Frenet coordinate system

The Frenet coordinate system can express the road position more intuitively; it is commonly expressed by s and l variables;

1. concept

(1) s: the longitudinal coordinate, the front and rear displacement along the road;
(2) s': the reciprocal of the longitudinal coordinate to time, that is, the longitudinal speed;
(3) s'': the second-order reciprocal of the longitudinal coordinate to time , longitudinal acceleration;
(4) l: transverse coordinate, left and right displacement along the road;
(5) l`: derivative of transverse coordinate with respect to time, transverse velocity;
(6) l`` second derivative of transverse coordinate with respect to time , transverse acceleration;
(7) l': the reciprocal of the transverse coordinate to the longitudinal coordinate;
(8) l'': the second-order reciprocal of the transverse coordinate to the longitudinal coordinate;

2.2.1 Cartesian coordinate system

(1) x vector, the coordinate of the horizontal axis in the Cartesian coordinate system;
(2) vx, the coordinate velocity standard quantity of the horizontal axis in the Cartesian coordinate system;
(3) ax, the acceleration of the horizontal axis in the Cartesian coordinate system;
( 4) θx, the heading angle (orientation) of the horizontal axis in the Cartesian coordinate system;
(5) kx, the curvature in the Cartesian coordinate system, ds is the arc length of the current trajectory,

2.3 Conversion between Frenet coordinate system and Cartesian coordinate system

2.3.1 Cartesian to Frenet（x,y->s,l）

Derived formula:

2.3.2 Frenet to Cartesian（s,l->x,y）

三、Lattice planner sampling

Lattice is suitable for structural and regular sampling of roads, highways, etc. The sampling methods are divided into:
(1) Lateral sampling: Lateral sampling, the sampling interval is set according to the width of the vehicle to form a horizontal sampling offset.

(2) Longitudinal sampling: Longitudinal sampling, using the displacement length s of the look-ahead point as the reference sampling length, to sample the velocity ds of the trajectory.

(3) Time sampling: Trajectory time period sampling
(4) State space: Sampling according to the environment
(5) Control space: Sampling according to the vehicle motion mode

四、Lattice Planner：speed planning

The trajectory is generated by a polynomial curve, which is to solve the coefficients of the polynomial curve based on the state of the first and last points. Based on the sampling explained above, the planning function of horizontal l(s) and vertical s(t) is generated and passed through 4 times or The vehicle polynomial of degree 5 is fitted to ensure the smoothness of the trajectory.

(1) For longitudinal trajectories, jerk requirements will be higher when parking, following, or overtaking. You can use a 5th degree polynomial to ensure the smoothness of the path. The first and last states are (s, v, a);

(2) For the longitudinal trajectory, during cruising, there is no need to know the state of s, and a 4th degree polynomial can be used for fitting (first: s, v, a, last: v, a); (3) For the transverse trajectory
, A 5th degree polynomial can be used for fitting.

4.1 Follow the parking scene

4.1.1 Longitudinal polynomial fitting solution (5th order)

4.1.2 Constraint functions

In the trajectory planning process, constraints are generally divided into: initial state and end state (sampling state);

(1) Initial state:

Initial state longitudinal displacement, velocity, acceleration.
(2) Sampling state (t1 is the trajectory running time period):

sampling state longitudinal displacement, longitudinal velocity, and longitudinal acceleration.
(3) Overall constraints

The constraint coefficients can be obtained by using the constraint equation.

4.2 Cruise scene

The cruise mode actually controls the speed under uniform and constant speed movement, and is not constrained by changes in longitudinal displacement.

4.2.1 Longitudinal polynomial fitting solution (4th order)

4.2.2 Constraints

(1) Initial state:
initial longitudinal displacement, longitudinal velocity, longitudinal acceleration

(2) Sampling state:
longitudinal velocity, longitudinal acceleration

(3) General constraints

四、Lattice Planner：Lateral trajectory planning

(1) Fifth degree polynomial fitting
The transverse fitting function takes the longitudinal displacement as the independent variable l(s) (5th degree polynomial)

(2) Constraint conditions


Initial lateral displacement: d0;
Initial lateral velocity: sd0;
Initial lateral acceleration: ssd0;
sampled lateral displacement: d1;
sampled lateral velocity: sd1;
sampled lateral acceleration: ssd1;

五、Lattice Planner processing

5.1 Evaluation

After generating the horizontal and vertical one-dimensional trajectory trees, it is necessary to evaluate the feasibility.

If any trajectory point exceeds the range of displacement, velocity, and acceleration, it will be filtered out, greatly easing subsequent work.

5.2 cost function

Sort according to the calculation of the horizontal and vertical costs, evaluate the horizontal and vertical trajectory pairs, and select a path that is close to the reference trajectory, has good comfort, and is far away from obstacles.

5.3 trajectory cost function

5.3.1 Longitudinal Objective achievement cost

Find a feasible path closest to the target point.

Generally divided into two scenarios:
(1) In the parking instruction scene (red light), because the vehicle speed will drop rapidly, the corresponding trajectory cost will be very large;
(2) In the scene without parking instruction, the cost of the low-speed trajectory will be even greater
Implementation effect:
1) Jspeed: The actual speed should be close to the reference speed and optimized;
2) Jdist: The smaller the dist, the greater the cost; the larger the dist, the smaller the cost;
3) The combination of Jspeed and Jdist, w is the weight ratio .

5.3.2 Comfort Objective

Look for a comfortable path with small changes in acceleration. Generally, a large jerk will feel like a shaky ride.

5.3.3 Centripetal Objective

Find a feasible path with a small change in centripetal acceleration. You can slow down when making big turns or U-turns.

5.3.4 Collsion Objective

To choose a path away from an obstacle.

The two trajectories on the left are reflected in the ST image. The red trajectory and the blue obstacle vehicle overlap in the shaded area, indicating that there is a risk of collision. If the risk of collision is high, the cost of collision will be relatively high. If the risk of collision is low, then the cost of collision will be relatively high. The cost will be relatively low.

5.3.5 Lateral offset Objective

Choose a feasible path that follows the center of the road. This leaves more headroom for interacting with road boundaries and vehicles.

As shown in the figure, the cost of driving sideways or swaying sideways will be relatively large, while the cost of driving in the center of the road will be relatively small.

5.3.6 Lateral Acceleration Objective

Choose a feasible path to ensure a smooth lane change process and improve the smoothness of lane changes.

As shown in the picture on the left, if you turn the steering wheel hard and the curvature changes dramatically, the cost of lateral acceleration will be relatively high, while in the picture on the right if you change lanes smoothly, the cost will be low.

Summarize

This article mainly studies the Lattice Planner grid planner in action planning in autonomous driving planning control. It mainly introduces the concept of its planner, the conversion relationship between the cartesian coordinate system and the Frenet coordinate system, and the horizontal and vertical trajectory planning fitting. Finally, it summarizes how to use planning to select the optimal path. This article hopes to be of some help to students who want to learn the direction of autonomous driving planning and control.
Friends who like it, move your little hands and click on it. I will regularly share some of my knowledge summaries and experiences. Thank you all!