# Efficient Optimization-Based Trajectory Planning for Truck-Trailer Systems
## 2 Methods
### Proposed Solution
#### 2.3.6 Phase 6: Fine-Tuning of the Trajectories  
In order to find optimal trajectories in the shortest possible computational time, and consequently lower the dimension of the decision vector, a small number of nodes are considered for the discretization scheme. The bottleneck in this approach is the possibility of finding trajectories that collide with an obstacle even though the optimization algorithm considers them collision-free. To suppress this problem, the final Phase 6 is added to the solution, during which the number of nodes is considerably increased and the optimization problem is solved again. Setting the weighting factor \(W_{1}\) in Algorithm 3 can significantly reduce the risk of finding collision trajectories. When collisions are penalized in the cost function, the constraints can be characterized as either \"soft\" or \"hard\", and there is no absolute guarantee of non-collision trajectories. The primary reason for potential collision is due to by discretization process; therefore, we double the number of nodes here. Because the initial estimate is based on the previously-found optimal trajectories, the computation time needed here is significantly reduced. A larger number of nodes where the found optimal trajectories are defined is also beneficial, and is more effective for the trajectory tracking problem because the sampling period becomes smaller. Reducing the sampling period increases the number of time points at which control inputs and trajectories are determined. In this phase, an initial estimate is obtained by interpolating with cubic splines based on the solution from the previous phase. First, the resulting optimal decision vector must be decomposed into individual variables before applying interpolation. Additionally, it is necessary to ensure that the interpolated points remain within the range specified by the simple bounds in Equation (17), as interpolation can sometimes lead to minor violations of these bounds. An example of how to apply this correction for \(x_{1}\) is provided in Equations (30) and (31), where \(x_{1_{\text{max}}}\) and \(x_{1_{\text{min}}}\) represent the simple bounds for this variable. A tolerance threshold of 0.001 can be used to enforce these bounds, which is essential because the solver requires the initial guess to meet the constraints on the design variables. During interpolation, newly determined points may not inherently satisfy these limits. \[x_{1_{\text{n}}}=\left\{ \begin{aligned} x_{1_{\text{n}}},& x_{1_{\text{n}}}<x_{1_{\text{max}}}\\ x_{1_{\text{max}}}-0.001,& x_{1_{\text{n}}}\geq x_{1_{ \text{max}}}\end{aligned} \right. \tag{30}\]  
\[x_{1_{\text{n}}}=\left\{ \begin{aligned} x_{1_{\text{n}}},& x_{1_{\text{n}}}>x_{1_{\text{min}}}\\ x_{1_{\text{min}}}+0.001,& x_{1_{\text{n}}}\leq x_{1_{ \text{min}}}\end{aligned} \right. \tag{31}\]  
The system's model remains the same as in the previous phase, and is again described by Equation (2). The constraints on the initial state provided by Equation (26) and the differential defect constraints for fulfilling the dynamics according to Equation (10) also remain unchanged. The form of the cost function is based on the previous step, but is extended by suppressing the difference between the original and newly searched trajectories (between Phase 5 and Phase 6). The implementation of this penalty is complicated by the different lengths of the decision vector \(\vec{z}\), where the original one is always shorter than the current one. In order to compute the difference, a newly defined initial estimate \(\vec{z_{0}}\) is used which has the same length as the searched vector. In order to prevent some values from being unintentionally penalized against the values found by the spline function, a newly introduced weighting vector \(\vec{z_{w}}\) is introduced to indicate the positions for difference computation. It contains values of 1 at positions where the difference is to be computed and zeros at positions where the difference is not required.