# 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
\quad i=1,\;2 \tag{16}\]  
The radius of a single circle is:  
\[R^{disc}=\frac{1}{2}\sqrt{\left(\frac{L_{1f}+L_{1}+L_{1r}}{2}\right)^{2}+(L_{ W})^{2}} \tag{17}\]  
For a circle modeled as a semi-trailer body, the coordinates of the position of the center of the circle can be expressed according to the coordinates of the reference point and the vehicle parameters:  
\[ \begin{cases}x_{i}(t)=x_{m}(t)-\left[L^{\prime}+\frac{2i-5}{6}(L_{1f}+L_{1}+L _{1r})\right]\cos\theta_{2}(t)\\ y_{i}(t)=x_{m}(t)-\left[L^{\prime}+\frac{2i-5}{6}(L_{1f}+L_{1}+L_{1r})\right] \sin\theta_{2}(t)\end{cases} \quad i=3,\;4,\;5 \tag{18}\]  
where  
\[L^{\prime}=L_{2}+L_{2r}-\frac{3(L_{1f}+L_{1}+L_{1r})}{2} \tag{19}\]  
In modeling the ambient vehicle, considering the moving car's sizeable longitudinal speed and low lateral speed, the longitudinal distance between the ego-vehicle and the ambient vehicle should be fully considered when dealing with the obstacle avoidance constraint. So, the ambient vehicle is modeled as an ellipse. The ellipse's long axis coincides with the vehicle's longitudinal axis, and the short axis coincides with the transverse axis. To meet the demand for obstacle avoidance in high-speed conditions, the ellipse for modeling ambient vehicles is therefore designed as a dynamic ellipse that varies with vehicle speed, considering the effect of vehicle speed on obstacle avoidance capability. The ellipse's long axis is designed as a function of the vehicle speed [31] to achieve the purpose that the higher the vehicle speed, the longer the long axis of the ellipse, the larger the obstacle area, and the lower the probability of collision. The ambient vehicle modeling is shown in Figure 3.  
Figure 3: Ambient vehicle dynamic elliptic modeling. The long and short axes of the dynamic ellipse are denoted as:  
\[a=\frac{L}{2}+safebound_{a}+v_{a}\cdot safebound_{t} \tag{20}\]  
\[b=\frac{W}{2}+safebound_{b} \tag{21}\]  
where \(L\) is the body length of the ambient vehicle, \(W\) is the width of the body, \(v_{a}\) is the vehicle speed of the modeled vehicle, \(safebound_{a}\) is the longitudinal safety redundancy distance, \(safebound_{b}\) is the lateral safety redundancy distance, and \(safebound_{t}\) is the safety redundancy time. Since the outer contour of the ego-vehicle and the ambient vehicle are represented quantitatively by geometry, judging whether there is an overlap between the two can determine whether a collision has occurred. By putting the center of the circle around one week along the dynamic ellipse boundary, the outermost motion trajectory of the circle forms a new ellipse. Define the new ellipse as a dynamically expanded ellipse, and the dynamically expanded ellipse is equivalent to adding a circle radius to the dynamic ellipse. This is shown in Figure 4.  
The long and short axes of the dynamically expanding ellipse are denoted as:  
\[a_{1}=\frac{L}{2}+safebound_{a}+v_{a}\cdot safebound_{t}+R^{disc} \tag{22}\]  
\[b_{1}=\frac{W}{2}+safebound_{b}+R^{disc} \tag{23}\]  
In this case, the center of the circle modeled by the ego-vehicle is sought based on the reference point on the planned trajectory. Collision detection is performed by determining the relative positional relationship between the center of the circle and the dynamically expanded ellipse. When the circle's center lies inside the expanded ellipse, a collision occurs, and the collision constraints are not satisfied. The obstacle avoidance constraint is satisfied when the circle's center lies outside the expanded ellipse.