跳到主要内容
版本：v0.7.0 beta

点对占据网格避障

在轨迹规划中, 占据网格 (Occupancy Grid Map) 是一种常用的环境表示方法. 它将环境划分为离散的网格, 每个网格单元 (cell) 存储该区域是否被障碍物占据的信息. 点对占据网格的避障功能可以实现点与占据网格之间的避障约束, 以确保点不会进入被障碍物占据的区域.

建模​

下面为点对二维占据网格的避障不等式约束的例子:

  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)

首先, 通过occupancy_map_2d接口定义二维占据网格. 其中, occupancy_map_2d接口的入参如下:

  • name: 占据网格的名称
  • length, width: 占据网格在长度和宽度方向的格子数 (单位: cell), 必须为大于1的整数
  • resolution: 占据网格的分辨率 (单位: m/cell), 可以为常数或关于参数pp的表达式
  • origin_x, origin_y, origin_phi: 占据网格的原点位姿 (左下角为原点, 逆时针为正), 可以为常数或关于参数pp的表达式

然后, 通过point_to_occupancy_map_inequality接口定义点对二维占据网格的避障不等式约束, 其接口的入参如下:

  • occupancy_map: 二维占据网格, 通过occupancy_map_2d接口定义
  • px, py: 需要避障的点的坐标, 关于优化变量vv的表达式
  • distance_to_avoid: 避障距离, 可以为常数, 关于参数pp的表达式, 或者关于优化变量vv的表达式

point_to_occ

求解​

占据网格的数据都保存在prob结构体中. 在求解前, 需要设置占据网格的数据并且计算距离场 (Euclidean Signed Distance Field, ESDF). 下面为C/C++代码示例 (假设占据网格名称为static, 大小为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);

其中, 有以下说明:

  • vehicle为问题的名称, static为占据网格的名称, 与建模时定义的名称一致
  • vehicle_static_occupancymap2d_build_esdf函数为自动生成函数, 用于计算距离场
  • 占据网格的数据通过occupancymap2d_<occ_name>.occ_map进行访问, 其数据格式为0/1值, 其中0表示该格子为空闲, 1表示该格子被占据
  • 占据网格的数据为行优先 (row-major)格式存储 (与OPTIMake的其他部分不同), 从左下角开始依次按行存储
信息

ESDF距离场被存储在occupancymap2d_<occ_name>.esdf中, 有以下说明:

  • ESDF距离场的长度为length+3, 宽度为width+3, 即在原始占据网格的四周各增加了一圈空闲格子后的占据网格对应的距离场
  • ESDF距离场存储格式与占据网格相同, 为行优先 (row-major)格式存储, 从左下角开始依次按行存储
  • 存储的值为单位resolution下的距离值, 其精度为+-1 resolution. 例如, 当某个点的esdf值为5, 且resolution为0.1 m/cell, 则表示该格子到最近障碍物的距离为0.5 m
  • 该距离值为正值表示该格子为空闲, 负值表示该格子被障碍物占据, 0表示该格子在障碍物边界上
  • 可通过<prob_name>_<occ_name>_occupancymap2d_build_esdf函数计算距离场, 也可以不调用该函数, 而是自行计算距离场并且手动填充occupancymap2d_<occ_name>.esdf数据

下图为上面的示例中展示的6x6的占据网格 (内层为原始占据网格, 外层为边界扩展的空闲格子, 实心圆点为占据网格的原点):

occ_6x6

该占据网格的ESDF距离场为 (该值与prob结构体中的occupancymap2d_static.esdf数据一致):

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
};

效果​

下面为利用点对二维占据网格的避障功能实现的车辆避障轨迹规划的问题设定与效果图:

  • 虚线为车辆的参考线
  • 车长宽为4x2 m, 通过对车身上采集30个点施加点对二维占据网格的避障约束实现车辆避障
  • 避障距离为0.5 m (网格分辨率为0.1 m/cell)

occ_traj