Pure pursuit trajectory tracking algorithm

At present, I have a small trailer and need to use the pure pursuit algorithm, so after reading the blog of the Great God, I compiled this article, and first provided the link of the Great God

The Pure Pursuit algorithm was first used for differential wheel chassis cars, and now it has been adapted for use in Ackerman chassis motion model vehicles (Ackerman motion model is not mentioned here). First of all, in the previous figure, the

PP algorithm takes the center of the rear axle of the vehicle as the tangent point, and the longitudinal body of the vehicle as the tangent. By controlling the front wheel angle, the vehicle passes through the target waypoint in a circular arc, $l_d$Indicates the distance from the center of the current rear axle of the vehicle to the target point, $\alpha$ represents the heading angle deviation between the current vehicle and the target waypoint.
According to the law of sine,
$$\frac{l_d}{sin\left(2\alpha \right)}=\frac{R}{sin(\frac{Fr.}{2}-a)}$$
After simplification,
$$\frac{l_d}{sin\alpha }=2R$$
can also be expressed as
$$k=\frac{2sin\alpha }{l_d}$$
In the formula, $k=1/R$ is the arc of the Ackerman motion model vehicle, then the front wheel angle$[delta]$ expression is:
$$d=\arctan (kL)$$
$L$ is the wheelbase of the vehicle. In summary,
$$\delta \left(t\right)=\arctan \frac{2Lsin(\alpha (t))}{l_d}$$
At $At$ time$t$ , know the angle between the vehicle body and the target pointα (t) \alpha(t)The forward-looking distance between$\alpha$$($$t$$)$ and the target waypoint$l_d$In the case, the wheelbase of the vehicle is a fixed value $L$ , the above formula can be used to calculate the current vehicle front wheel deflection control amount$\alpha$ .
In order to facilitate subsequent calculations, define a new quantity$e_l$, Which represents the lateral error between the current position of the vehicle and the target point of the vehicle, thus:
$$sin\left(\alpha \right)=\frac{e_l}{l_d}$$
The arc calculation can be expressed as:
$$k=\frac{2}{l_d^2}{e}_l$$
It can be seen that the essence of the PP algorithm is similar to a horizontal P controller, the coefficient $P$ is$\frac{2}{l_d^2}$, This $The\; P$ coefficient is affected by the parameter$l_d$Therefore, a new problem is introduced. How to select the forward-looking distance is the key to the PP algorithm. Generally speaking, the PP algorithm in Autoware is to make the forward-looking distance the current vehicle speed multiplied by a proportional coefficient. Choose a different foresight distance at the moment.
The PP algorithm is actually more suitable for the motion tracking of the differential wheel car. In the Ackerman model, it is not very suitable to use, especially when the speed of the vehicle changes greatly. How to choose the forward looking distance is a big problem. If the selected distance is too short, it will cause the trajectory to fail to converge, and the lateral control of the vehicle will diverge. If the selected distance is too long, it will cause insufficient vehicle cornering.