# Trajectory Planning of a Semi-Trailer Train Based on Constrained Iterative LQR
## 3 Trajectory Iterative Solution  
### ILQR Algorithm  
The vehicle trajectory planning problem aims to find an optimal set of control profiles that minimize the cost function of the evaluated trajectory. Since a computer controls the physical object, the optimal control problem for solving the trajectory is discretized as follows:  
\[ \begin{split}& x^{*},u^{*}=\arg\,\min_{x,u}\left\{J(x_{N})+\sum \limits_{k=0}^{N-1}J^{k}(x_{k},u_{k})\right\}\\ & s.t.\ x_{k+1}=f^{k}(x_{k},u_{k}),\ \ k=0,1,\cdot\cdot\cdot,N-1\\ & x_{0}=x_{start}\\ & g^{k}(x_{k},u_{k})<0,\ k=0,1,\cdot\cdot\cdot,N-1\\ & g^{N}(x_{N})<0\end{split}  \tag{29}\]  
where \(J(x_{N})\) is the cost of the final state. \(J^{k}(x_{k},u_{k})\) is the stage cost at the time step \(k\). \(x_{k+1}=f^{k}(x_{k},u_{k})\) is the state transfer equation of the system. \(x_{0}=x_{start}\) is the initial state. \(g^{k}(x_{k},u_{k})<0\) is the various types of inequality constraints. The ILQR (iterative linear quadratic regulator) algorithm can solve optimal control problems for nonlinear systems without inequality constraints based on the LQR algorithm. The algorithm is characterized by fast solution speed and is one of the best algorithms for solving complex optimization problems. Omit the inequality constraints in Equation (29) to obtain the standard form that the iterative linear quadratic regulator can handle. \[ \begin{split}& x^{*},u^{*}=\arg\,\min_{x,u}\left\{J_{f}(x_{N})+ \sum\limits_{k=0}^{N-1}J^{k}(x_{k},u_{k})\right\}\\ & s.t.\ x_{k+1}=f^{k}(x_{k},u_{k}),\ \ k=0,1,\cdot\cdot\cdot,N-1\\ & x_{0}=x_{start}\end{split}  \tag{30}\]  
Figure 5: Boundary constraint. The ILQR is solved using the idea of DDP (differential dynamical programming), defining the Bellman equation as [33]:  
\[V^{k}(x_{k})=\min_{u_{k}}[J^{k}(x_{k},u_{k})+V^{k+1}(f^{k}(x_{k},u_{k}))] \tag{31}\]  
where \(V^{k}(x_{k})\) is the cost of the state profiles \(x_{k}\). The DDP to solve the ILQR can be divided into three steps: constructed standard form, backward pass, and forward pass. Step 1: Constructed Standard Form: In the first iteration, the initial trajectory is given as the optimization objective and is discretized. The initial trajectory contains the initial state profiles and the initial control profiles. During the subsequent iterations, the forward pass gives the optimization target trajectory (Step 3). The Taylor quadratic and Taylor linear expansions of the cost function and the state transfer equation are performed at the discrete points of the trajectory, respectively. A standard form that the ILQR can handle is obtained. Step 2: Backward pass: Since the target state is known, the value function for the last step can be found as follows:  
\[V^{N}(x_{N})=J(x_{N}) \tag{32}\]  
Define the perturbation term for each time step as follows:  
\[Q^{k}(\delta x_{k},\delta u_{k})=J^{k}(\bar{x}_{k}+\delta x_{k}, \bar{u}_{k}+\delta u_{k})-J^{k}(\bar{x}_{k},\bar{u}_{k})+V^{k+1}(f^{k}(\bar{x} _{k}+\delta x_{k},\bar{u}_{k}+\delta u_{k}))\] \[-V^{k+1}(f^{k}(\bar{x}_{k},\bar{u}_{k}))\]  
The perturbation term is calculated starting from step N-1 of the time step. Taylor quadratic expansion of the action value function at the discrete points of the trajectory:  
\[Q^{k}(\delta x_{k},\delta u_{k})=\frac{1}{2} \begin{bmatrix}1\\ \delta x_{k}\\ \delta u_{k}\end{bmatrix} ^{T} \begin{bmatrix}0&(Q^{k}_{x})^{T}&(Q^{k}_{u})^{T} \\ Q^{k}_{x}&Q^{k}_{xx}&Q^{k}_{xu}\\ Q^{k}_{xu}&Q^{k}_{ux}&Q^{k}_{uu}\end{bmatrix}\begin{bmatrix}1\\ \delta x_{k}\\ \delta u_{k}\end{bmatrix}