# Trajectory Planning of a Semi-Trailer Train Based on Constrained Iterative LQR
## 2 Constructing Trajectory Planning Problems
### Constraint Condition
#### 2.2.4 Semi-Trailer Train Articulation Angle Constraint  
The articulation angle of a semi-trailer train is the angle between the tractor heading and the semi-trailer heading. In the semi-trailer train driving process, when the articulation angle is greater than a certain threshold, the two body parts interfere, causing the vehicle to fold and lose stability. To improve the safety and stability of vehicle travel, it is necessary to constrain the articulation angle of semi-trailer trains. In the Cartesian coordinate system, the tractor's and semi-trailer's positive and negative yaw angles are defined as in the positive direction of the \(X\)-axis, at an angle less than \(\pi\), the angle obtained by clockwise rotation is negative, and the angle obtained by counterclockwise rotation is positive. The formula for calculating the vehicle articulation angle is as follows [32]:  
Figure 4: Dynamic expansion ellipse. (1) \(\theta_{1}\cdot\theta_{2}\geq 0\), the tractor yaw angle and the semi-trailer yaw angle have the same sign, and the articulation angle is the difference between the two yaw angles:  
\[\psi=|\theta_{1}-\theta_{2}| \tag{24}\]  
(2) \(\theta_{1}\cdot\theta_{2}<0\), tractor and semi-trailer yaw angle symbol opposite:  
\[ \begin{cases}if:0<\theta_{1}<\frac{\pi}{2},\ -\frac{\pi}{2}<\theta_{2}<0&\psi=| \theta_{1}|+|\theta_{2}|\\ if:\frac{\pi}{2}<\theta_{1}\leq\pi,\ -\pi<\theta_{1}<-\frac{\pi}{2}&\psi=2\pi-(| \theta_{1}|+|\theta_{2}|)\\ if:-\pi<\theta_{1}<-\frac{\pi}{2},\ \frac{\pi}{2}<\theta_{2}\leq\pi&\psi=2\pi-(| \theta_{1}|+|\theta_{2}|)\\ if:-\frac{\pi}{2}<\theta_{1}\leq 0,\ 0<\theta_{1}\leq\frac{\pi}{2}&\psi=(| \theta_{1}|+|\theta_{2}|)\end{cases}  \tag{25}\]  
Vehicle speed is generally categorized into low, medium, and high speeds. Low speed generally refers to speeds less than or equal to 20 km/h; medium speed refers to speeds between 20 and 80 km/h; and high speed refers to speeds greater than or equal to 80 km/h. A semi-trailer train is turning at low speed to avoid folding between the tractor and the semi-trailer and losing stability. The steering wheel can be quickly reversed to reduce the articulation angle when folding is about to occur. Since the transmission ratio of the steering system of the semi-trailer train is large, generally around 25, a certain time delay is required from the steering wheel turning to the change in the tractor front wheel angle. Assuming that the articulation angle threshold is set to 90\({}^{\circ}\), even if the steering wheel is turned at a large angle when folding is about to occur, the body attitude will be difficult to adjust quickly, and the folding phenomenon will occur very easily. The vehicle's maneuverability will be affected if the articulation angle threshold is set to a smaller value. In summary, the articulation angle threshold is set at 80\({}^{\circ}\). Among them, the articulation angle threshold is a physical limit constraint to prevent the semi-trailer train from folding, which is a hard constraint and can be expressed as follows:  
\[|\psi|\leq 80^{\circ} \tag{26}\]  
When planning the trajectory, ensuring that the articulation angle corresponding to each point on the planned trajectory satisfies the constraint of the physical threshold will ensure that the vehicle does not fold during travel. When the vehicle is turning and braking at high speed. Due to the semi-trailer train, the high vehicle mass center, and the narrow wheelbase, it is easy to produce the push-head phenomenon. This causes the rear axle tires of the tractor to become saturated and slip sideways, causing the tractor to fold with the semi-trailer under the inertial force of the semi-trailer. To ensure the safety and stability of vehicle driving, soft constraint is applied to the articulated angle when the vehicle is driving at high speed. In determining the soft constraint threshold for the articulation angle, if the soft constraint threshold is selected as small, the tractor tends to produce a small corner, and the semi-trailer train body is straighter.