路径规划与避障算法(三)---规划层与感知层的接口定义及数据融合

接口概述

1. 规划层与感知层的数据交互主要体现在以下几个方面:

• 感知层主要提供可行驶区域信息,车辆前进道路方向上的障碍物信息
• 目前本身程序可以实现静态障碍物避障功能,车辆识别动态障碍物并停车让行功能
• 感知层将障碍物信息通过costmap发布给规划层
• 发布的信息包含在一个一维数组当中,感知层与规划层以统一的收发方式维护彼此相同的costmap

2. 感知层与定位层的数据交互关系主要体现在以下几个方面:

• 感知层需要将传感器的座标系转换到baselink中
• 感知层主要关注costmap的原点在baselink中位置,长,宽以及分辨率大小

3. 由于规划层同时与定位层以及感知层进行数据交互,定位层与感知层的信息发布时间往往是不一致的,这样会对规划层的避障规划结果造成比较大的影响.因此建议首先对激光雷达以及惯导在硬件上进行硬同步操作,同时在软件上利用message_filters对两层进行软同步.

代码详述

  /**
   * @brief  take the basic properites of the costmap to planner
   * @param  cell_size_x ：the total number of the index in X axis
   * @param  cell_size_y ：the total number of the index in Y axis
   * @param  resolution  ：分辨率
   * @param  origin_x_in_map  ：costmap原点在baselink下的位置
   * @param  origin_y_in_map  ：costmap原点在baselink下的位置
   * @return The associated index
   */
 void dwa_planner_node::rt_costmap_cb(nav_msgs::OccupancyGrid costmap_msg)
{
cell_size_x = costmap_msg.info.height / costmap_msg.info.resolution;
cell_size_y = costmap_msg.info.width / costmap_msg.info.resolution;
resolution = double(costmap_msg.info.resolution);
origin_x_in_map = double(costmap_msg.info.origin.position.x);
origin_y_in_map = double(costmap_msg.info.origin.position.y);

cost_map_.resizeMap(cell_size_x, cell_size_y, resolution, origin_x_in_map, origin_y_in_map);//通过实际数据对costmap进行size,position的设置

cost_map_.setValue(yaw, odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y);//将当前位置与角度传递以进行maptoworld的转换

for (int ii = 0; ii < cell_size_x; ii++)
    {
    for (int jj = 0; jj < cell_size_y; jj++)
    {
      cost_map_.setCost(ii, jj, costmap_msg.data[cost_map_.getIndex(ii, jj)]);
    }
  }//将感知层中costmap的一位数组读进规划层的costmap中

}

costmap底层支持函数

  /**
   * @brief  Given two map coordinates... compute the associated index
   * @param mx The x coordinate，index count
   * @param my The y coordinate, index count
   * @return The associated index
   */
 unsigned int getIndex(unsigned int mx, unsigned int my) const//from x start to end then switch to y 
  {
    return my * size_x_ + mx;
  }

  /**
   * @brief  Set the cost of a cell in the costmap
   * @param mx The x coordinate of the cell
   * @param my The y coordinate of the cell
   * @param cost The cost to set the cell to
   */
void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost)
{
  costmap_[getIndex(mx, my)] = cost;
}

调试经验

• costmap座标系的问题,是以车辆前进方向为X正半轴方向,车辆左侧为Y正半轴方向的右手准则来判定的
• costmap本身的原点设定为map的右下角
• costmap根据ROS_Navigation库,其中的值是0_{255,但是此项目中是0}100,且程序中设置为当栅格数值大于10时就认定为障碍物
• 乘用车上的激光雷达感知区域稳定,盲区一般较小,障碍物信息往往可以控制在影响较小的固定范围内
• 在工程项目中,由于工程机械的特定结构遮挡,激光雷达常常存在各种盲区,因此仅仅只靠一个32线或64线激光雷达的话是无法完整的实现避障功能的,还需要感知层完成多传感器的数据融合工作
• 在开展避障工作前最好先完成传感器(激光雷达等)的标定工作
• 在工程机械上,当激光雷达等传感器安装在可活动的工程机械部件上时,需考虑每次传感器的位置是否存在误差与区别,若存在,则推荐做一个动态tf关系来补偿传感器与baselink之间的误差

总结

• 感知层的问题主要存在于感知的准确性与精度的问题
• 感知的范围：感知的盲区也是影响避障效果的重要因素
• costmap所用的座标系，原点的位置，一维数组的发送与读取方式是否相同都是会造成避障偏差的可能原因