本文我介绍一下我发明的在自动驾驶中使用的滞后碰撞检测法。滞后碰撞检测的基本原理可以参考我的论文：
Dynamic Motion Planning Framework for Autonomous Driving in Urban Environments
核心思想就是把碰撞检测脱离开来，不再放在cost function里面，通常来说，很多人都会将碰撞检测作为cost function 中的一项，如果发生碰撞就会将cost function变成infinity。 但是由于我们采样点撒的很密集，如果每个采样周期都有100个点撒出去，意味着要做100次碰撞检测，这个有可能是NP-hard的。于是，不如直接把碰撞检测独立出来，我先把cost function按从小到大的顺序排好，然后从第一个开始做碰撞检测，如果幸运一开始就可能通过碰撞检测，就算开始碰到了障碍物，通过一段检测后也会通过碰撞检测，这个方法就有效的避免了遍历所有candidate曲线来找能够通过碰撞检测的最优曲线了。
而在碰撞检测方面，我们做简单化处理，为的还是加速运算。首先，对于碰撞检测半径，我们有很多选择：
论文：Fast Collision Checking for Intelligent Vehicle Motion Planning里面给出了一个将车辆分割成圆形的方法和公式。我们就按照三个圆的分割方案，计算出对应半径，这就是我们的碰撞最小安全距离指标。
在真实情况下，当我们知道了车辆的位置和朝向，从后轴中心往前画出这三个圆，然后计算生成的轨迹到障碍物的最短距离是否满足要求。对于计算最短距离这一步骤，有比较好的方案：
计算距离的过程中，我们通常是选择reference line上面的点到障碍物中心的距离来作为直线到点的距离，其实这个距离并不准确，事实上只有在reference line上面的点非常密集的时候，这个距离才相对准确，因此，我们还要多做一步。找到最短距离的点后，在这个点的前面和后面各取N个点，将这些点拟合成直线，计算障碍物中心点到直线的距离作为障碍物到reference line 的最短距离。这里的N是根据reference line 上面的点的密集程度来决定的。在短距离内，曲线是完全可以被拟合成直线的，而且计算出来的距离也相对上面的方法准确的多。
