Baidu Apollo代码解析之碰撞检测

在Lattice Planner中，需要对1D横纵向轨迹结合（combine）后形成的2D轨迹做限制检测（速度、加速度等）和碰撞检测。碰撞检测由CollisionChecker类完成，文件路径：apollo\modules\planning\constraint_checker\collision_checker.cc。碰撞检测主要是遍历每个障碍物的预测轨迹的每个轨迹点、遍历自车规划轨迹的每个轨迹点，分别构造轮廓box（近似bounding box），查看box是否重叠（overlap）。原理比较简单，粗暴贴代码。

/**
     * @brief Constructor
     * @param obstacles obstacles information from Prediction module
     * @param ego_vehicle_s s position of ego vehicle in Frenet Coordinate System(s-l, s-d)
     * @param ego_vehicle_d d position of ego vehicle in Frenet
     * @param discretized_reference_line the sampling points on reference line, the reference line in discretized form
     * @param ptr_reference_line_info
     * @param ptr_path_time_graph s-t graph
     */
CollisionChecker::CollisionChecker(
    const std::vector& obstacles, const double ego_vehicle_s,
    const double ego_vehicle_d,
    const std::vector& discretized_reference_line,
    const ReferenceLineInfo* ptr_reference_line_info,
    const std::shared_ptr& ptr_path_time_graph) {
  ptr_reference_line_info_ = ptr_reference_line_info;
  ptr_path_time_graph_ = ptr_path_time_graph;
  BuildPredictedEnvironment(obstacles, ego_vehicle_s, ego_vehicle_d,
                            discretized_reference_line);
}

    /**
     * @brief Check if there are overlaps between obstacles' predicted trajectories and ego vehicle's planning trajectory
     * @param obstacles
     * @param ego_trajectory
     * @param ego_length the length of ego vehicle
     * @param ego_width
     * @param ego_back_edge_to_center not sure. I guess it's the gap between the ego vehicle's back and backshaft's center
     * @return true if collision
     */
bool CollisionChecker::InCollision(
    const std::vector& obstacles,
    const DiscretizedTrajectory& ego_trajectory, const double ego_length,
    const double ego_width, const double ego_back_edge_to_center) {
  //traverse every point on ego vehicle's trajectory
  for (size_t i = 0; i < ego_trajectory.NumOfPoints(); ++i) {
    const auto& ego_point =
        ego_trajectory.TrajectoryPointAt(static_cast(i));
    const auto relative_time = ego_point.relative_time();
    const auto ego_theta = ego_point.path_point().theta();
    //construct a bounding box whose center is the point on trajectory with theta
    Box2d ego_box({ego_point.path_point().x(), ego_point.path_point().y()},
                  ego_theta, ego_length, ego_width);

    // correct the inconsistency of reference point and center point
    // TODO(all): move the logic before constructing the ego_box
    double shift_distance = ego_length / 2.0 - ego_back_edge_to_center;
    Vec2d shift_vec(shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta));
    ego_box.Shift(shift_vec);

    std::vector obstacle_boxes;
    //traverse every obstacle
    for (const auto obstacle : obstacles) {
      //get obstacle's trajectory point according to the time
      auto obtacle_point = obstacle->GetPointAtTime(relative_time);
      //construct a bounding box according to obstacle's length and width got from Perception module
      Box2d obstacle_box = obstacle->GetBoundingBox(obtacle_point);
      //if there is overlap between obstacle and ego vehicle, return
      if (ego_box.HasOverlap(obstacle_box)) {
        return true;
      }
    }
  }
  return false;
}

    /**
     * @brief Check if there are overlaps between obstacles and ego vehicle according to the predicted obstacles environment
     * @pre BuildPredictedEnvironment() is called before this, so that predicted_bounding_rectangles_ is ready
     * @param discretized_trajectory ego vehicle's trajectory sampling points
     * @return true if collision
     */
