# Efficient Optimization-Based Trajectory Planning for Truck-Trailer Systems
## 2 Methods
### Proposed Solution
#### 2.3.4 Phase 4: Truck Dimensions and Collision Check  
The outputs of previous Phase 3 are feasible trajectories for the simplified model of the truck according to Equation (18). The goal of Phase 4 is to find trajectories with respect to the dimensions of the truck and the presence of obstacles. The results of the previous Phase 3 are used here as an initial estimate. The most significant change is that the path points are no longer considered in the cost function. Similar to Phase 3, the dynamics here relate to the model of the truck provided by Equation (18). Regarding the constraints, all angles defining the entire truck-trailer system's position are newly introduced. In this phase of the solution, collision of the truck with static obstacles placed between the initial and target points is newly detected. Collision detection consists of four sub-algorithms (Algorithms 1-4), as described below. The output \(q\) from the last one (Algorithm 4) enters the used cost function provided by Equation (25), which ensures the penalization of collisions between the truck and obstacles. The collision detection approach used here is based on the approximation of all objects by rectangles. Such a simplification is effective in the case of a truck with trailers, as real vehicles are frequently rectangular. Integrating potential collisions into the cost function is a very important part of the solution, and represents one of the main benefits of this paper. The procedure for detecting the collision of two quadrilaterals described by Equation (22) and Algorithms 1 and 2 is taken from [20]. The remaining parts of the collision detection (Algorithms 3 and 4) are our own contribution. Two quadrilaterals collide only if one or more vertices of one of the quadrilaterals is inside the other quadrilateral. We can reduce the collision problem to the question of whether the point \(K\) representing any vertex of one of the quadrilaterals is inside the other quadrilateral. If a collision occurs, Equation (22) must apply. We repeat this procedure for all four points of the quadrilateral. \[\left(0<\overrightarrow{AK}\cdot\overrightarrow{AB}<\overrightarrow{AB} \cdot\overrightarrow{AB}\right)\cap\left(0<\overrightarrow{AK}\cdot \overrightarrow{AD}<\overrightarrow{AD}\cdot\overrightarrow{AD}\right) \tag{22}\]  
Assuming that a regular quadrilateral has vertices \(A,B,C,D\), we can construct a function to detect its collision with point \(K\). The detection procedure is captured in Algorithm 1. ```
\(A,B,C,D\leftarrow\) obstacle points \(K\leftarrow\) point for which a collision is detected functionCollisionRectanglePoint\((A,B,C,D,K)\) \(ABAK\leftarrow\overrightarrow{AB}\cdot\overrightarrow{AK}\), \(ADAK\leftarrow\overrightarrow{AD}\cdot\overrightarrow{AK}\) \(ABAB\leftarrow\overrightarrow{AB}\); \(ABAB\leftarrow\overrightarrow{AB}\); \(ABAD\leftarrow\overrightarrow{AD}\cdot\overrightarrow{AD}\) \(\triangleright\) (\(0<\text{AMAB}<\text{ABAB}\)) \(\land\) (\(0<\text{AMAD}<\text{ADAD}\)) temp1 \(\leftarrow\) min(\(\text{ABAB}-\text{ABAK},\text{ABAK}\)) temp2 \(\leftarrow\) min(\(\text{ADAD}-\text{ADAK},\text{ADAK}\)) temp3 \(\leftarrow\) min(temp1,temp2) return\(\max(\text{temp3},0)\)\(\triangleright\) 0 \(\leftarrow\) no collision (otherwise positive value returned) \(\triangleright\) returns a value in case of a collision \(>\) 0 endfunction
```  
**Algorithm 1** Collision detection of a regular quadrilateral and a point  
Assuming that point \(K\) is one vertex of the quadrilateral \(K,L,M,N\), we can construct a function for detecting the collision of two regular quadrilaterals (Algorithm 2) based on the function for the collision of a quadrilateral and a point (Algorithm 1).