沉默杀手HomeArchivesTagsCategories
障碍层5 raytraceFreespace从代价地图清除障碍
|路径规划代价地图|
Word count: 178|Reading time: 1 min

先提一下栅格地图的占用值更新的方式。当雷达扫到一个障碍时，会对击中点的栅格更新一次占用，这时在ROS地图下这个点将被表示为障碍物．

如果ROS中设置障碍物的occupied_thresh是0.65，free_thresh是0.25。第一次更新地图时，这个格子的占用值为 ，就会将ROS地图中这个格子设置为100，表示障碍物．如果这个障碍离开了这个位置，在之后的过程中，如果有4次雷达扫描这个栅格都是空闲状态，也就是没有障碍物，则这个格子的占用值将变为 ，所以就会将ROS地图中对应的栅格设置为0，变成可通过区域了．

这就导致了不能通过一次两次的更新之后的值，来代表这个栅格是占用还是空闲的，因为这个状态十分有可能是错的．只有通过长时间的累计之后，最后通过这个值是否大于零，来近似地将这个栅格设置为是否是障碍物，这时候依然不能百分百地确定这个栅格是否就是障碍物．

在上一篇的UpdateBounds函数最后到了

1
2
3
4
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
  raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}

流程

清理传感器到障碍物间的cell，会首先处理测量值越界的问题，然后调用

1
2
3
4
MarkCell marker(costmap_, FREE_SPACE);
// and finally... we can execute our trace to clear obstacles along that line
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);

最终raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 会将所有在(x0,y0)>>(x1,y1)之间的所有cell标记为FREE_SPACE。而updateRaytraceBounds 会根据测量的距离，更新扩张（min_x, min_y, max_x, max_y）
updateBounds在根据测量数据完成 clear 操作之后，就开始了mark 操作，对每个测量到的点，标记为obstacle ：
1
2
3
4
5
6
7
double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;

......省略

unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(px, py, min_x, min_y, max_x, max_y);

1
2
3
4
5
6
7
8
9
10
/*
 * @brief  Given distance in the world, convert it to cells
 * @param  world_dist: The world distance
 * @return The equivalent cell distance
 */
unsigned int Costmap2D::cellDistance(double world_dist)
{
  double cells_dist = max(0.0, ceil(world_dist / resolution_));
  return (unsigned int)cells_dist;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
class MarkCell
{
public:
  MarkCell(unsigned char* costmap, unsigned char value) :
      costmap_(costmap), value_(value)
  {
  }
  inline void operator()(unsigned int offset)
  {
    costmap_[offset] = value_;
  }
private:
  unsigned char* costmap_;
  unsigned char value_;
};

updateCosts

代价地图中每个cell可用255个不同值中任何一个值，可是下层数据结构仅需要3个值。具体来说，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 当投影到代价地图时候，每种状态被赋一个特定的代价值，也就是说 每个cell的cost值是由这个cell对应的各层中对应的cell的状态进行加权得到的。 如果列有一定量的占用就被赋代价值。 如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图。

更新障碍地图代价，将机器人足迹范围内设置为 FREE_SPACE，并且在 bound 范围内将本层障碍地图的内容合并到主地图上。

障碍物层是将传感器检测到的点云投影到地图中，不同的传感器来源会分别存到Observation类中。只要遍历存储Observation的容器，将检测到的点云的每个点分别投影到地图中，将对应的网格的代价值设为LETHAL_OBSTACLE，实现函数在ObstacleLayer::updateBounds中：

清除代价地图的方式也很好理解，以单线激光雷达的激光线为例子，当激光线发射出去之后，在某处检测到一个障碍物，那说明：从发射的地方至某处之间是free的，那么这之间的旧的障碍物应当被删除，这之间网格的代价值应当被修改为free

可能是Bresenham2D的round off error(舍入误差)造成的. It marks a particular cell as obstacle but the next time when the sensor reading changes instantaneously, it no longer traces it through the same path and hence, the blob seem remains in the costmap.

added 2 lines of code to the Bresenham2D algorithm. This basically clears the cell to the left and right of the grid through which the Line segment constructed by the algorithm passes. This results in loosing some resolution of the map, but the solution works pretty well in real life application

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// A 2D implementation of Bresenham's raytracing algorithm, applies an action 
// at each step template<class ActionType>
inline void bresenham2D( ActionType at, unsigned int abs_da, unsigned int abs_db, 
                        int error_b, int offset_a, int offset_b, unsigned int offset, 
                        unsigned int max_length)
{
  unsigned int end = std::min(max_length, abs_da);
  for (unsigned int i = 0; i < end; ++i)
  {
    at(offset);
    at(offset+1); // **ADDED THIS LINE**
    at(offset-1); // **ADDED THIS LINE**
    offset += offset_a;
    error_b += abs_db;
    if ((unsigned int)error_b >= abs_da)
    {
      offset += offset_b;
      error_b -= abs_da;
    }
  }
  at(offset);
}