代码解析之range_sensor_layer中costmap更新的方法和模型（一）

         大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/c_1132958996826546176）。以后，我会把日常的思考放在CSDN上，梳理过的精华文章放在知乎上，希望大家可以多多交流，互相学习。

---

在ROS Navigation中，costmap_2d定义了代价地图，range_sensor_layer根据传感器探测结果更新之。这里的传感器主要指的是超声波/红外测距传感器，共同特征是探测范围是一个扇形区域，而不是像激光雷达一样是一条线。所以，给定一个距离返回值，我们并不能确定具体是扇形的弧边沿的哪个栅格才是障碍物，弧上任一点都有可能，需要对该扇形区域内的栅格占用概率建模。

         建模的理论请参考文献《一种基于概率的超声波模型的设计》（刘利枚）和《a comparison of three uncertainty calculi for building sonar-based occupancy grids》。

void RangeSensorLayer::updateCostmap(sensor_msgs::Range& range_message, bool clear_sensor_cone)
{
  max_angle_ = range_message.field_of_view/2;
    // max_angle_是视场角的一半。

  geometry_msgs::PointStamped in, out;
  in.header.stamp = range_message.header.stamp;
  in.header.frame_id = range_message.header.frame_id;

  if(!tf_->waitForTransform(global_frame_, in.header.frame_id, in.header.stamp, ros::Duration(0.1)) )
  {
    ROS_ERROR_THROTTLE(1.0, "Range sensor layer can't transform from %s to %s at %f",
        global_frame_.c_str(), in.header.frame_id.c_str(),
        in.header.stamp.toSec());
    return;
  }

  tf_->transformPoint (global_frame_, in, out);
    //in是sensor的本地信息，out是sensor的全局信息

  double ox = out.point.x, oy = out.point.y;
    //ox,oy是sensor在全局坐标系的X,Y坐标

  in.point.x = range_message.range;
    //现在in是障碍点的本地信息

  tf_->transformPoint(global_frame_, in, out);
    //out是障碍点的全局信息

  double tx = out.point.x, ty = out.point.y;
    //tx,ty是障碍点在全局坐标系的X,Y坐标

  // calculate target props
  double dx = tx-ox, dy = ty-oy, theta = atan2(dy,dx), d = sqrt(dx*dx+dy*dy);
    //theta是障碍点-sensor连线与全局坐标系X轴正方向的夹角，也就是sensor的朝向，d是连线距离


  // Integer Bounds of Update
  int bx0, by0, bx1, by1;

  // Triangle that will be really updated; the other cells within bounds are ignored
  // This triangle is formed by the origin and left and right sides of sonar cone
  int Ox, Oy, Ax, Ay, Bx, By;

  // Bounds includes the origin
  worldToMapNoBounds(ox, oy, Ox, Oy);
  bx1 = bx0 = Ox;
  by1 = by0 = Oy;
  touch(ox, oy, &min_x_, &min_y_, &max_x_, &max_y_);

  // Update Map with Target Point
  unsigned int aa, ab;
  if(worldToMap(tx, ty, aa, ab)){
    setCost(aa, ab, 233);
    touch(tx, ty, &min_x_, &min_y_, &max_x_, &max_y_);
  }

  double mx, my;

  // Update left side of sonar cone
  mx = ox + cos(theta-max_angle_) * d * 1.2;
  my = oy + sin(theta-max_angle_) * d * 1.2;
  worldToMapNoBounds(mx, my, Ax, Ay);
  bx0 = std::min(bx0, Ax);
  bx1 = std::max(bx1, Ax);
  by0 = std::min(by0, Ay);
  by1 = std::max(by1, Ay);
  touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);

  // Update right side of sonar cone
  mx = ox + cos(theta+max_angle_) * d * 1.2;
  my = oy + sin(theta+max_angle_) * d * 1.2;

  worldToMapNoBounds(mx, my, Bx, By);
  bx0 = std::min(bx0, Bx);
  bx1 = std::max(bx1, Bx);
  by0 = std::min(by0, By);
  by1 = std::max(by1, By);
  touch(mx, my, &min_x_, &min_y_, &max_x_, &max_y_);

  // Limit Bounds to Grid
  bx0 = std::max(0, bx0);
  by0 = std::max(0, by0);
  bx1 = std::min((int)size_x_, bx1);
  by1 = std::min((int)size_y_, by1);

    // 以上是确定要更新栅格的大体区域，下面是确定具体要更新哪个栅格

  for(unsigned int x=bx0; x<=(unsigned int)bx1; x++){
    for(unsigned int y=by0; y<=(unsigned int)by1; y++){
      bool update_xy_cell = true;

      // Unless inflate_cone_ is set to 100 %, we update cells only within the (partially inflated) sensor cone,
      // projected on the costmap as a triangle. 0 % corresponds to just the triangle, but if your sensor fov is
      // very narrow, the covered area can become zero due to cell discretization. See wiki description for more
      // details
      if (inflate_cone_ < 1.0){
        // Determine barycentric coordinates
        int w0 = orient2d(Ax, Ay, Bx, By, x, y);
        int w1 = orient2d(Bx, By, Ox, Oy, x, y);
        int w2 = orient2d(Ox, Oy, Ax, Ay, x, y);

        // 重心坐标Barycentric coordinates inside area threshold; this is not mathematically sound at all, but it works!
        float bcciath = -inflate_cone_*area(Ax, Ay, Bx, By, Ox, Oy);
        update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath;
      }

      if (update_xy_cell){
        double wx, wy;
        mapToWorld(x,y,wx,wy);
        update_cell(ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone);
      }
    }
  }

  buffered_readings_++;
  last_reading_time_ = ros::Time::now();
}

        上面判定了要更新的区域和栅格后，调用update_cell()具体执行。update_cell()中体现了贝叶斯定理的应用。记事件A是该栅格检测到障碍物，B是该栅格是occupied，由条件概率，P(B|A) = P(A|B)P(B)/P(A)，而P(A) = P(A|B)P(B) + P(A|B)P(B) = prob_occ + prob_not = sensor * prior + (1-sensor) * (1-prior)。

         sensor_model(r, phi, theta)中，r是传感器的测距结果（米)，phi是costmap中某个栅格到传感器的距离，theta是该栅格到传感器探测范围中轴线（即传感器朝向）的夹角（rad），该函数的返回值是根据栅格相对于传感器位置的概率值，表明该栅格是occupied的概率。