bool CollisionChecker::InCollision(
    const DiscretizedTrajectory& discretized_trajectory) {
  CHECK_LE(discretized_trajectory.NumOfPoints(),
           predicted_bounding_rectangles_.size());
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  double ego_length = vehicle_config.vehicle_param().length();
  double ego_width = vehicle_config.vehicle_param().width();
  //traverse every point on ego vehicle's trajectory
  for (size_t i = 0; i < discretized_trajectory.NumOfPoints(); ++i) {
    const auto& trajectory_point =
        discretized_trajectory.TrajectoryPointAt(static_cast(i));
    double ego_theta = trajectory_point.path_point().theta();
    //construct a bounding box according to ego vehicle's length and width got from config
    Box2d ego_box(
        {trajectory_point.path_point().x(), trajectory_point.path_point().y()},
        ego_theta, ego_length, ego_width);
    double shift_distance =
        ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
    Vec2d shift_vec{shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta)};
    ego_box.Shift(shift_vec);
    //traverse every obstacle's bounding box stored in predicted_bounding_rectangles_
    for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) {
      if (ego_box.HasOverlap(obstacle_box)) {
        return true;
      }
    }
  }
  return false;
}

    /**
     * @brief choose obstacles of interest and build bounding boxes for every obstacle's every trajectory point
     * @param obstacles
     * @param ego_vehicle_s
     * @param ego_vehicle_d
     * @param discretized_reference_line
     */
void CollisionChecker::BuildPredictedEnvironment(
    const std::vector& obstacles, const double ego_vehicle_s,
    const double ego_vehicle_d,
    const std::vector& discretized_reference_line) {
  CHECK(predicted_bounding_rectangles_.empty());

  // If the ego vehicle is in lane,
  // then, ignore all obstacles from the same lane behind of the ego vehicle.
  bool ego_vehicle_in_lane = IsEgoVehicleInLane(ego_vehicle_s, ego_vehicle_d);
  std::vector obstacles_considered;
  for (const Obstacle* obstacle : obstacles) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    if (ego_vehicle_in_lane &&
        (IsObstacleBehindEgoVehicle(obstacle, ego_vehicle_s,
                                    discretized_reference_line) ||
         !ptr_path_time_graph_->IsObstacleInGraph(obstacle->Id()))) {
      continue;
    }

    obstacles_considered.push_back(obstacle);
  }

  double relative_time = 0.0;
  while (relative_time < FLAGS_trajectory_time_length) {
    std::vector predicted_env;
    for (const Obstacle* obstacle : obstacles_considered) {
      // If an obstacle has no trajectory, it is considered as static.
      // Obstacle::GetPointAtTime has handled this case.
      TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
      //construct a bounding box according to obstacle's length and width got from Perception module
      Box2d box = obstacle->GetBoundingBox(point);
      //consider the safe distance
      box.LongitudinalExtend(2.0 * FLAGS_lon_collision_buffer);
      box.LateralExtend(2.0 * FLAGS_lat_collision_buffer);
      predicted_env.push_back(std::move(box));
    }
    predicted_bounding_rectangles_.push_back(std::move(predicted_env));
    relative_time += FLAGS_trajectory_time_resolution;
  }
}

bool CollisionChecker::IsEgoVehicleInLane(const double ego_vehicle_s,
                                          const double ego_vehicle_d) {
  double left_width = FLAGS_default_reference_line_width * 0.5;
  double right_width = FLAGS_default_reference_line_width * 0.5;
  ptr_reference_line_info_->reference_line().GetLaneWidth(
      ego_vehicle_s, &left_width, &right_width);
  return ego_vehicle_d < left_width && ego_vehicle_d > -right_width;
}

bool CollisionChecker::IsObstacleBehindEgoVehicle(
    const Obstacle* obstacle, const double ego_vehicle_s,
    const std::vector& discretized_reference_line) {
  double half_lane_width = FLAGS_default_reference_line_width * 0.5;
  TrajectoryPoint point = obstacle->GetPointAtTime(0.0);
  auto obstacle_reference_line_position = PathMatcher::GetPathFrenetCoordinate(
      discretized_reference_line, point.path_point().x(),
      point.path_point().y());

  if (obstacle_reference_line_position.first < ego_vehicle_s &&
      std::fabs(obstacle_reference_line_position.second) < half_lane_width) {
    ADEBUG << "Ignore obstacle [" << obstacle->Id() << "]";
    return true;
  }
  return false;
}

考虑到感知模块对不同类型障碍物的检测准确度可能不同（检测私家车可能比检测货车、卡车更准），预测模块对于不同时刻轨迹点的预测准确度也不同（越往后越不准），可以设计合理的轮廓尺寸的extend程度，自适应调整。

应该有一个指标量化碰撞检测的置信度，可以根据此置信度制定对应的行为决策，比如适当减速、提示信息等。但是该置信度会受考察的轨迹时间长度影响较大。