Apollo中Lattice轨迹碰撞检测

今天又向大佬们迈向一步～～～～～
Apollo中Lattice规划器的碰撞检测实现在HasOverlap中：

for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) {
      if (ego_box.HasOverlap(obstacle_box)) {
        return true;
      }
    }

轨迹碰撞检测目的

目的：剔除存在同障碍物碰撞风险的轨迹…如下，大致就是对于每个采样点，自车采用矩形box包络，障碍物们采用矩形box包络，然后检测是否有干涉。
原理：检查矩形框之间是否有重叠，自动泊车中车辆碰撞检测感觉也是类似方法，求指教~

两种常见的物体包络框：
OBB—Oriented Bounding Box
AABB—Axis Aligned Bounding Box
大致是jio样子：

Planning中自车采用OBB方式。在感知领域box用的很多，图像处理、激光雷达分割聚类这些需要对物体标记跟踪，当然，3D的box更复杂了，不懂就不乱写了。详细的介绍，可看这里

https://zhuanlan.zhihu.com/p/99911487 ：自动驾驶的碰撞检测原理并不新鲜，和2 D游戏中的碰撞检测原理相同，原理大多是超平面分割定理，难点在如何提高碰撞检测的效率或者精度，这个方法和技巧就不简单了

实施方法

1.建立自车Box

    const auto& trajectory_point =
        discretized_trajectory.TrajectoryPointAt(static_cast<std::uint32_t>(i));
    double ego_theta = trajectory_point.path_point().theta();
    Box2d ego_box(
        {trajectory_point.path_point().x(), trajectory_point.path_point().y()},
        ego_theta, ego_length, ego_width);
    // 车辆center和车辆的几何中心不重合，所以box需要校正一下
    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);

这块比较简单，锁定当前轨迹点，构建Box框，由于几何中心和车辆中心不一致(普通乘用车前后配比几乎没有1:1，除了柯尼塞格one:1，别说，说就是五菱宏光)，所以做了一下校正，及ego_box就是我们要的。

2.快速剔除非碰撞Box

因为检测过程是根据采样点遍历进行的，可以理解为逐帧进行，后边可以知道精确的碰撞检测其实是比较繁琐的，对于显而易见的不会碰撞，方法也是简单粗暴快捷明了：

if (box.max_x() < min_x() || box.min_x() > max_x() || box.max_y() < min_y() ||
      box.min_y() > max_y()) {
    return false;
  }

用图说话就是：凡是在灰色区域外障碍物box，都是一定没有发生碰撞的，这是一个充分不必要条件…因为，会有类似t3时刻的发生。上述方法只能用在轴对齐的box中，对于非轴对齐的，只能用于快速粗略的剔除不碰撞Box：

3.碰撞的精确检测

第2步中能够快速跳过无碰撞的时刻，对于出现t3时刻的情况，采取了另外的检测方法：分离轴定理。
通俗理解就是—投影。如果空间中两个物体，我们想知道是否接触，最直接的方法便是来回绕着看看，确认一下两者间是否有间隙，换个概念就是两个演员拍吻戏，从正面看是亲上了，从侧面看也亲上了，从上面看，额，也亲上了…那就是真亲上了，否则只要有一面是没亲上的，那就是假戏～
人是立体的，而且还是凸多面体，这方法可还行？好在Planning中，我们是以上帝视觉观测，是二维空间，事情更加简单，最少投影数：是几边形，就投几次影，apollo中采用的是凸四边形，所以四次投影，到位，分别是：
1.在自车纵轴上投影
2.在自车横轴上投影
3.在障碍物纵轴上投影
4.在障碍物横轴上投影
没图说个球，上图：
$$a_p=中心连线构成的向量在投影线上的投影长，b_p、c_p分别为自车、障碍物在投影线上的投影长度$$
判断的依据：
$$a_p\le b_p+c_p$$
成立，则说明两者在拍吻戏了…呸，两者有接触或碰撞的可能了。很明显，从下面这张图已经看出它俩是假戏了，并没有发生碰撞，不妨碍进行额外的三次确认，继续：

keep on going…

go go:

从其它三次其实是显示二者有重叠的，所以这四次确认需要一套做完，可以看到第2步粗暴高效的跳过了很多不必要的计算。

4.具体实现

详细可看另一位大神《分离轴定理的应用》，地址：https://zhuanlan.zhihu.com/p/99761177

投影这种计算，向量运算是很便捷的，
确定边界相对于自身坐标系的坐标→转换为笛卡尔坐标(全局)→向量常规运算→得到对应投影长度

Apollo具体代码部分：

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();
  // 对于OBB边框，使用分离轴定理进行碰撞检测
  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();

至此，障碍物碰撞检测over，函数外部根据返回值，做下一步处理即可。

---

充实一天到此结束…