Point-to-Occupancy Grid Collision Avoidance

In trajectory planning, an Occupancy Grid Map is a commonly used environment representation method. It divides the environment into discrete grids, where each grid cell stores information about whether that area is occupied by an obstacle. The point-to-occupancy grid collision avoidance feature enables collision avoidance constraints between a point and an occupancy grid, ensuring that the point does not enter areas occupied by obstacles.

Modeling

Below is an example of a point-to-2D occupancy grid collision avoidance inequality constraint:

  occupancy_map = occupancy_map_2d(name, length, width, resolution, origin_x, origin_y, origin_phi)
  ineq = point_to_occupancy_map_inequality(occupancy_map, px, py, distance_to_avoid)
  prob.inequality(ineq)

First, define a 2D occupancy grid using the occupancy_map_2d interface. The parameters of the occupancy_map_2d interface are as follows:

• name: Name of the occupancy grid
• length, width: Number of cells in the length and width directions of the occupancy grid (unit: cell), must be integers greater than 1
• resolution: Resolution of the occupancy grid (unit: m/cell), can be a constant or an expression in terms of parameter $p$
• origin_x, origin_y, origin_phi: Pose of the occupancy grid origin (lower-left corner as the origin, counterclockwise is positive), can be constants or expressions in terms of parameter $p$

Then, define the point-to-2D occupancy grid collision avoidance inequality constraint using the point_to_occupancy_map_inequality interface. Its parameters are as follows:

• occupancy_map: A 2D occupancy grid defined using the occupancy_map_2d interface
• px, py: Coordinates of the point that needs collision avoidance, expressions in terms of optimization variable $v$
• distance_to_avoid: Avoidance distance, can be a constant, an expression in terms of parameter $p$, or an expression in terms of optimization variable $v$

Solving

Occupancy grid data is stored in the prob struct. Before solving, you need to set the occupancy grid data and compute the distance field (Euclidean Signed Distance Field, ESDF). Below is a C/C++ code example (assuming the occupancy grid name is static, with size 6x6):

/* set occupancy map data */
unsigned char occ[36] = {
    0, 0, 0, 0, 0, 0,
    0, 1, 1, 0, 0, 0,
    0, 1, 1, 1, 0, 0,
    0, 1, 1, 1, 0, 0,
    0, 1, 1, 1, 0, 0,
    0, 0, 0, 0, 0, 0
};

for (size_t row = 0; row < 6; row++) {
    for (size_t col = 0; col < 6; col++) {
        prob->occupancymap2d_static.occ_map[row * 6 + col] = occ[row * 6 + col];
    }
}
/* build esdf */ 
vehicle_static_occupancymap2d_build_esdf(&prob->occupancymap2d_static);

The following notes apply:

• vehicle is the problem name, static is the occupancy grid name, consistent with the names defined during modeling
• The vehicle_static_occupancymap2d_build_esdf function is an auto-generated function used to compute the distance field
• Occupancy grid data is accessed via occupancymap2d_<occ_name>.occ_map. The data format consists of 0/1 values, where 0 indicates that the cell is free, and 1 indicates that the cell is occupied
• Occupancy grid data is stored in row-major format (different from other parts of OPTIMake), stored row by row starting from the lower-left corner

info

The ESDF distance field is stored in occupancymap2d_<occ_name>.esdf, with the following notes:

• The length of the ESDF distance field is length+3, and the width is width+3, i.e., the distance field corresponding to the occupancy grid with an extra ring of free cells added around the original occupancy grid
• The ESDF distance field storage format is the same as the occupancy grid: row-major format, stored row by row starting from the lower-left corner
• The stored values are distance values in units of resolution, with a precision of ±1 resolution. For example, if a cell's ESDF value is 5 and the resolution is 0.1 m/cell, this means the distance from that cell to the nearest obstacle is 0.5 m
• A positive distance value indicates the cell is free, a negative value indicates the cell is occupied by an obstacle, and 0 indicates the cell is on the obstacle boundary
• You can compute the distance field using the <prob_name>_<occ_name>_occupancymap2d_build_esdf function, or alternatively not call this function and instead compute the distance field yourself and manually populate the occupancymap2d_<occ_name>.esdf data

The figure below shows the 6x6 occupancy grid from the example above (the inner layer is the original occupancy grid, the outer layer is the extended free cells, and the solid dot is the occupancy grid origin):

The ESDF distance field for this occupancy grid is (this value is consistent with the occupancymap2d_static.esdf data in the prob struct):

double esdf[81] = {
    2.828427, 2.236068, 2.000000,  2.000000,  2.000000, 2.236068, 2.828427, 3.535534, 4.242641, 
    2.236068, 1.414214, 1.000000,  1.000000,  1.000000, 1.414214, 2.121320, 2.828427, 3.605551, 
    2.000000, 1.000000, 0.000000,  0.000000,  0.000000, 0.707107, 1.414214, 2.236068, 3.162278, 
    2.000000, 1.000000, 0.000000, -1.000000, -0.707107, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.000000, 1.000000, 0.000000, -1.000000, -1.000000, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.000000, 1.000000, 0.000000, -1.000000, -1.000000, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.000000, 1.000000, 0.000000,  0.000000,  0.000000, 0.000000, 1.000000, 2.000000, 3.000000, 
    2.236068, 1.414214, 1.000000,  1.000000,  1.000000, 1.000000, 1.414214, 2.236068, 3.162278, 
    2.828427, 2.236068, 2.000000,  2.000000,  2.000000, 2.000000, 2.236068, 2.828427, 3.605551
};

Results

Below is the problem setup and result for vehicle collision avoidance trajectory planning using the point-to-2D occupancy grid collision avoidance feature:

• The dashed line is the vehicle's reference line
• The vehicle dimensions are 4x2 m. Vehicle collision avoidance is achieved by applying point-to-2D occupancy grid collision avoidance constraints on 30 sampled points on the vehicle body
• The avoidance distance is 0.5 m (grid resolution is 0.1 m/cell)