点对占据网格避障

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

0.7.0版本支持二维和三维占据网格. 二维网格使用occupancy_map_2d定义, 三维网格使用occupancy_map_3d定义; 两者均通过point_to_occupancy_map_inequality添加点的避障约束.

二维占据网格

建模

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

  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), 可以为常数或关于参数$p$的表达式
• origin_x, origin_y, origin_phi: 占据网格的原点位姿 (左下角为原点, 逆时针为正), 可以为常数或关于参数$p$的表达式

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

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

求解

占据网格的数据都保存在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的占据网格 (内层为原始占据网格, 外层为边界扩展的空闲格子, 实心圆点为占据网格的原点):

该占据网格的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)

三维占据网格

三维占据网格将空间划分为体素 (voxel), 每个体素记录该区域是否被障碍物占据. 与二维接口相比, 三维接口增加了高度方向的尺寸和待避障点的z坐标, 并通过旋转矩阵和平移向量描述地图在世界坐标系中的位姿.

建模

三维占据网格及其避障约束的接口如下:

occupancy_map = occupancy_map_3d(name, length, width, height, resolution=0.1, T=None)
ineq = point_to_occupancy_map_inequality(
    occupancy_map, px, py, pz, distance_to_avoid, scaling_ratio)
prob.inequality(ineq)

occupancy_map_3d的入参如下:

参数	说明
name	地图名称, 必须为合法的C标识符, 例如static
length, width, height	地图局部x、y、z方向的体素数, 必须为大于等于1的整数, 在代码生成时确定
resolution	每个体素的边长, 默认为0.1; 若世界坐标使用m, 单位为m/voxel. 可以为正常数或关于已声明参数的表达式, 运行时必须大于0
T	地图位姿, 格式为(R, t). R为将地图坐标系旋转至世界坐标系的3×3旋转矩阵, t为地图原点在世界坐标系中的位置

R必须使用3×3的list/tuple定义, t必须使用长度为3的list/tuple定义. 省略T时使用单位旋转和零平移; 也可将R或t分别设置为None, 使用对应的默认值. t对应体素(0, 0, 0)的起始角点, 不是体素中心. 接口不检查R的正交性, 调用方需保证其为有效的旋转矩阵.

resolution、R和t的元素可以是常数、已声明参数或仅依赖这些参数的表达式, 也支持阶段参数. 它们不能依赖优化变量; 使用未声明参数或优化变量时, 代码生成会报错.

地图原点$O_m$位于首体素(0, 0, 0)的起始角点, 三条正坐标轴沿该角点相交的体素棱延伸. 该角点在世界坐标系中的坐标为t.

point_to_occupancy_map_inequality的三维入参如下:

参数	说明
occupancy_map	通过occupancy_map_3d定义的三维占据网格
px, py, pz	待避障点的世界坐标, 可以为关于优化变量的表达式
distance_to_avoid	安全距离, 与世界坐标使用相同的长度单位; 可以为常数、关于参数或优化变量的表达式
scaling_ratio	不等式的数值缩放系数, 为(1e-6, 1.0]内的数值常量, 默认为0.1; 不改变实际安全距离

下面为occ3d_traj轨迹规划问题中的建模片段. 地图尺寸为40×30×24, 地图位姿、分辨率和安全距离均通过参数设置; 目标函数、动力学和起终点约束可继续按常规多阶段问题设置:

from optimake import multi_stage_problem
from optimake.extension import occupancy_map_3d, point_to_occupancy_map_inequality

prob = multi_stage_problem('occ3d_traj', 41)
x, y, z = [prob.variable(name) for name in ('x', 'y', 'z')]
resolution = prob.parameter('resolution')
safe_distance = prob.parameter('safe_distance')
R = [list(prob.parameters([f'r{i}{j}' for j in range(3)])) for i in range(3)]
t = list(prob.parameters(['tx', 'ty', 'tz']))

occ_map = occupancy_map_3d('static', 40, 30, 24, resolution, T=(R, t))
ineq = point_to_occupancy_map_inequality(
    occ_map, x, y, z, safe_distance)
prob.inequality(ineq)

求解

三维占据数据保存在prob->occupancymap3d_<occ_name>.occ_map中. 求解前, 先填充完整的占据数组, 再调用自动生成的<prob_name>_<occ_name>_occupancymap3d_build_esdf函数构建距离场.

下面的C/C++片段对应问题名称occ3d_traj、地图名称static和40×30×24的网格. 假设已分配prob、option、ws和out, 并通过occ3d_traj_init完成初始化:

/* 填充全部体素: x方向连续, 然后是y和z方向. */
for (size_t z = 0; z < 24; ++z) {
    for (size_t y = 0; y < 30; ++y) {
        for (size_t x = 0; x < 40; ++x) {
            size_t index = ((z * 30) + y) * 40 + x;
            prob->occupancymap3d_static.occ_map[index] =
                (x >= 16 && x < 24 && y >= 6 && y < 24 && z < 8) ? 1 : 0;
        }
    }
}
occ3d_traj_static_occupancymap3d_build_esdf(&prob->occupancymap3d_static);

