# Trajectory Planning of a Semi-Trailer Train Based on Constrained Iterative LQR
## 1 Introduction
This framework can achieve faster and smoother spatiotemporal trajectory planning under the precondition of satisfying various types of constraints. [PERSON] et al. [16] proposed a trajectory planning algorithm considering the obstacle avoidance needs of vehicles by combining the A* algorithm, the artificial potential field method, and model predictive control. The algorithm ensures the safety and stability of vehicle travel. [PERSON] et al. [17] proposed a two-tier trajectory planning framework to solve the trajectory planning problem of self-driving cars on unstructured roads. In the first layer, the hybrid A* algorithm is improved by adding constraints between nodes to obtain an initial solution. In the second layer, the smoothness of the planned trajectory is further enhanced by solving the optimization problem with spatial constraints. [PERSON] et al. [18] combined the potential field method with model predictive control to address the problem that the possible field-based trajectory planning method does not consider passenger ride comfort. A human-centered dynamic trajectory optimization method for autonomous vehicles is proposed that considers the safety and comfort of trajectory planning. [PERSON] et al. [19] proposed a trajectory planning method that combines vehicle accessibility analysis with convex optimization to improve the efficiency of vehicle trajectory planning in narrow and complex scenarios. [PERSON] et al. [20] integrate the advantages of sampling-based and optimization-based trajectory planning algorithms. A trajectory planning method is proposed using sampling-based methods for lateral vehicle motion and numerical optimization-based methods for longitudinal movement. The safety of planning trajectories in complex, dynamic scenes is improved. [PERSON] et al. [21] describe the trajectory planning problem of self-driving cars as an optimal control problem. The nonlinear constraints are handled by establishing spatiotemporal safety corridors, and a lightweight iterative computational framework is constructed to solve the nonlinear planning problem. [PERSON] et al. [22], to minimize the risk of vehicle collision, considered the uncertainty in the driving behavior of ambient vehicles and solved the trajectory by ILQR. [PERSON] [23] proposes a Reactive ILQR (RILQR) algorithm, which, for the first time, clarifies how self-driving cars should react in real-time to changes in future driving scenarios and improves the flexibility of trajectory planning. It makes the trajectory planning method based on vehicle accessibility analysis feasible in complex traffic scenarios. The literature [24; 25; 26]proposes a path-planning method using a geofencing-based approach that can plan safe flight paths for UAVs in 3D space by solving an optimization problem. Most of the current research on autonomous vehicle trajectory planning is based on two-axle vehicles, and the planning algorithms designed for two-axle vehicles cannot be directly applied to semi-trailer trains [27]. This article uses an optimization-based approach to deal with the trajectory planning problem of semi-trailer trains. The objective function and constraints are designed for the unique body structure of the semi-trailer train, and the CILQR algorithm is used to solve the trajectory. The algorithm proposed in this article can efficiently plan the trajectory for the semi-trailer train in complex scenarios. The rest of the article is structured and organized as follows: Section 2 designs the cost function and various types of constraints. The cost function is used to represent the cost of the planned trajectory, and the constraints are the various types of constraints that need to be satisfied when the semi-trailer train is traveling. The cost function and the various constraints constitute the optimization problem for solving the optimal trajectory. Section 3 constructs the standard form of the CILQR algorithm by transforming the constraints of the optimization problem into an interior point penalty function.