【自动驾驶】碰撞检测算法

参考链接:

【自动驾驶】碰撞检测算法 - 知乎

【规划】Box2d::HasOverlap() 碰撞检测接口详解_lemon_zy的博客-CSDN博客_box2d碰撞检测

一个常用方法即为超平面分离定理(Hyperplane Separation Theorem),用于检测N维欧几里得空间内的凸集是否存在交集,严格的定义可以参考维基百科。其二维情形被称为分离轴定理(Separating Axis Theorem),简要描述如下:

【自动驾驶】碰撞检测算法_第1张图片

两个平面凸集不相交的充要条件是,存在某条直线(即分离轴),使得两平面在直线上的投影不相交,类似的可以写出否命题。显然,要证明相交我们需要穷尽所有的直线,这是不可能的。幸运的是,对于凸多边形,我们只需要检查其边所在的直线是否为分离线(不是分离轴)即可。(大家可以思考一下对于凹多边形和圆怎么实现)

在自动驾驶的范畴内,对于障碍物一般用平面矩形表示(更高精度的会用平面凸多边形),对于两个矩形来说,我们只需要遍历4条边(每个矩形各自相邻的两条边)即可,只要这四条边有一条为分离轴那么两个矩形就不相交。相反,若两个矩形相交,则四条边都不是分离轴(比较自车和目标轮廓在分离轴上投影长度之和的一半,与投影距离的大小,投影距离全部小于投影及有重叠hasOverLap),即他们在上面的投影都有重合。

为了高效地表示平面矩形,我们一般会这样定义:

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];
};

投影原理:针对自车两条相邻边和障碍物相邻的两条边做投影【自动驾驶】碰撞检测算法_第2张图片

 我们只需要比较自车和目标轮廓在分离轴上投影长度之和的一半,与投影距离的大小,即可判断二者投影有没有相交。显然,自车在分离轴上的投影长度为length_。

以下代码是对应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_

 即vehicle任意一点不在obstacle对应的橙色虚线框内。

 

【自动驾驶】碰撞检测算法_第3张图片【自动驾驶】碰撞检测算法_第4张图片

以下代码是对应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()

【自动驾驶】碰撞检测算法_第5张图片 【自动驾驶】碰撞检测算法_第6张图片

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();
}

 

你可能感兴趣的:(自动驾驶,算法)