/* 设置地图位姿、分辨率和安全距离. 此处使用单位旋转. */
const double rotation[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
for (size_t i = 0; i < 9; ++i) {
    prob->param[OCC3D_TRAJ_PARAM_R00 + i] = rotation[i];
}
prob->param[OCC3D_TRAJ_PARAM_TX] = 0.3;
prob->param[OCC3D_TRAJ_PARAM_TY] = -0.4;
prob->param[OCC3D_TRAJ_PARAM_TZ] = 0.2;
prob->param[OCC3D_TRAJ_PARAM_RESOLUTION] = 0.1;
prob->param[OCC3D_TRAJ_PARAM_SAFE_DISTANCE] = 0.15;

/* 继续设置起终点、参考轨迹等其余参数和初值, 然后求解. */
int status = occ3d_traj_solve(prob, option, ws, out);

其中, 有以下说明:

• occ_map的类型为unsigned char, 长度为length * width * height; 0表示空闲, 非零表示占据.
• 体素(x, y, z)的下标为((z * width) + y) * length + x, 其中0 <= x < length, 0 <= y < width, 0 <= z < height.
• prob_name和occ_name分别与建模时的问题名称、地图名称一致; 上面的参数宏由对应模型自动生成.
• 修改占据数据后, 需要重新调用build_esdf; 仅修改作为参数传入的T或resolution时, 无需重建ESDF. 地图尺寸或建模常数改变时, 需要重新生成并编译代码.
• 占据数组和ESDF均内嵌在问题结构体中, 大尺寸地图会增加内存开销, 建议像示例一样在堆上分配问题结构体.

距离场与查询

三维ESDF保存在prob->occupancymap3d_<occ_name>.esdf中, 以网格顶点为采样位置:

• 顶点数量为(length + 1) * (width + 1) * (height + 1), 不额外扩展二维接口中的外围空闲格子.
• 顶点(x, y, z)的下标为((z * (width + 1)) + y) * (length + 1) + x, 各轴下标分别可取到length、width和height.
• 存储的距离以体素格数为单位, 乘以resolution后才是世界坐标中的长度. 正值表示空闲侧, 负值表示障碍物内部, 0表示采样的障碍物边界.
• 网格内部通过三线性插值查询距离和梯度, 网格外使用近似外部距离. 完全空闲的地图返回网格距离1e20和零梯度.

也可以直接调用自动生成的C查询函数. 例如, 查询网格坐标(5, 15, 5)处的距离:

Occ3d_traj_ESDFQueryResult3D result;
occ3d_traj_static_occupancymap3d_query(
    &prob->occupancymap3d_static, 5.0, 15.0, 5.0, &result);
double distance = prob->param[OCC3D_TRAJ_PARAM_RESOLUTION] * result.val;

result包含val、grad_x、grad_y和grad_z, 分别为网格距离及其对网格坐标的梯度. 显式C查询接收的是网格坐标, 不能直接传入世界坐标; 查询世界坐标点时, 需先按$q = R^T(p-t)/r$变换坐标, 并将返回距离乘以resolution. 通过建模接口生成的避障约束会自动完成坐标变换、距离换算和梯度链式求导.

信息

ESDF插值是距离近似, 在网格单元交界处一般不可导. 单体素厚度的障碍内部可能插值得到0, 因此应设置正的distance_to_avoid, 避免将零距离视为满足安全要求. 轨迹避障约束作用于所添加的轨迹节点, 节点之间的连续运动仍需另行检查.

ESDF梯度的符号导数采用零近似, 即使使用exact配置, 也不会得到真实的ESDF Hessian.

效果

下面为同一条三维轨迹同时避开两张占据网格的问题设定与效果图:

• 两张地图的尺寸分别为60×30×24和20×18×12, 分辨率分别为0.10 m/voxel和0.15 m/voxel, 并分别设置地图原点.
• 对每个轨迹节点分别施加两张地图的避障约束, 与两张地图中占据区域的安全距离均为0.15 m.
• 红色虚线为穿过占据区域的直线参考, 橙色虚线为从障碍物上方绕过的初始轨迹, 蓝色实线为同时避开两处占据区域的优化轨迹.

下面为iiwa14机械臂搬运纸箱时, 利用三维占据网格避开工作台和中央夹具的效果:

• 中央夹具和两侧工作台写入同一张32×32×28的三维占据网格, 分辨率为0.05 m/voxel.
• 通过包络球覆盖机械臂连杆、腕部和纸箱, 对球心施加点对占据网格的避障约束, 避障距离包含包络球半径和0.02 m的安全间距.