【Autonomous Driving】Collision Detection Algorithm

Reference link:

[Autonomous Driving] Collision Detection Algorithm

[Planning] Detailed explanation of Box2d::HasOverlap() collision detection interface_lemon_zy's blog - CSDN blog_box2d collision detection

A common method is the Hyperplane Separation Theorem, which is used to detect whether there is an intersection of convex sets in N-dimensional Euclidean space. For the strict definition, please refer to Wikipedia . Its two-dimensional case is called the Separating Axis Theorem, which is briefly described as follows:

The necessary and sufficient condition for the disjointness of two convex sets of planes is that there is a certain line (ie, the separation axis), so that the projections of the two planes on the line do not intersect, and similar negative propositions can be written . Obviously, to prove the intersection we need to exhaust all the lines, which is impossible. Fortunately, for convex polygons, we only need to check if the line on which its edge lies is a separating line (not a separating axis). (You can think about how to implement concave polygons and circles)

In the context of autonomous driving, obstacles are generally represented by plane rectangles (planar convex polygons will be used for higher precision), and for two rectangles, we only need to traverse 4 sides (two adjacent two rectangles for each rectangle). side), as long as one of the four sides is the separation axis, the two rectangles do not intersect . On the contrary, if the two rectangles intersect, the four sides are not the separation axis ( compare half of the sum of the projection length of the ego vehicle and the target contour on the separation axis, and the size of the projection distance, the projection distance is all smaller than the projection and overlap hasOverLap ), That is, their projections on it all coincide.

In order to efficiently represent a flat rectangle, we generally define it like this:

struct Box2d {
  float center_x_;
  float center_y_;
  float length_;
  float width_;
  float half_length_;
  float half_width_;
  float heading_;
  float cos_heading_;
  float sin_heading_;
  float min_x_;
  float max_x_;
  float min_y_;
  float max_y_;
  struct Vec2d corners_[4];
};

Projection principle: Project the two adjacent sides of the vehicle and the two adjacent sides of the obstacle

 We only need to compare half the sum of the projection lengths of the ego vehicle and the target contour on the separation axis, and the size of the projection distance, to determine whether the projections of the two intersect. Obviously, the projected length of the ego car on the separation axis is length_.

The following code is the projection calculation on the two adjacent sides of the corresponding Vehicle

std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
             std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
                 std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
                 half_length_ &&
std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
             std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
                 std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
                 half_width_

 That is, any point of the vehicle is not within the orange dotted box corresponding to the obstacle.

 

The following code is the projection calculation on the two adjacent edges corresponding to the Obstacle

std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=
             std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) +
                 std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) +
                 box.half_length() &&
         std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=
             std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) +
                 std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) +
                 box.half_width()

 

Box2d::HasOverlap(const Box2d &box) 

bool Box2d::HasOverlap(const Box2d &box) const {
  if (box.max_x() < min_x() || box.min_x() > max_x() || box.max_y() < min_y() ||
      box.min_y() > max_y()) {
    return false;
  }

  const double shift_x = box.center_x() - center_.x();
  const double shift_y = box.center_y() - center_.y();

  const double dx1 = cos_heading_ * half_length_;
  const double dy1 = sin_heading_ * half_length_;
  const double dx2 = sin_heading_ * half_width_;
  const double dy2 = -cos_heading_ * half_width_;
  const double dx3 = box.cos_heading() * box.half_length();
  const double dy3 = box.sin_heading() * box.half_length();
  const double dx4 = box.sin_heading() * box.half_width();
  const double dy4 = -box.cos_heading() * box.half_width();

  return std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
             std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
                 std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
                 half_length_ &&
         std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
             std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
                 std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
                 half_width_ &&
         std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=
             std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) +
                 std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) +
                 box.half_length() &&
         std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=
             std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) +
                 std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) +
                 box.half_width();
}