路径规划与避障算法(七)---DWA算法流程之三---碰撞检测评价函数

版权声明：本文为博主原创文章，原创不易, 转载请联系博主。
本篇博客主要介绍DWA算法所采用的评价函数中障碍物相关的评价函数

评价函数:

轨迹主要依据以下三条准则进行评分,综合评分后选取分数最小的路径作为下一时刻选择路径:

• Obstacle_costs 轨迹上是否存在障碍物以及距离障碍物的距离
• Path_costs 轨迹上点距离局部参考路径最近距离
• Goal_costs 轨迹上点距离局部参考路径终点最近距离

由上篇博客(路径规划与避障算法(六))描述,无人车依据自行车模型以及当前速度条件及约束可以得到相应的轨迹采样空间,接下来,将利用以上三条评价标准对采样空间中的每一条路径进行评分与比较,选择综合分数最小的一条轨迹作为无人车下一时刻的运动轨迹.

对采样空间中的所有轨迹都进行遍历评分是一个十分耗时且浪费计算资源的事情,因此在对所遍历的路径进行评分时,比较建议采用优先进行碰撞检测的方式来减小计算资源的损耗与浪费.同时,车辆避障的实时性也是衡量无人车性能的一个重要标准,在低速场景下如园区物流,矿山卡车,港口物流车等实时性的优劣可能并不会影响无人车避障的性能,但是在高速场景下,无人车的实时性好坏将直接决定无人车避障功能的安全性.

因此,本篇博客将重点介绍与无人车避障路径中与障碍物相关的评价函数:

• 当采样路径上包含障碍物时,直接排除该路径,不再评分后续的评价函数
• 当采样路径上不包含障碍物时,则考虑轨迹上点到障碍物的最短距离

碰撞检测

通常碰撞检测主要有以下两类方法:

• 几何边界碰撞检测(Bounding Space and Hierarchies)
• 栅格空间覆盖枚举(Spatial Occupancy Enumeration)

1. 几何边界碰撞检测

常规的几何边界碰撞检测只需要考虑车辆的包络图形是否与障碍物存在覆盖,若存在覆盖,则判断出现碰撞现象如图1.这种检测方法可以快速的进行碰撞重叠测试,但是因为包络图形过大覆盖的问题,可能导致检测结果精度不高,因此后期又提出了改进型(Hirearchies)的措施.

改进型的方法将复杂的凸边形分解成为一颗树,该树代表了包含多组比原始对象更小子集的边界空间，这种层次结构允许对对象进行更精确的几何描述，同时降低了碰撞检测的计算成本，因为只有当较大的父对象发生碰撞时，才需要测试父对象内部的子对象。如图2所示,左图采用车辆外接圆对车辆进行包络,无人车碰撞检测的精度会受到较大影响.右图则采用改进型方法,只有当无人车外接包络圆与障碍物碰撞时,才需要检测子包络圆与障碍物是否碰撞,既提高了精度也没有降低检测的速度.

此篇文献 [Fast Collision Checking for Intelligent Vehicle Motion Planning] 是该类方法的代表 . 此类方法在检测过程中依然存在计算成本较大的风险,特别是需要利用判断子包络图形与障碍物是否碰撞的情况.另外,该方法的子包络图形的选择也是一个极大的挑战.

图 1 几何边界碰撞检测
图 2 改进型碰撞检测

2. 栅格空间覆盖枚举

该方法基于栅格地图,将车辆以及障碍物等周边环境信息转换到栅格地图中,其中,每个格子都可以准确的反应障碍物或者车辆的占有信息.由此,车辆行驶过程中的free space以及obstacles信息都可以精确的反应出来.具体如图3所示,车辆的在栅格中的信息(左图)被转换到每个格子中(右图).转换后,只需要判断车辆所占据的栅格原本是否存有障碍物占据信息.

由于其简洁性,选用均匀统一大小的栅格是通用的方法,因此,栅格尺寸大小将是影响碰撞检测的重要性能之一.如果栅格尺寸过于精细,则将影响车辆避障计算的实时性,同时也将降低传感器在每个栅格信息的精度.如果栅格尺寸过于粗大,将会使得自由空间被低估,碰撞检测过于保守，同时存在路径规划算法无法找到解决方案（即使存在）的风险。

图 3 栅格空间覆盖枚举

3. 基于Costmap的障碍物碰撞检测

Costmap是无人车感知层收集传感器信息(例如32线激光雷达)建立和更新的二维或三维信息.Costmap本身可以理解为自带分数的栅格地图.因此此方法属于栅格空间覆盖枚举的改进型方法.如图4所示,每个栅格的分数都在0~255之间,根据不同的分数,将Costmap划分为五层,其中红色为无人车或者机器人的外形轮廓(Footprint):

1. Lethal(致命层):分数为253~255,无人车的几何中心与该网格的中心重合，此时无人车必然与障碍物碰撞.
2. Inscribed(内切层):分数为253~255,栅格小于无人车的内切圆,此时无人车也必然与障碍物碰撞.
3. Possibly circumscribed(外接层):分数128~252,栅格与无人车的外接圆外切，此时无人车相当于靠在障碍物附近，不一定碰撞,需要根据车辆朝向等信息具体判断.
4. Freespace(自由层):分数0~127,无障碍物的自由空间.
5. Unknown(未知层):未知空间.

以上分数规划是由ROS官方定义的,在编写代码时也可以根据自身项目需求进行重新更改.

图 4 Costmap栅格分类

4. 基于Costmap的障碍物检测代码

以下代码为基于Costmap的碰撞检测部分代码,仅供交流,由于属于项目开发,不能提供全套代码,望见谅

整理碰撞检测流程如下:

1. 首先判断采样路径上所有采样点所占据栅格是否存在障碍物占据信息
2. 其次在每个采样点上计算实时车辆footprint对应的各个点所占据的栅格是否存在障碍物占据信息
3. 若有障碍物存在信息,则判定为碰撞并舍弃路径
4. 若没有障碍物存在信息,则继续其余两条评价函数分数

/****
@对轨迹上每个点进行评分,并将最终累加的分数作为obstacle_costs
@if cost < 0, we can determine the trajectory is stucked in collosion
@traj : Class Trajectory 
@Using Trajectory to construct a trajectory which contains:
*@param double xv The x velocity used to seed the trajectory
*@param double yv The y velocity used to seed the trajectory
*@param double thetav The theta velocity used to seed the trajectory
*@param int num_pts The expected number of points for a trajectory
*@param double costs_ The cost of a trjactory, 
*@param double time_delta_ The time gap between points.
****/
double scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }
  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,footprint_spec_,costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)//sum_scores_=true
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}

/****
@判断轨迹上是否存在障碍物及基于距离障碍物远近产生的分数
@param double x The position of the vehicle in world coordinates
@param double y The position of the vehicle in world coordinates
@param double th The position of the vehicle in world coordinates
@param double scale If we're over a certain speed threshold, we'll scale the robot's footprint to make it either slow down or stay further from walls
@param foot_spec Footprint points of the vehicles
****/
double ObstacleCostFunction::footprintCost (
    const double& x,
    const double& y,
    const double& th,
    std::vector<geometry_msgs::Point> footprint_spec,
    Costmap2D* costmap,
    WorldModel* world_model) {
  double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);
  if (footprint_cost < 0) {
    return -6.0;
  }
  unsigned int cell_x, cell_y;

  if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
    return -7.0;
  }

  double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
  return occ_cost;
}