Collision avoidance

OPTIMake实现了对几何图形/占据网格避障约束的直接支持, 可通过建模接口直接定义避障不等式约束, 支持以下类型的避障不等式约束:

• point_to_rectangle_inequality: 点对矩形的避障不等式约束
• rectangle_to_rectangle_inequality: 矩形对矩形的避障不等式约束
• point_to_occupancy_map_inequality: 点对二维占据网格的避障不等式约束

在使用前需要先导入相关的接口:

from optimake.extension.collision_avoidance import *

warning

避障约束为非凸约束, 根据初始解的设置, 可能会导致求解器陷入不同的局部最优解

OPTIMake并不要求初始解为满足避障约束的可行解, 但当初始解具备清晰的避障模态时, 求解结果的避障模态一般会与初始解避障模态保持一致. 例如在车与车避障中, 当初始解为A车在B车的左侧时, 求解结果的A车轨迹通常也会在B车的左侧

反之当避障模态不清晰时, 求解结果的避障模态可能会与初始解避障模态不一致, 甚至可能导致求解不收敛. 例如A车与B车的初始解重叠时, 避障模态不清晰, 求解模态可能会在A车与B车的左侧或右侧, 甚至可能会导致求解不收敛

点对矩形 - point_to_rectangle_inequality

建模

下面为点对矩形的避障不等式约束的例子:

point2rect_ineq = point_to_rectangle_inequality(
    px, py,
    x, y, phi, width, length,
    distance_to_avoid
)
prob.inequality(point2rect_ineq)

其中, point_to_rectangle_inequality接口的入参如下:

• px, py: 需要避障的点的坐标
• x, y, phi: 矩形的中心坐标和朝向 (逆时针为正)
• width, length: 矩形的宽度和长度
• distance_to_avoid: 避障距离

info

width, length可以为常数, 或者关于参数$p$的表达式. 而其余参数 (px, py, x, y, phi, distance_to_avoid)可以为常数, 关于参数$p$的表达式, 或者关于优化变量$v$的表达式

效果

下面为利用点对矩形的避障功能实现的车辆避障轨迹规划的问题设定:

• 车辆起始位姿为0, 目标为跟踪x轴 (横轴)的参考线并且避开两个人 (通过点描述)
• 车辆的优化轨迹为100个stage, 每个stage, 即每个时间点处的车辆都与两个点进行避障, 避障距离为1.0 m

注意, 人在不同时间点处的位置可以不同, 即可处理动态避障问题.

下面为不同初始解下的轨迹规划效果图:

初始解在上方

初始解: 最优轨迹:

初始解在中间

初始解: 最优轨迹:

初始解在下方

初始解: 最优轨迹 (受最大转角约束限制, 求解器返回solve_status = 3的不可行标志位与最小约束违反解):

矩形对矩形 - rectangle_to_rectangle_inequality

建模

下面为矩形对矩形的避障不等式约束的例子:

rect2rect_ineq = rectangle_to_rectangle_inequality(
    x_a, y_a, phi_a, width_a, length_a,
    x_b, y_b, phi_b, width_b, length_b,
    distance_to_avoid,
    center_point_avoidance
)
prob.inequality(rect2rect_ineq)

其中, rectangle_to_rectangle_inequality接口的入参如下:

• x_a, y_a, phi_a: 矩形a的中心坐标和朝向 (逆时针为正)
• width_a, length_a: 矩形a的宽度和长度
• x_b, y_b, phi_b: 矩形b的中心坐标和朝向 (逆时针为正)
• width_b, length_b: 矩形b的宽度和长度
• distance_to_avoid: 避障距离
• center_point_avoidance: 是否对矩形的中心点进行避障 (默认为True, 即对中心点进行避障). 当为True时, 会增加矩形a的中心点对矩阵b的避障约束与矩形b的中心点对矩形a的避障约束, 避障距离分别为distance_to_avoid + min(width_a, length_a) / 2 (即矩形a的中心点与矩形b之间的距离需要大于避障距离加上矩形a的半宽度或半长度) 与distance_to_avoid + min(width_b, length_b) / 2

info

width_a, length_a, width_b, length_b可以为常数, 或者关于参数$p$的表达式. 而其余参数 (x_a, y_a, phi_a, x_b, y_b, phi_b, distance_to_avoid)可以为常数, 关于参数$p$的表达式, 或者关于优化变量$v$的表达式

warning

• rectangle_to_rectangle_inequality的原理为构建8个point_to_rectangle_inequality约束, 其中4个约束为矩形a的四个角点对矩形b的避障约束, 4个约束为矩形b的四个角点对矩形a的避障约束
• 该约束不能处理矩形a与矩形b重叠且各自的四个角点在对方矩形外部的情况 (例如十字交叉的情况), 可以通过设置center_point_avoidance为True添加中心点的point_to_rectangle约束来减少这种情况的发生, 或者可以自行添加其他点处的point_to_rectangle约束来处理这种情况

效果

下面为利用矩形对矩形的避障功能实现的车辆避障轨迹规划的问题设定:

• 车辆起始位姿为0, 目标为跟踪x轴 (横轴)的参考线并且避开两个其他车辆
• 车辆的优化轨迹为100个stage, 每个stage, 即每个时间点处的车辆都与两个其他车辆进行避障, 避障距离为0.2 m

注意, 其他车辆在不同时间点处的位置可以不同, 即可处理车辆间的动态避障问题.

下面为不同初始解下的轨迹规划效果图:

初始解在上方

初始解: 最优轨迹:

初始解在中间

初始解: 最优轨迹:

初始解在下方

初始解: 最优轨迹 (受最大转角约束限制, 求解器返回solve_status = 3的不可行标志位与最小约束违反解):

点对占据网格 - 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的其他部分不同), 从左下角开始依次按行存储

info

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)