# Efficient Optimization-Based Trajectory Planning for Truck-Trailer Systems
## 2 Methods
### Proposed Solution
#### 2.3.4 Phase 4: Truck Dimensions and Collision Check
Furthermore, the target position of the truck's reference point is no longer penalized; instead, all truck vertices are required to match within the target area. The rotation angle at the initial point is an extension of the equality constraints for the initial state provided by Equation (23). Adding them allows us to find feasible trajectories even if the truck is not aligned precisely with the \(x\)-axis at the beginning of movement. This assumption, which was used in the previous phases, is replaced here by specifying the desired initial rotation of the truck with respect to the \(x\)-axis. \[\psi= \begin{bmatrix}x_{1_{1}}\\ x_{2_{1}}\\ x_{3_{1}}\\ x_{4_{1}}\\ x_{5_{1}}\end{bmatrix} - \begin{bmatrix}x_{0_{1}}\\ x_{0_{2}}\\ x_{0_{3}}\\ x_{0_{4}}\\ x_{0_{5}}\end{bmatrix}  \tag{23}\]  
The final rotation and position of the truck is written in the form of an inequality constraint, which is less strict, introduces some relaxation for the optimization task, and improves the existence and convergence likelihood of the solution. The inequality constraints defined in Equation (24) represent a 2D region in which \(X\in[-6;6]\) and \(Y\in[-1.5;1.5]\) [m]. In the first eight constraints, all vertices of the truck (modeled as a rectangle with vertices \(K,L,M,N\)) need to be located within the desired target region. The last constraint concerns the angle of rotation of the truck. Because the endpoints of the truck are not included in the vector of state variables of the dynamics, we need to compute their coordinates at the final time point \((K_{x_{if}},K_{y_{if}},L_{x_{if}},L_{y_{if}},M_{x_{if}},M_{y_{if}},N_{x_{if}},N _{y_{if}})\) in the same way as in the case of collision detection with obstacles. Compared to the previous phase, the target area is slightly enlarged, meaning that its dimensions are larger than the dimensions of the system. \[ \begin{split} x_{tf_{1}}-6\leq& K_{x_{tf}}\leq x_{ tf_{1}}+6\\ x_{tf_{1}}-6\leq& L_{x_{tf}}\leq x_{tf_{1}}+6\\ x_{tf_{1}}-6\leq& M_{x_{tf}}\leq x_{tf_{1}}+6\\ x_{tf_{1}}-6\leq& N_{x_{tf}}\leq x_{tf_{1}}+6\\ x_{tf_{2}}-1.5\leq& K_{y_{tf}}\leq x_{tf_{2}}+1.5\\ x_{tf_{2}}-1.5\leq& L_{y_{tf}}\leq x_{tf_{2}}+1.5\\ x_{tf_{2}}-1.5\leq& M_{y_{tf}}\leq x_{tf_{2}}+1.5\\ x_{tf_{2}}-1.5\leq& N_{y_{tf}}\leq x_{tf_{2}}+1.5\\ x_{tf_{4}}-\frac{pi}{2}\leq& x_{t_{N}}\leq x_{tf_{4}}+ \frac{pi}{2}\end{split}  \tag{24}\]  
The cost function can already fulfill the original requirement expressed by Equation (25) of minimizing the distance traveled and energy spent on the control action; furthermore, we retain the variable \(q\), ensuring that collision between the truck-trailer system and obstacles are penalized. \[ \begin{split} J=& 10\cdot\frac{1}{N}\cdot\left(\frac{ 1}{2}(u_{1_{1}}-u_{1_{2}})^{2}+\sum_{n=2}^{N-2}(u_{1_{n}}-u_{1_{n+1}})^{2}+\frac {1}{2}(u_{1_{N-1}}-u_{1_{N}})^{2}\right)+\\ +& 10\cdot\frac{1}{N}\cdot\left(\frac{1}{2}(u_{2_{1}}-u_ {2_{2}})^{2}+\sum_{n=2}^{N-2}(u_{2_{n}}-u_{2_{n+1}})^{2}+\frac{1}{2}(u_{2_{N-1 }}-u_{2_{N}})^{2}\right)+\\ +&\sum_{n=1}^{N-1}\sqrt{\left(x_{1_{n}}-x_{1_{n+1}} \right)^{2}+\left(x_{2_{n}}-x_{2_{n+1}}\right)^{2}}+q\end{split}  \tag{25}\]