The method of curve interpolation is to carry out the curve fitting of the path according to the vehicle under certain conditions (safety, fastness, high efficiency). The common ones are polynomial curve, double arc segment curve, sine function curve, Bezier curve , B-spline curve, etc.
1. Algorithm thinking:
The core idea of the curve interpolation method is based on the pre-constructed curve type, according to the expected state of the vehicle (for example, the speed and acceleration of the vehicle reaching a certain point are required to be the expected value), and the expected value is substituted into the curve class: type as the boundary condition to solve the equation. Get the correlation coefficient of the curve.
Once all the correlation coefficients of the curves are determined, trajectory planning is then completed.
2. Intensive lecture on algorithms
2.1 Polynomial curve
The polynomial curve is taken as an example to explain the trajectory planning of the curve interpolation method.
Polynomial curves are divided into three-degree polynomial curves , five-degree polynomial curves , and seven-degree polynomial curves
- For the cubic polynomial curve, the expected state of each expected point can be determined at most in two dimensions, generally speaking, position and velocity .
- For the quintic polynomial curve, the expected state of each expected point in three dimensions can be determined at most, generally speaking, it is position, velocity, and acceleration .
- For the seven-degree polynomial curve, the expected state of each expected point in four dimensions can be determined at most, generally speaking, it is position, velocity, acceleration, and jerk (the jerk is called
jerk, the jerk is calledsnap, and the UAV trajectory planning useful insnap)
3. Five degree polynomial curve
Take the quintic polynomial curve as an example to explain the trajectory planning of the curve value method.
Let t0 be the initial time, and the position, velocity, and acceleration are all known. Obviously, the x and yx and yx and y directions have the following three equations respectively:
Define the end time of lane change as t1, and the horizontal and vertical directions (that is, x, yx, yx, y directions) have expected position, velocity, and acceleration, and the following three equations can be obtained respectively:
Express the horizontal and vertical equations of the first and last points in a unified matrix as:
The independent variable of the polynomial curve is time t, so once the coefficient matrices A and B are solved, the curve equation can be determined (to put it bluntly, the method of finding the coefficient is the undetermined coefficient method we learned in junior high school). After the curve is uniquely determined, the position and speed of each point on the curve are determined, and the trajectory can be calculated. The derivative of each point on the curve represents the speed of the vehicle passing the point, indicating that the polynomial curve lane change trajectory planning is the coupling result of path + speed.
Note that the quintic polynomial lane change trajectory curve specifically means that the lateral position/longitudinal position is a quintic polynomial with respect to time t, rather than the quintic polynomial of longitudinal position y with respect to lateral position x.
4. Double arc segment curve
As shown in the figure, for the double arc segment lane change trajectory, it is composed of arc AC+line segment CD+arc DF.
Obviously, at point C, the curvature of the trajectory suddenly changes from the fixed value of arc AC to 0, so in order for the vehicle to follow the trajectory completely, considering that the steering wheel angle is a continuous and gradual process , the vehicle must have a speed of 0 after reaching point C , the driving can only be continued after the steering wheel is returned to normal, so it cannot be applied to the planning of the driving path , but to the planning of the parking path .