碰撞检测技术介绍

自动驾驶决策规划模块中会经常使用到碰撞检测计算分析Ego vehicle行为的安全性，并且可以用在planning计算的多个方面。例如下图中第一幅图，黄色车辆为主车，灰色车辆为交通参与车辆，其中一辆车辆在前方静止，另一辆车辆意图向右变道行驶。在此场景下，碰撞检测算法可以用来计算路径规划的SL边界值，如下图中第二幅图。也可以用来计算路径的安全走廊，如下图中的第三幅图。也可计算速度规划的ST图，如下图中的第四幅图等等。虽然碰撞检测在planning中是一个小算法模块，但是却至关重要。

本文将对常用的碰撞检测算法进行介绍，并简要的进行benchmark。在planning中一般将主车以及障碍物处理为凸多边形(Polygon/Box)，因此碰撞检测多是检测两个Polygon是否重叠，但是为了不失一般性，本文也将介绍Polygon和Point的位置关系计算方法，因为在两个Polygon的位置关系计算中可能会用到。
本文介绍的碰撞检测方法有：

• Box和Point的碰撞检测：
  • OpenCV方法；
  • 射线法；
  • 轮廓六分圆法；
• Box和Box的碰撞检测：
  • OpenCV方法；
  • SAT（分离轴定理）；
  • GJK；

1. Box和Point的碰撞检测

1.1 OpenCV

此方法使用了OpenCV开源库中的图像处理的方法，可以简要分为以下计算步骤：

• 根据碰撞检测环境范围(矩形)，建立两个cv::Mat数据；
• 将ADC位置或者轨迹车辆投影转化到cv::Mat中，被ADC占据位置数据设置为1(非零)，其他为0；
• 将障碍物投影转化到cv::Mat中，被据位置数据设置为1 (非零) ，其他为0；
• 两个cv::Mat进行求与操作，得到的cv::Mat中数据如果有非0数据，则发生碰撞；

1.2 射线法

判断一个点是否在多边形内，我们可以从该点引出一条水平射线（任意射线都可，但水平便于计算），观察射线与多变形的交点个数，如果交点个数为奇数，则该点在多边形内，如果为偶数则在多边形外。如下左图所示，判断点P和多边形的关系，从点P得到一个水平向右的射线，通过多边形的每两个相邻顶点可以得到边的直线方程，例如$P_1{P}_2$，有$y_0$可以计算得到点B的坐标，就可以判断射线是否与$P_1{P}_2$相交了。此方法对多边形的凸凹性没有要求，但是如果点P在多边形边上或者顶点需要特殊处理。

struct Point{
   
   
  double x, y;
};
bool IsInPolygon(Point p,Point *ptPolygon,int ncount) {
   
   
  int ncro