交叉路口碰撞预警（1）

       在V2X预警方面，有很多难点，其中之一就是交叉路口碰撞预警。

       交叉路口碰撞预警包含左转辅助预警和碰撞预警，其中的关键点就是车辆的运动轨迹和碰撞点的估计，最后根据车速对碰撞可能性进行判断。

       交叉路口碰撞是个比较复杂的场景，可以简单实现、也可以和路侧单元信息、雷达信息、图像信息融合，进行精确预警。

       简单实现：只基于V2X中的BSM消息，只有车与车的信息，没有路的信息，进行实现。

主要是基于本车与远车的经纬度、速度、距离、航向角，利用直线和园的方程进行碰撞点的预测。

方法一：

直接根据现有的车辆行驶方向进行判断，我们从最简单的直线轿车路口开始推导。

在最简单的十字路口，首先可以通过经纬度进行距离和方位判断。

通过经纬度计算出HV与RV的直线距离D（图中红线部分）和tmp值。

假设它们在交叉点碰撞，则车辆驶到碰撞点的时间分别为：

假设两车处在不规则的十字路口处，如图所示：

 利用十字路口的思路，最终要求取两车到碰撞点的距离。

d1=sin(tmp)*D;  d2=cos（tmp）*D;  d3=abs（d1*cos（angle）/sin（angle））；

d4=d2-d3; d5=sqrt(d1*d1+d3*d3);

到碰撞点的距离除以车辆速度，就是碰撞时间。