碰撞检测技术介绍

自动驾驶决策规划模块中会经常使用到碰撞检测计算分析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
    
   
  
  
   P_1P_2
  
 
P1​P2​，有

 
  
   
    
     y
    
    
     0
    
   
  
  
   y_0
  
 
y0​可以计算得到点B的坐标，就可以判断射线是否与

 
  
   
    
     P
    
    
     1
    
   
   
    
     P
    
    
     2
    
   
  
  
   P_1P_2
  
 
P1​P2​相交了。此方法对多边形的凸凹性没有要求，但是如果点P在多边形边上或者顶点需要特殊处理。

structPoint{double x, y;};boolIsInPolygon(Point p,Point *ptPolygon,int ncount){int ncross =0;for(int i =0; i < ncount; i++){
    Point p1 = ptPolygon[i];
    Point p2 = ptPolygon[(i +1)% ncount];//相邻两条边p1,p2if(p1.y == p2.y)continue;if(p.y <min(p1.y, p2.y))continue;if(p.y >=max(p1.y, p2.y))continue;double x =(p.y - p1.y)*(p2.x - p1.x)/(p2.y - p1.y)+ p1.x;if(x > p.x)  ncross++;//只统计单边交点}return(ncross %2==1);}

1.3 轮廓六分圆法

此方法是将矩形的碰撞检测转化为圆之间的碰撞检测，通过两个圆的半径和圆心之间的距离判断两个圆是否重叠。给定车辆矩形轮廓，该算法首先计算矩形轮廓的外接圆，然后将整个矩形区域分解成与四个角点对齐的同等大小的正方形，轮廓矩形区域剩下的部分再进一步分解成等大小的小矩形，最后计算每一个小矩形或正方形的外接圆[1]。


此方法的计算精度损失较大，并且和矩形的长宽比例有关，可以看到当矩形为正方形时误差最小。此外此方法可以使用更多的圆计算矩形，并且更改圆的覆盖方法，但并不能彻底消除误差，同时更多的圆会增大计算时间。

• 矩形长度对横向误差影响较大；
• 矩形宽度对纵向误差影响较大；

2. Box和Box的碰撞检测

2.1 OpenCV方法

和上述方法一致。

2.2 SAT

如果凸多边形在某个轴上的投影不重叠，则两个凸多边形不相交。需要对所有的轴(每个边的法向量)进行投影，存在一个轴上的投影不相交，则两个凸多边形不相交。如果所有轴上的投影都相交，则多边形相交。
见之前的文章：Planning-碰撞检测之分离轴定理(SAT)
此外，SAT也可以用来计算Box和Point的位置关系。

2.3 GJK

    G
   
   
    J
   
   
    K
   
  
  
   GJK
  
 
GJK是基于

 
  
   
    M
   
   
    i
   
   
    n
   
   
    k
   
   
    o
   
   
    w
   
   
    s
   
   
    k
   
   
    i
   
  
  
   Minkowski
  
 
Minkowski

 
  
   
    S
   
   
    u
   
   
    m
   
  
  
   Sum
  
 
Sum概念上的，即形状1的所有点和形状2的所有点之和。


 
  
   
    A
   
   
    +
   
   
    B
   
   
    =
   
   
    {
   
   
    a
   
   
    +
   
   
    b
   
   
    ∣
   
   
    a
   
   
    ∈
   
   
    A
   
   
    ,
   
   
    b
   
   
    ∈
   
   
    B
   
   
    }
   
  
  
   A + B = \{a + b | a \in A, b \in B \}
  
 
A+B={a+b∣a∈A,b∈B}

如果

    s
   
   
    h
   
   
    a
   
   
    p
   
   
    e
   
   
    A
   
  
  
   shape A
  
 
shapeA和

 
  
   
    B
   
  
  
   B
  
 
B是凸的，则它们的和也是凸的

相应的可以定义它们的差运算：

    A
   
   
    −
   
   
    B
   
   
    =
   
   
    {
   
   
    a
   
   
    −
   
   
    b
   
   
    ∣
   
   
    a
   
   
    ∈
   
   
    A
   
   
    ,
   
   
    b
   
   
    ∈
   
   
    B
   
   
    }
   
  
  
   A - B = \{a - b | a \in A, b \in B \}
  
 
A−B={a−b∣a∈A,b∈B}

**如果两个形状重叠，进行

     M
    
    
     i
    
    
     n
    
    
     k
    
    
     o
    
    
     w
    
    
     s
    
    
     k
    
    
     i
    
   
   
    Minkowski
   
  
 Minkowski
 
  
   
    
     S
    
    
     u
    
    
     m
    
   
   
    Sum
   
  
 Sum后的形状包含原点**。

 
  
   
    M
   
   
    i
   
   
    n
   
   
    k
   
   
    o
   
   
    w
   
   
    s
   
   
    k
   
   
    i
   
  
  
   Minkowski
  
 
Minkowski

 
  
   
    S
   
   
    u
   
   
    m
   
  
  
   Sum
  
 
Sum的运算是

 
  
   
    s
   
   
    h
   
   
    a
   
   
    p
   
   
    e
   
  
  
   shape
  
 
shape

 
  
   
    A
   
  
  
   A
  
 
A的每个顶点和

 
  
   
    s
   
   
    h
   
   
    a
   
   
    p
   
   
    e
   
  
  
   shape
  
 
shape

 
  
   
    B
   
  
  
   B
  
 
B的所有顶点求和（或求差）。所得到点的外包络即是运算所得形状。

见之前的文章：Planning-碰撞检测之GJK

3. Benchmark

通过Benchmark分析，这里给出定性的计算结果：

3.1 Box和Point的碰撞检测

• 性能：轮廓六分圆 >> 射线法 >> OpenCV；
• 精度：射线法 >> OpenCV > 轮廓六分圆；

3.2 Box和Box的碰撞检测

• 性能：GJK > SAT >> OpenCV；
• 精度：GJK = SAT >> OpenCV； 当Polygon为四边形时，GJK和SAT的计算时间基本相等，但是随着Polygon边数的增多，GJK的性能优越性就凸显出来了。 具体代码实现以及Benchmark可见：collision_detection

4. 其他算法

4.1 Apollo中碰撞检测算法

Apollo的planning模块中的碰撞检测算法使用的有SAT和射线法两种。

4.1.1 SAT

• 车辆作为一个3D物体，降维到(x,y)二维上，使用长方形bounding box简化代替；
• 对bounding box进行AABB快速检测；
• 对bounding box进行OBB快速检测：应用SAT，用到坐标系旋转计算投影；

4.1.2 射线法

由于Box(矩形)是凸的，因此，当两个Box发生碰撞时，必然首先发生在角点处，即一个box的角点进入另一个box的内部。Apollo中使用射线法判断一个点是否在一个多边形内部。

for(constauto& point : ADCpoints){for(constauto& obstacle : obstacls){if(obstacle.IsPointIn(point)){return ture;}}}// 正确的做法应该同时进行：for(constauto& obstacle : obstacls){for(constauto& point : obstacle.points){if(ADC.IsPointIn(point)){return ture;}}}

boolPolygon2d::IsPointIn(const Vec2d &point)const{if(IsPointOnBoundary(point)){returntrue;}int j = num_points_ -1;int c =0;for(int i =0; i < num_points_;++i){if((points_[i].y()> point.y())!=(points_[j].y()> point.y())){constdouble side =CrossProd(point, points_[i], points_[j]);if(points_[i].y()< points_[j].y()? side >0.0: side <0.0){++c;}}
     j = i;}return c &1;}

上述Box的碰撞检测是根据矩形特点对SAT方法的简化，Polygon是任意多边形，则不再适用上述方法。由于Polygon是凸的，两个Polygon的最短距离必然发生在Polygon A的某个点和Polygon B的某个点，或者Polygon A的某个点和Polygon B的某条边之间。因此，判断两个Polygon是否有重合，转化为计算两个Polygon的最短距离是否小于等于0。其中用到了射线法判断点是否在Polygon内和点到线段的距离。

4.2 开源算法

• OpenGJK(GJK based Signed Volumes): https://github.com/MattiaMontanari/openGJK
• Bullet: https://github.com/bulletphysics/bullet3
• FCL: https://github.com/flexible-collision-library/fcl
• Box2D: https://github.com/erincatto/box2d

5. 参考文献：

[1] 张玉.自动驾驶车辆混合运动规划研究[D].北京理工大学,2018.