# Trajectory Optimization with Dynamic Drivable Corridor-Based Collision Avoidance
## 3 Collision Avoidance Constraint Formulation
### Dynamical Merging of Occupied Grids  
The obtained occupancy grids are dynamically merged to reduce the number of collision detection objects and computational complexity (c.f. Figure 5a). The merging process follows a structured approach, which is detailed in this subsection. First, as shown in Figure 5b, for the occupied grids in each column, they are merged according to specific conditions. The merging conditions are as follows: Within the same column, if two occupied grids overlap or are adjacent in the vertical direction, or if one occupied grid completely contains the other, they are merged into a larger occupied grid. Figure 5: Schematic illustration of occupied grids before and after merging: (**a**) before merging; (**b**) after vertical merging; and (**c**) after horizontal merging. Using this method, the number of objects involved in collision detection is reduced from the greater number of individual occupied grids before merging to a smaller number of merged grids, thereby significantly decreasing the computational complexity of collision detection and reducing the detection time. It is important to note that the merged occupancy grids remain AABBs, ensuring that the complexity of collision detection does not increase due to the merging process. The vertical merging process is shown in Algorithm 2. Algorithm 2 takes as input a set of occupied grid cells, labeled as Boxes, which represent obstacles in the environment. Meanwhile, Algorithm 2 outputs a vertically merged version of these grid cells, referred to as VBoxes. Execute the Sort() (c.f. line 2 in Algorithm 2) to sort all occupied grids according to specific rules: They are first sorted in ascending order based on the position of the left boundary, and for grids with the same left boundary, they are sorted by the height of the bottom edge in ascending order. As shown in lines 3-12 of Algorithm 2: Starting from the lowest occupied grid in each column, the algorithm sequentially checks upward and attempts to merge grids. If the currently occupied grid and the next grid satisfy the merging conditions (overlapping, adjacent, or containment in the vertical direction), the function MergeVerticalGrids() (c.f. line 6 in Algorithm 2) is executed to combine them into a larger occupied grid along the vertical direction. After merging, the algorithm continues to check upward from the newly formed occupied grid. If a grid that does not meet the merging conditions is encountered, the upward merging process restarts from that grid. This process is repeated until all occupied grids are processed. ```
Input: Boxes; Output: Vertically merged occupied grids VBoxes. 1. Initialize \(i=1\); VBoxes = \(\emptyset\);
2. Boxes \(\leftarrow\) Sort (Boxes);
3. while\(i\leq\) length (Boxes) do
4. \(j\gets i+1\);
5. while\(j\leq\) length (Boxes) \(\cap\) Boxes\({}_{i}\)\(x_{\min}\) = Boxes\({}_{j}\)\(\cdot\)\(x_{\min}\)\(\cap\) Boxes\({}_{i}\)\(x_{\max}\) = Boxes\({}_{j}\)\(\cdot\)\(x_{\max}\)\(\cap\) Boxes\({}_{i}\)\(\cdot\)\(y_{\max}\)\(\geq\) Boxes\({}_{j}\)\(\cdot\)\(y_{\min}\)do
6. Boxes\({}_{i}\)\(\leftarrow\) MergeVerticalGrids (Boxes\({}_{i}\), Boxes\({}_{j}\));
7. \(j\gets j+1\);
8. endwhile
9. VBoxes\({}_{i}\)\(\leftarrow\)\(j\);
10. \(i\gets j\);
11. endwhile
12. return VBoxes;
```  
**Algorithm 2** Vertical Merge of Occupied Grids  
Building upon the vertical merging process, further horizontal merging optimization is performed as illustrated in Figure 4(c). Merging conditions are defined as follows: Two occupied grids with identical boundaries in the vertical direction are merged into a larger grid if they are horizontally adjacent, overlapping, or if one grid fully contains the other. The horizontal merging process is shown in Algorithm 3. The horizontal merging procedure follows a workflow analogous to vertical merging (Algorithm 2).