Pure Tracking Partial Path Planner
After obtaining the local path, this project uses the pure tracking algorithm to control the corner of the car through the feedback of the lateral controller, so as to achieve the effect of tracking the local path. It is confirmed by experiments that the tracking effect is very good.
The principle of pure tracking algorithm is as follows:
When building the model, the car can be regarded as a vehicle kinematics bicycle model
Kinematics is the study of the laws of motion of objects from the perspective of geometry, including the changes in the position and speed of objects in space over time. Therefore, the vehicle kinematics model can reflect the relationship between vehicle position, speed, acceleration, etc. and time. The controller designed based on the kinematics model can also ensure that the instructions issued to the vehicle's bottom execution layer conform to the vehicle's kinematics constraints. For the kinematic bicycle model, we generally make the following assumptions:
Do not introduce any forces that would affect the vehicle's motion
The vehicle only moves on the xy plane, and the roll on the z axis (vertical direction), front and rear pitch, etc. are not considered.
The structure of the vehicle is like a bicycle, the left and right front wheels have the same rotation angle and speed, and the same is true for the rear wheels. So each of the two front and rear wheels can be described by only one tire. (Small steering angle)
The respective velocity vectors of the front and rear wheels are consistent with the orientation of the respective tires, that is, it is assumed that the tires have no sideslip.
Generally, only at low speeds, the lateral force generated by the tires is negligible, so this model is mainly used in low-speed scenarios.
The simplified diagram of the model is as follows:
The symbols represent the physical meaning as follows
Formula Derivation:
From the law of sines it can be deduced that:
Right now:
where k is the curvature of the turning arc. It can also be deduced from the above figure:
So:
It can be seen from the above formula that the essence of this controller is to control the rotation angle and to reduce the lateral error as the goal of the lateral controller. Among them, 2L/ld2 can be regarded as the parameter of the controller P, L is the wheelbase of the vehicle, and is the set preview distance. The control effect of this controller mainly depends on the value of the preview distance. Generally speaking, the longer the preview distance, the smoother the control effect, and the shorter the preview distance, the more accurate the control effect will be (it will also bring a certain amount of shock. ), the selection of the preview distance is also related to the current vehicle speed, the following formula is an example:
Where v is the current vehicle speed, g is an adjustable parameter, and lf is a preset look-ahead distance.
In some literatures, the selection method of the preview distance is expressed as follows:
In the above formula, the constant A=1/(2amax), and amax is the maximum braking acceleration. Av2 represents the shortest vehicle braking distance. B represents the reaction time required for the vehicle to encounter an abnormality, Bv represents the corresponding reaction distance, and C represents the minimum turning radius of the vehicle.
In the practical application of this algorithm, it is usually not required that the distance from the tracked target point to the center of the rear axle of the vehicle is exactly equal to the preview distance. Instead, it will select the point from the sampled series of target points that is closest to the preview distance from the center of the rear axle to approximate tracking. The advantage of this is that the real predicted distance coordinates can be solved without the functional equation of the target trajectory, which greatly improves the efficiency of the algorithm.
Before the algorithm is applied to the vehicle, it has passed the experimental test, and the test results are as follows:
According to the experiment, it is concluded that through the pure tracking method, we can track the curve basically without error when dealing with too small corners. And in the actual vehicle running process, we found that basically error-free tracking can also be achieved.
Partially refer to the following blog (165 messages) Unmanned vehicle control (1) Pure Pursuit (Pure Pursuit) algorithm and Stanley algorithm_Pure Pursuit algorithm does not work well at high speeds_Xiaozheng loves learning blog-CSDN Blog