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<const Obstacle*>& obstacles, const double ego_vehicle_s,
    const double ego_vehicle_d,
    const std::vector<PathPoint>& discretized_reference_line,
    const ReferenceLineInfo* ptr_reference_line_info,
    const std::shared_ptr<PathTimeGraph>& 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