# Trajectory Optimization with Dynamic Drivable Corridor-Based Collision Avoidance
## 3 Collision Avoidance Constraint Formulation
### Occupied Grid Formation for Polygonal Obstacles  
For polygonal obstacles, sampling along their boundaries is a convenient approach. By extracting sample points from each edge and applying the grid-based representation method introduced in the previous section, the resulting occupied grid map can be obtained (c.f. Figure 4a). However, this approach only represents the boundaries of the polygon and fails to effectively capture its interior region. When dealing with large polygonal obstacles, a target entering the interior may not be detected in time, posing potential safety risks. To address this issue, we propose an approach that ensures complete coverage of both the  
Figure 3: Grid map. boundary and interior regions (c.f. Figure 4b) by sampling only the polygon edges. The implemented algorithm is presented in Algorithm 1. The input of Algorithm 1 includes the polygonal obstacle information and the initial trajectory. The output of Algorithm 1 is the set of occupied grid cells Boxes that cover the polygonal obstacle. The core of Algorithm 1 lies in the sampling, determining the grid map boundaries, discretizing obstacle coordinates into grid cells, and obtaining the complete set of occupied grids covering the polygonal obstacles. First, the function Sample() (c.f. line 2 in Algorithm 1) is executed to perform uniform sampling along each edge of the polygon, generating a series of discrete points to ensure comprehensive coverage of the obstacle boundaries. Subsequently, the function DetermineBoundary() (c.f. line 3 in Algorithm 1) computes and establishes the grid map boundaries based on (9), ensuring that all relevant obstacle data are correctly projected onto the grid representation. Following this, for each sampled point that lies within the grid map boundaries, the algorithm applies a grid discretization process, mapping the obstacle edges to the corresponding grid cells and thereby obtaining the occupied grid cells that represent the polygonal obstacle boundaries, as illustrated in Figure 4a. Additionally, the algorithm computes and stores the indices of these occupied grid cells within the grid map to facilitate subsequent computations (c.f. lines 4-9 in Algorithm 1). Next, the column index set of the occupancy grid is traversed, and for each column, the following operations are performed: determine the minimum and maximum row positions of the occupied grids in that column and then connect the occupied grids at the minimum and maximum row positions vertically to form a new occupied grid block (c.f. lines 10-17 in Algorithm 1). As shown in Figure 4b, the new occupied grids generated by this method not only cover the polygon boundary but also effectively cover its interior region. Additionally, the number of occupied grids is reduced from 62 in Figure 4a to 14 in Figure 4b, significantly decreasing the number of collision detection targets. Each polygonal obstacle in the environment is processed individually using the aforementioned method, and the resulting occupancy grids are stored in a container. These grids are then used to replace the obstacles in the environment for subsequent collision detection during the DC construction process. Figure 4: Comparative analysis of polygonal obstacle grid discretization under different algorithmic settings and resolutions (blue grid cells indicate obstacle-occupied areas in the grid map): (**a**) before improvement, resolution = 0.4 m; (**b**) after improvement, resolution = 0.4 m; and (**c**) after improvement, resolution = 0.1 m.  
```
Input:\(obs\), initialpath; Output: Boxes. 1. Initialize Boxes = \(\O\); Rows = \(\O\); Cols = \(\O\); define resolution;
2. \(points\leftarrow\) Sample (\(obs\));
3. \(x_{\min}\), \(x_{\max}\), \(y_{\min}\), \(y_{\max}\leftarrow\) DetermineBoundary (initialpath);
4. for each \(point\in points\)do
5. if\(point.x\geq x_{\min}\) and \(point.x\leq x_{\max}\) and \(point.y\geq y_{\min}\) and \(point.y\leq y_{\max}\), then
6. \(Row\), \(Col\leftarrow\) ObstacleToGridIndex (\(x_{\min}\), \(x_{\max}\), \(y_{\min}\), \(y_{\max}\), resolution);
7.