# Efficient Optimization-Based Trajectory Planning for Truck-Trailer Systems
## 2 Methods
### Proposed Solution  
The proposed solution suggests a six-phase procedure combining global optimization for path planning (Phase 1) and local optimization for trajectory planning (Phases 2-6).  
The problem of trajectory planning considered in this paper can be formulated as continuous optimal control problem (OCP) in Bolza form; see Equation (3):  
Figure 2: System of a truck and two trailers.  
\[J=\Phi[t_{0},t_{F},\mathbf{x}(t_{0}),\mathbf{x}(t_{F})]+\int_{t_{0}}^{t_{F}}L[\mathbf{x}(t),\mathbf{ u}(t),t]\mathrm{d}t. \tag{3}\]  
The original OCP is converted to a general nonlinear programming problem (NLP) in the form of Equation (4), described as finding an optimal solution \(\mathbf{z}^{*}\) which minimizes the cost function \(J\) depending on the decision vector variable \(\mathbf{z}\) which contains both control inputs and states. We find its optimal value, provided as Equation (4):  
\[\mathbf{z}^{*}=\operatorname*{arg\,min}_{\mathbf{z}}\{J(\mathbf{z})\}. \tag{4}\]  
The optimization task includes the nonlinear equality constraints in Equation (5), the nonlinear inequality constraints in Equation (6), and the simple lower and upper bounds in Equation (7):  
\[\mathbf{c}_{eq}(\mathbf{z})=\mathbf{0} \tag{5}\]  
\[\mathbf{c}(\mathbf{z})\leq\mathbf{0} \tag{6}\]  
\[\mathbf{z}_{l}\leq\mathbf{z}\leq\mathbf{z}_{u}. \tag{7}\]  
The set of equality constraints \(\mathbf{c}_{eq}\) corresponding to Equation (5) is composed of equality boundary constraints \(\mathbf{\psi}\) and equality differential defect constraints \(\mathbf{\xi}\), as follows:  
\[\mathbf{c}_{eq}=\begin{bmatrix}\mathbf{\psi}\\ \mathbf{\xi}\end{bmatrix}. \tag{8}\]  
Converting the OCP into an NLP is achieved through a direct transcription discretization scheme using the fourth-order Runge-Kutta numerical method (RK4) in the form of Equation (9). To remain consistent with notation in Equations (5) and (8), we express Equation (9) as the set of differential defect constraints provided in Equation (10), where \(k\in[0,N-2]\in\mathbb{N}^{0}\) stands for the current step of the computation, \(N\) is the number of nodes, \(k_{1}\), \(k_{2}\), \(k_{3}\), and \(k_{4}\) are determined by Equation (11), and \(h\) represents the sampling period.  
\[\mathbf{x}_{\mathbf{k+1}}=\mathbf{x}_{\mathbf{k}}+\frac{1}{6}\cdot h(\mathbf{k}_{1}+2\cdot\mathbf{k}_ {2}+2\cdot\mathbf{k}_{3}+\mathbf{k}_{4}) \tag{9}\]  
\[\mathbf{\xi}_{k}=\mathbf{x}_{k+1}-\mathbf{x}_{k}-\frac{1}{6}\cdot h\cdot(\mathbf{k}_{1}+2\cdot \mathbf{k}_{2}+2\cdot\mathbf{k}_{3}+\mathbf{k}_{4}) \tag{10}\]  
\[\mathbf{k}_{1}=f(\mathbf{x}_{k},\mathbf{u}_{k}) \tag{11}\] \[\mathbf{k}_{2}=f(\mathbf{x}_{k}+\frac{h}{2}\cdot\mathbf{k}_{1},\frac{\mathbf{u}_{ k+1}+\mathbf{u}_{k}}{2})\] \[\mathbf{k}_{3}=f(\mathbf{x}_{k}+\frac{h}{2}\cdot\mathbf{k}_{2},\frac{\mathbf{u}_{ k+1}+\mathbf{u}_{k}}{2})\] \[\mathbf{k}_{4}=f(\mathbf{x}_{k}+h\cdot\mathbf{k}_{3},\mathbf{u}_{k+1})\]  
Altogether, the solution of both path planning and trajectory planning that we propose in this paper consists of the six phases introduced in Table 1, which are each described in detail in separate subsections.  
\begin{table}
\begin{tabular}{c l} \hline \hline
**Phase** & **Phase Identification** \\ \hline
1 & Path Planning \\
2 & Trajectory Planning with Constant Velocity Profile \\
3 & Velocity Profile Planning \\
4 & Truck Dimensions and Collision Checking \\
5 & Adding Trailers \\
6 & Fine-Tuning the Trajectories \\ \hline \hline \end{tabular}
\end{table}
Table 1: The six phases of the proposed solution.