在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 the ego vehicle's back and backshaft's center
* @return true if collision
*/
bool CollisionChecker::InCollision(
const std::vector<const Obstacle*>& 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<std::uint32_t>(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<Box2d> 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<std::uint32_t>(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<const Obstacle*>& obstacles, const double ego_vehicle_s,
const double ego_vehicle_d,
const std::vector<PathPoint>& 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<const Obstacle*> 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<Box2d> 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<PathPoint>& 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程度，自适应调整。
应该有一个指标量化碰撞检测的置信度，可以根据此置信度制定对应的行为决策，比如适当减速、提示信息等。但是该置信度会受考察的轨迹时间长度影响较大。
版权声明：本文为linxigjs原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。