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

点对边界避障

在轨迹规划中, 有时需要避开复杂形状的边界, 例如道路边缘、建筑物轮廓等. 点对边界的避障功能可以实现点与由多个线段组成的边界之间的避障约束.

建模​

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

left_bound = boundary('left_bound', max_dim=50, left_positive=False, free_extrapolation=True)
right_bound = boundary('right_bound', max_dim=50, left_positive=True, free_extrapolation=True)
ineq_left = point_to_boundary_inequality(left_bound, px, py, distance_to_avoid)
ineq_right = point_to_boundary_inequality(right_bound, px, py, distance_to_avoid)
prob.inequality(ineq_left)
prob.inequality(ineq_right)

首先, 通过boundary接口定义边界, 该接口定义了一个二维边界, 该边界由若干个点组成, 这些点通过线段依次连接形成一条折线, 其入参如下:

  • name: 边界名称
  • max_dim: 组成该边界的最大点数, 必须为大于等于2的整数
  • left_positive: bool类型, 指定边界左侧是否为正距离区域 (左侧的定义为从边界起点沿着点序列方向的左侧)
  • free_extrapolation: bool类型, 指定边界是否可以无限延伸. 当为False时, 边界沿着第一段线段的反方向和最后一段线段的方向无限延伸. 当为True时, 边界不进行延伸. 该参数的默认值为True, 即不进行延伸.
信息

当free_extrapolation为True时, 边界不进行延伸, 超出边界的点的距离为到端点的距离, 并且符号由left_positive决定, 即边界外方向为可行区域方向.

当free_extrapolation为False时, 边界的延伸规则如下:

  • 边界沿着第一段线段的反方向无限延伸
  • 边界沿着最后一段线段的方向无限延伸
  • 当上述的两条射线相交或者与边界的其他线段相交时, 则停止延伸
注意
  • 线段相交时, 可能会导致符号或者距离计算错误

然后, 通过point_to_boundary_inequality接口定义点对边界的避障不等式约束, 其入参如下:

  • boundary: 通过boundary接口定义的边界
  • px, py: 需要避障的点的坐标, 关于优化变量vv的表达式
  • distance_to_avoid: 避障距离, 可以为常数, 关于参数pp的表达式, 或者关于优化变量vv的表达式

求解​

边界的数据都保存在prob结构体中. 在求解前, 需要设置边界数据. 下面为C/C++代码示例:

prob.boundary_left_bound.valid_dim = 6;
prob.boundary_left_bound.x[0] = -5.0; prob.boundary_left_bound.y[0] = -5.0;
prob.boundary_left_bound.x[1] = 5.0; prob.boundary_left_bound.y[1] = -5.0;
prob.boundary_left_bound.x[2] = 5.0; prob.boundary_left_bound.y[2] = 1.0;
prob.boundary_left_bound.x[3] = 15.0; prob.boundary_left_bound.y[3] = 1.0;
prob.boundary_left_bound.x[4] = 25.0; prob.boundary_left_bound.y[4] = -1.0;
prob.boundary_left_bound.x[5] = 35.0; prob.boundary_left_bound.y[5] = -3.0;

prob.boundary_right_bound.valid_dim = 5;
prob.boundary_right_bound.x[0] = -5.0; prob.boundary_right_bound.y[0] = 4.0;
prob.boundary_right_bound.x[1] = 5.0; prob.boundary_right_bound.y[1] = 4.0;
prob.boundary_right_bound.x[2] = 15.0; prob.boundary_right_bound.y[2] = 4.0;
prob.boundary_right_bound.x[3] = 30.0; prob.boundary_right_bound.y[3] = 1.0;
prob.boundary_right_bound.x[4] = 35.0; prob.boundary_right_bound.y[4] = 1.0;

其中, 有以下说明:

  • left_bound和right_bound为边界的名称, 与建模时定义的名称一致
  • valid_dim为边界的有效点数, 即通过多少个点定义该边界，必须小于等于建模时定义的max_dim且大于等于2
  • x和y为边界上各个点的坐标数组
信息
  • 当valid_dim小于2时, 求解器会返回0距离
  • 当valid_dim大于max_dim时, 求解器会使用max_dim作为有效点数
  • 计算量与valid_dim成正比, 因此建议合并相邻且共线的点以减少valid_dim

效果​

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

  • 车辆起始位置为(-30, 2) m, 目标为跟踪x轴的参考线并且避开左右两侧的边界 (边界值为上述定义的left_bound和right_bound)
  • 车辆的优化轨迹为100个stage, 每个stage, 车辆都与两个边界进行避障 (通过车辆不同角点与内部点对边界避障实现), 避障距离为0.3 m

下面为对边界避障的轨迹规划效果图:

boundary_precise