void RangeSensorLayer::update_cell(double ox, double oy, double ot, double r, double nx, double ny, bool clear)
{
  unsigned int x, y;
  if(worldToMap(nx, ny, x, y)){
    double dx = nx-ox, dy = ny-oy;
    double theta = atan2(dy, dx) - ot;
    theta = angles::normalize_angle(theta);
    double phi = sqrt(dx*dx+dy*dy);
    double sensor = 0.0;
    if(!clear)
      sensor = sensor_model(r,phi,theta);
    double prior = to_prob(getCost(x,y));
    double prob_occ = sensor * prior;
    double prob_not = (1 - sensor) * (1 - prior);
    double new_prob = prob_occ/(prob_occ+prob_not);

    //ROS_INFO("%f %f | %f %f = %f", dx, dy, theta, phi, sensor);
    //ROS_INFO("%f | %f %f | %f", prior, prob_occ, prob_not, new_prob);
    unsigned char c = to_cost(new_prob);
    setCost(x,y,c);
  }
}

double RangeSensorLayer::sensor_model(double r, double phi, double theta)
{
  double lbda = delta(phi)*gamma(theta);
  double delta = resolution_;
  if(phi >= 0.0 and phi < r - 2 * delta * r)
    return (1- lbda) * (0.5);
  else if(phi < r - delta * r)
    return lbda* 0.5 * pow((phi - (r - 2*delta*r))/(delta*r), 2)+(1-lbda)*.5;
  else if(phi < r + delta * r){
    double J = (r-phi)/(delta*r);
    return lbda * ((1-(0.5)*pow(J,2)) -0.5) + 0.5;
  }
  else
    return 0.5;
}

         sensor_model()函数中，lbda用来定量的评估栅格与传感器距离、与中轴线夹角对概率的影响，resolution_是地图的分辨率（m/grid）。