# Trajectory Planning of a Semi-Trailer Train Based on Constrained Iterative LQR
## 2 Constructing Trajectory Planning Problems
### Constraint Condition
#### 2.2.1 Vehicle Kinematic Constraint  
In exploring the trajectory planning problem for semi-trailer trains, only the state of the vehicle during its motion is considered, ignoring the influence of the ground forces of the vehicle. Considering the unique body structure of the semi-trailer train, the midpoint of the rear axle of the tractor is taken as the reference point of the vehicle, and the articulation point is assumed to be located in front of the vehicle reference point. Establish the vehicle kinematics model. As in Figure 1, the front and rear axles of the tractor and the semi-trailer axle are simplified to three points in the Cartesian coordinate system. \(O_{f}\left(x_{r},y_{f}\right)\) represents the midpoint of the front axle of the tractor. \(O_{r}(x_{r},y_{r})\) represents the vehicle reference point. \(O_{m}(x_{m},y_{m})\) represents the hitching point between the tractor and the semi-trailer. The semi-trailer axles are arranged in the form of three axles, equivalent to a single axle, with the midpoint denoted by \(O_{t}(x_{t},y_{t})\). \(L_{1}\) represents the tractor wheelbase, \(L_{2}\) is the length from the midpoint of the rear axle of the semi-trailer to the point of articulation, and \(L_{3}\) is the length from the vehicle reference point to the point of articulation. \(\theta_{1}\) is the yaw angle of the tractor, and \(\theta_{2}\) is the yaw angle of the semi-trailer. \(\delta_{f}\) represents the front wheel rotation angle. When the semi-trailer train is moving without considering lateral sliding, the axial velocity at the midpoint of all axes of the tractor and semi-trailer is 0, expressed as:  
\[\left\{ \begin{array}{l}\dot{x}_{r}\sin\theta_{1}-\dot{y}_{r}\cos\theta_{1}=0\\ \dot{x}_{f}\sin(\theta_{1}+\delta_{f})-\dot{y}_{f}\cos\Bigl{(}\theta_{1}+\delta _{f}\Bigr{)}=0\\ \dot{x}_{t}\sin\theta_{2}-\dot{y}_{t}\cos\theta_{2}=0\end{array} \right. \tag{5}\]  
The position relationship between the vehicle reference point and the articulation point is expressed according to the geometric relationship:  
\[\left\{ \begin{array}{l}x_{m}=x_{r}+L_{3}\cos\theta_{1}\\ y_{m}=y_{r}+L_{3}\sin\theta_{1}\end{array} \right. \tag{6}\]  
The position relationship between the vehicle reference point and the midpoint of the front axle of the tractor is expressed according to the geometric relationship:  
\[\left\{ \begin{array}{l}x_{f}=x_{r}+L_{1}\cos\theta_{1}\\ y_{f}=y_{r}+L_{1}\sin\theta_{1}\end{array} \right. \tag{7}\]  
The position relationship between the vehicle reference point and the midpoint of the rear axle of the semi-trailer is expressed according to the geometric relationship:  
\[\left\{ \begin{array}{l}x_{t}=x_{r}+L_{3}\cos\theta_{1}-L_{2}\cos\theta_{2}\\ y_{t}=y_{r}+L_{3}\sin\theta_{1}-L_{2}\sin\theta_{2}\end{array} \right. \tag{8}\]  
The longitudinal and lateral vehicle velocities at the vehicle reference point in the geodetic coordinate system can be expressed as:  
\[\left\{ \begin{array}{l}\dot{x}_{r}=v\cos\theta_{1}\\ \dot{y}_{r}=v\sin\theta_{1}\end{array} \right. \tag{9}\]  
The derivation of Equation (7) yields the velocity relationship at the midpoint of the front axle of the tractor:  
\[\left\{ \begin{array}{l}\dot{x}_{f}=\dot{x}_{r}-L_{1}\dot{\theta}_{1}\sin \theta_{1}\\ \dot{y}_{f}=\dot{y}_{r}+L_{1}\dot{\theta}_{1}\cos\theta_{1}\end{array} \right. \tag{10}\]  
Figure 1: Vehicle kinematic model. The derivation of Equation (8) yields the velocity relationship at the midpoint of the rear axle of the semi-trailer:  
\[ \begin{cases}\dot{x}_{t}=\dot{x}_{r}-L_{3}\dot{\theta}_{1}\sin\theta_{1}+L_{2} \dot{\theta}_{2}\sin\theta_{2}\\ \dot{y}_{t}=\dot{y}_{r}+L_{3}\dot{\theta}_{1}\cos\theta_{1}-L_{2}\dot{\theta}_{ 2}\cos\theta_{2}\end{cases}