自动驾驶-轨迹碰撞检测方法

目录

1.自动驾驶的碰撞检测怎么做

2.基于圆形的检测方法

3.基于AABB方法

4.基于OBB方法

---

1.自动驾驶的碰撞检测怎么做

 

如上图所示，自动驾驶系统中，通过感知模块，针对周围环境的障碍物，如人-车-自行车等，会输出一个长方体 框住，而感知模块则是将长方体的尺寸和坐标输入给下游的规划模块，用于给规划模块在生成轨迹时，做碰撞检测，那么规划模块是如何进行碰撞检测的呢？

自动驾驶场景，主要是针对一个二维平面在做碰撞检测，以下介绍几种典型的碰撞检测方法。

2.基于圆形的检测方法

典型的是用三圆包裹车辆，如上图所示，采用三个半径为1.3m的圆，那么可以计算各个圆心之间的距离，碰撞检查为：

所有的distance  > 2R 则表示无碰撞风险。

方法优缺点：计算速度快，但精度较低。

3.基于AABB方法

AABB ：Axis Aligned Bounding Box，即轴对齐的边界框

如图所示，这种方法为计算两个方框在二维平面的是否碰撞，计算方法很简单，

分别计算两个物体在X轴上的最小和最大值 X1min  X1max  以及 X2min  X2max

同时计算Y轴上的最小和最大值，Y1min, Y1max ,Y2min,Y2max

通过比较各个轴上的最大最小值是否有重叠的部分，若有任一轴上不存在重叠，则认为无碰撞。

4.基于OBB方法

OBB：Oriented Bounding Box  有方向的边界框

与AABB不同的是，OBB获取的方框是吻合到物体的方向，如红色框所示，这个时候选择的投影轴则有四个，如下图所示：

因此问题转化为，坐标点，到某一直线的投影，同样检测各个线段投影是否重叠判断是否存在碰撞，若任一一条投影不重叠，则认为不存在碰撞。

可进一步参考Appllo代码了解详情：

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

https://github.com/ApolloAuto/apollo/blob/576222dcc3a3d844f0385ec57fa85e1b2e515e7a/modules/common/math/box2d.cc