# Trajectory Planning of a Semi-Trailer Train Based on Constrained Iterative LQR
## 2 Constructing Trajectory Planning Problems
### Constraint Condition
#### 2.2.3 Obstacle Avoidance Constraint  
Obstacle avoidance constraints for autonomous vehicles are dynamic constraints that avoid collisions with obstacles while the vehicle is in motion. The constraint is usually to limit the minimum safe distance of the vehicle from the obstacle. To ensure vehicle driving safety, autonomous vehicles need to continuously update their environment perception results and adjust the driving strategy based on information such as obstacle location, speed, and size to avoid collisions with obstacles. Autonomous vehicles drive with onboard sensors (lidar, cameras, radar, etc.) to obtain information about their environment. When an obstacle is detected, an obstacle boundary is generated based on the external dimensions of the obstacle [30]. The various static and dynamic obstacles on the road then impose constraints on the trajectory planning of the ego-vehicle. In describing obstacle avoidance constraints, a quantitative representation of the body contours of the ego-vehicle and the ambient vehicle is required. The body is usually modeled using a geometric-shaped overlay envelope. Due to the long body of the semi-trailer train and the articulated connection between the tractor and the semi-trailer, this article uses circles to model the outer contour of the semi-trailer train. The number of circles determines the degree of accuracy of the description of the body outline. The more circles, the smaller the radius of individual circles and the more accurate the definition of vehicle contours, but it will increase the computational effort of collision detection. The smaller number of circles is beneficial to reduce the computation of collision detection and improve real-time trajectory planning. Still, a large radius of a single circle leads to a long lateral safety distance for the vehicle, which will reduce its passability in a narrow space. Therefore, when choosing the number of circles, there is a trade-off between the computational solution's real-time, safety, and passability. Based on the comprehensive analysis of the actual body size and structure and considering the complexity of the calculation and the accuracy of the description of the body contour, the tractor is modeled with two circles, and the semi-trailer is modeled with three equal-radius circles. Since the front part of the semi-trailer overlaps with the rear part of the tractor, the motion range of the front part of the semi-trailer body is within the area covered by the circle at the rear of the tractor, so the head region of the semi-trailer is no longer modeled separately. In semi-trailer modeling, three circles equal to the radius of the tractor-trailer are used to cover the body modeling from back to front uniformly. The vehicle body modeling is shown in Figure 2.  
As shown in Figure 2, \(L_{1f}\) is the front overhang of the tractor, \(L_{1r}\) is the rear overhang of the tractor, and \(L_{2f}\) is the distance between the articulation point and the frontest edge of the semi-trailer. Additionally, \(L_{2r}\) is the distance from the rear ax  
Figure 2: Vehicle body modeling. the trailing edge, \(L_{W}\) is the width of the semi-trailer train, and \((x_{i},y_{i})\) denotes the center of the \(i\)th circle. The letter \(i\) is used to denote the serial number of the circles, ordered from the head to the tail of the vehicle. \(i=1,2\) are the two circles modeled for the tractor, and the coordinates of the circle center position can be expressed according to the reference point and body parameters as:  
\[ \begin{cases}x_{i}(t)=x_{r}(t)+\left[\frac{5-2i}{4}(L_{1f}+L_{1}+L_{1r})-L_{1r} \right]\cos\theta_{1}(t)\\ y_{i}(t)=y_{r}(t)+\left[\frac{5-2i}{4}(L_{1f}+L_{1}+L_{1r})-L_{1r}\right]\sin \theta_{1}(t)\end{cases}