# Efficient Optimization-Based Trajectory Planning for Truck-Trailer Systems
## 2 Methods
### Proposed Solution
#### 2.3.4 Phase 4: Truck Dimensions and Collision Check
```
\(A,B,C,D\leftarrow\) vertices of the first quadrilateral \(K,L,M,N\leftarrow\) vertices of the second quadrilateral \(k\equiv 0\) functionCollisionRectRect\((A,B,C,D,K,L,M,N)\) \(k\equiv k+CollisionRectanglePoint(A,B,C,D,K)\) \(k\equiv k+CollisionRectanglePoint(A,B,C,D,L)\) \(k\equiv k+CollisionRectanglePoint(A,B,C,D,M)\) \(k\equiv k+CollisionRectanglePoint(A,B,C,D,N)\) \(k\equiv k+CollisionRectanglePoint(K,L,M,N,A)\) \(k\equiv k+CollisionRectanglePoint(K,L,M,N,B)\) \(k\equiv k+CollisionRectanglePoint(K,L,M,N,C)\) \(k\equiv k+CollisionRectanglePoint(K,L,M,N,D)\) return\(k\) \(\triangleright\)\(k>0\leftarrow\) collision occurred endfunction
```  
**Algorithm 2** Collision detection of two regular quadrilaterals  
Multiple tests showed that detecting two quadrilaterals may be insufficient in terms of convergence; thus, we added assessment of the proximity between two quadrilaterals. This uses a logarithmic barrier function that penalizes situations when any part of the system is approaching obstacles. This procedure uses similar input information as the previous collision detection, and is captured in Algorithm 3. ```
\(A,B,C,D\leftarrow\) vertices of the first quadrilateral \(K,L,M,N\leftarrow\) vertices of the second quadrilateral \(\vec{Q}=[0,1,2,\ldots,15]\)\(\triangleright\) vector for the distances of all combinations of sides in two rectangles functionDistanceRctRct(\(A,B,C,D,K,L,M,N\)) \(Q[0]=MinDistnc(A,B,K,L);Q[1]=MinDistnc(A,B,K,N);Q[2]=MinDistnc(A,B,M,N);Q[3]=MinDistnc(A,B,M,L)\) \(Q[4]=MinDistnc(A,D,K,L);Q[5]=MinDistnc(A,D,K,N)\) \(Q[6]=MinDistnc(A,D,M,N);Q[7]=MinDistnc(A,D,M,L)\) \(Q[8]=MinDistnc(C,B,K,L);Q[9]=MinDistnc(C,B,K,N)\) \(Q[10]=MinDistnc(C,B,M,N);Q[11]=MinDistnc(C,B,M,L)\) \(Q[12]=MinDistnc(C,D,K,L);Q[13]=MinDistnc(C,D,K,N)\) \(Q[14]=MinDistnc(C,D,M,N);Q[15]=MinDistnc(C,D,M,L)\) \(q=-W_{1}\cdot\log(min(\vec{Q}))\) return\(q\) endfunction
```  
**Algorithm 3** Proximity detection of two regular quadrilaterals  
The combination of previous collision detections is then captured in Algorithm 4. The output variable \(q\) from this algorithm is used in the cost function of the current phase. ```
\(j\gets 0\) \(q\gets 0\) for\(i=1,2,\ldots,\)number of obstacles do \(k=1,2,\ldots,\)the number of vehicles of the system do \(j=j+CollisionRctRct(\text{obstacle points}_{i},\text{vehicle points}_{k})\) \(q=q+DistanceRctRct(\text{obstacle points}_{i},\text{vehicle points}_{k})\) endfor endfor if\(q\geq q_{inf}\)then \(q=W_{2}\cdot j\) endif return\(q\)
```  
**Algorithm 4** Resulting detection algorithm  
Here, the parameter \(q_{inf}\) represents either an infinitely large value, the maximum value feasible within a given software environment, or a user-defined value. Regarding parameters \(W_{1}\) and \(W_{2}\), we chose \(W_{1}=0.001\) and \(W_{2}=10\). These values represent weighting factors used for scaling the optimization task. Real-world optimization problems often comprise parameters of vastly different orders of magnitudes. In many applications, parameters with very large start values will vary over a wide range, and a change in that parameter will only lead to a relatively small change in the criterion function. If this is the case, the scaling of the optimization problem can be improved by some measures, usually based on heuristics. There are several approaches, such as simply dividing all parameter vectors by the start parameters or re-mapping all parameters such that the bounds are \([0,1]\) for all parameters. For the case study presented here, we used expert choice of these values. Additional tuning or applying a more sophisticated mechanism to determine these parameters might be needed in future modifications of the presented case study. The system model and constraints stay the same as in Phase 3. Because the truck is no longer treated as a point in this phase, but rather as a quadrilateral in space, it makes sense to penalize the vehicle's initial and target angular positions.