# Trajectory Planning of a Semi-Trailer Train Based on Constrained Iterative LQR
## 3 Trajectory Iterative Solution
### CILQR Algorithm  
The trajectory planning process for autonomous vehicles involves complex constraints beyond the solving power of ILQR. CILQR (constrained iterative linear quadratic regulator) is based on ILQR to convert inequality constraints into some form of constraint function. Then, the constraint function Taylor quadratic expansion is added to the cost function of Equation (30) to construct the interior point penalty function. The problem that ILQR cannot handle inequality constraints is solved. Assume that the inequality constraints that autonomous vehicles have to deal with when performing trajectory planning are:  
\[c_{\text{min}}\leq c_{k}^{x}\leq c_{\text{max}} \tag{39}\]  
If \(c_{x}^{k}\) is a nonlinear function, a Taylor linear expansion is performed on it. The inequality constraint is constructed into the equation form:  
\[ \begin{cases}f_{1k}^{x}=c_{k}^{x}-c_{\text{max}}\\ f_{2k}^{x}=c_{\text{min}}-c_{k}^{x}\end{cases}  \tag{40}\]  
Construct a constraint function in the form of an exponential function using the value of the equation as the independent variable:  
\[b_{k}^{x}=q_{1}\cdot\exp(q_{2}\cdot f_{ik}^{x}),\ \ \ i=1,2 \tag{41}\]  
where \(q_{1}\) and \(q_{2}\) are constant coefficients, and when \(q_{1}\) increases, it increases the overall constraint function value. When \(q_{2}\) increases, the curve of the exponential function becomes steeper, which also increases the value of the constraint function. By adjusting the values of \(q_{1}\) and \(q_{2}\), a curve with properties more in line with the demand can be obtained. As shown in Figure 6, the ideal constraint function curve should be a very steep exponential function curve. That is, when the independent variable is less than 0, the function value is 0, and when the independent variable is greater than 0, the function value tends to be infinitely great. Taylor quadratic expansion of the constraint function at discrete trajectory points:  
\[b_{k}^{x}(x_{k}+\delta x_{k})\approx\delta x_{x}^{T}\
abla^{2}b_{k}^{x}(x_{k} )\delta x_{k}+\delta x_{x}^{T}\
abla b_{k}^{x}(x_{k})+b_{k}^{x}(x_{k}) \tag{42}\]  
According to the discrete trajectory points, the expanded constraint function is brought into the corresponding cost function of Equation (30), and the constraint function is constructed as an interior point penalty function to obtain the CILQR algorithm. The CILQR algorithm solves the problem that the ILQR algorithm cannot deal with inequality constraints. The solution flow of the CILQR algorithm is shown in Figure 7.  
Taking obstacle avoidance constraints as an example, the specific judgment methods are as follows:  
When dealing with obstacle avoidance constraints, the position relationship between the circle center and the expansion ellipse is judged by the following equation:  
\[f_{i}^{x}(x_{i},y_{i})=1- \begin{bmatrix}x_{i}-x_{0}\\ y_{i}-y_{0}\end{bmatrix} ^{T}P \begin{bmatrix}x_{i}-x_{0}\\ y_{i}-y_{0}\end{bmatrix}  \tag{43}\]  
where  
\[P= \begin{bmatrix}\frac{\cos^{2}\theta}{a^{2}}+\frac{\sin^{2}\theta}{b^{2}}& \frac{\cos\theta\sin\theta}{a^{2}}+\frac{\cos\theta\sin\theta}{b^{2}}\\ \frac{\cos\theta\sin\theta}{a^{2}}+\frac{\cos\theta\sin\theta}{b^{2}}&\frac{ \sin^{2}\theta}{a^{2}}+\frac{\cos^{2}\theta}{b^{2}}\end{bmatrix}  \tag{44}\]  
In this equation, \((x_{i},y_{i})\) is the coordinates of the center of one of the circles modeled for the ego-vehicle, \((x_{0},y_{0})\) is the center point of the dynamically expanding ellipse, and \(\theta\) is the yawing angle of the dynamically expanding ellipse. Figure 6: Ideally constrained function curve. Figure 7: CILQR algorithm flow. To reduce the computational effort, a coordinate system conversion method is used to calculate the new coordinates of the circle center in the positive elliptic coordinate system by rotating the translation matrix. Taking the oblique ellipse with an angle \(\theta\) to the \(X\)-axis in Figure 8 as an example, the position transformation matrix \(M_{t}\) of the center of the circles is:  
\[M_{t}=