自动驾驶碰撞检测算法

Posted 2022-07-12 微嵌电子

参考链接：

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

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

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

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

在自动驾驶的范畴内，对于障碍物一般用平面矩形表示（更高精度的会用平面凸多边形），对于两个矩形来说，我们只需要遍历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];
;

投影原理：针对自车两条相邻边和障碍物相邻的两条边做投影

 我们只需要比较自车和目标轮廓在分离轴上投影长度之和的一半，与投影距离的大小，即可判断二者投影有没有相交。显然，自车在分离轴上的投影长度为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对应的橙色虚线框内。

 

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