交叉路口防撞预警装置及预警方法与流程

本发明涉及汽车技术领域，更具体地说，涉及汽车的安全技术。

背景技术：

随着汽车保有量的不断增加，城市道路交通状况日益复杂，交叉路口区域成为交通事故的高发地带。据数据统计，90％的重特大交通事故都发生在交叉路口。

为了提升行车安全性，越来越多的汽车配备了主动安全技术。主动安全技术一般通过车载的摄像头或者毫米波雷达来探测车辆周边目标，当周围的目标与车辆距离过近，或者处于车辆的行驶路线上时，会进行提示或报警，在预计到存在发生碰撞的危险时，还会通过整车控制器采取主动的减速、制动等等措施，来降低或者避免发生碰撞的危险。现有技术中的主动安全技术多数是通过计算碰撞时间ttc来进行碰撞预警。碰撞时间ttc是根据本车辆与目标之间的距离以及相对速度来计算相遇的时间，即碰撞时间ttc，如果ttc小于设定的安全阈值，则会进行提示，并采取主动的安全措施，比如减速或者制动。

基于ttc的主动安全技术在跟车场景中的表现较好。基于ttc的主动安全技术，比如自动紧急制动aeb、自适应控制巡航acc等，在路况较为简单的封闭公路上有良好的表现。在封闭公路，比如高速公路上，车辆都是同方向行驶，大多数情况下都是直线行驶，且行驶路径上除了目标之外，很少有其他的障碍物。诸如摄像头和毫米波雷达的传感器能够准确探测到目标的位置。同时，在简单路况下，本车辆和目标的行驶路线能够准确预测。在跟车场景下，本车辆与目标之间的相对速度是车速的差，相对较小，因而能够提供较多的反应时间。这样，对于车辆路径的预判比较准确，并且反应时间较充分，有较为充足的时间进行计算、判断、提示、动作等。

但单纯基于ttc的主动安全技术在交叉路口的表现不佳。交叉路口路况复杂，首先是交叉路口的地形复杂，可能存在建筑物、各种固定设施等，会遮挡诸如摄像头和毫米波雷达的传感器的视线，造成探测的盲区。其次交叉路口的车辆数量众多，行驶方向复杂，有同向、相对向、横向、还有转弯向的车辆。各车辆的行驶方向复杂多变，使得车辆的行驶轨迹的预测变得困难，尤其是转弯车辆的行驶轨迹的预测难度较高。在遇到相对向、横向、还有转弯的车辆时，本车辆与目标之间的相对速度是两者车速的不同程度的叠加，由于相对速度较大，因此提供的反应时间较少。在交叉路口，需要进行预判的计算量很大，但提供的反应时间又很少，并且路径和速度的预测较为困难，使得基于ttc的主动安全技术的表现不佳。此外，在城市的交叉路口场景中，还存在大量的行人和非机动车，行人和非机动车的存在使得主动安全措施的计算量更大，进一步加剧了处理的难度。

技术实现要素：

本发明旨在提出一种利用v2x通信技术和高精度导航来提升安全性的交叉路口的防撞预警技术。

根据本发明的一实施例，提出一种交叉路口防撞预警装置，包括：车载单元和路侧单元，车载单元安装在车辆上，路侧单元安装在交叉路口的路侧。车载单元包括：车端v2x通信设备、导航定位设备和整车控制器。车端v2x通信设备与其他车端v2x通信设备以及路端v2x通信设备进行通信。导航定位设备基于rtk，导航定位设备获取本车辆的位置信息和移动轨迹。整车控制器与车端v2x通信设备以及导航定位设备连接，整车控制器进行碰撞预警计算并基于碰撞预警计算的结果向本车辆发出控制指令，控制车辆的移动。路侧单元包括：路端v2x通信设备和目标捕捉设备。路端v2x通信设备与车端v2x通信设备进行通信。目标捕捉设备捕捉交叉路口区域的移动目标，获取移动目标的位置信息和移动轨迹，移动目标不具有v2x通信设备。车辆进入交叉路口区域，车辆通过车端v2x通信设备发送由导航定位设备获取的本车辆的位置信息和移动轨迹，同时通过车端v2x通信设备接受其他车辆的位置信息和移动轨迹，以及由目标捕捉设备获取的移动目标的位置信息和移动轨迹。整车控制器依据各车辆和移动目标的位置信息和移动轨迹计算预测航迹，并根据预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域，如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹，基于航迹交叉区域计算碰撞时间ttc和航迹交叉区域占用时间差pet，根据ttc和pet进行碰撞预警计算。

在一个实施例中，车载单元连接到显示设备，显示设备显示地图，在地图上显示本车辆的位置信息和移动轨迹，还在地图上显示交叉路口区域其他车辆以及移动目标的位置信息和移动轨迹。如果整车控制器进行碰撞预警计算后确定本车辆与其他车辆或者移动目标存在碰撞危险，则该显示设备在地图上显示该其他车辆或者移动目标的位置信息和移动轨迹，以及本车辆与该其他车辆或者移动目标的航迹交叉区域。

在一个实施例中，整车控制器依据各车辆和移动目标的位置信息和移动轨迹，通过车辆状态预估模型和扩展卡尔曼滤波计算各车辆和移动目标的预测航迹。

在一个实施例中，车辆状态预估模型依据各车辆和移动目标的移动轨迹计算各车辆和移动目标的初步预测航迹，其中，依据车辆或移动目标的移动轨迹，车辆或移动目标被分为直行轨迹和转弯轨迹。对于直行轨迹的车辆或移动目标，初步预测航迹为车辆或移动目标保持当前的移动速度，沿当前移动轨迹做匀速直线移动。对于转弯轨迹的车辆或移动目标，计算初步预测航迹如下：输入车辆或移动目标的速度和横摆角速度，以及交叉路口的几何形状；对于转弯轨迹的车辆或移动目标，其在交叉路口的运动过程分为加速和减速两个阶段，分别用三次曲线进行拟合，曲线的拟合系数为a1,a2,a3,a4，拟合时间变量为t，两条曲线的分界点为速度最小点vmin，该时刻的时间点记为tmin，在分界点处的加速度为零，交叉路口的总长度为l：

减速阶段的速度v1的变化为：

v1＝a1t³+a2t²+a3t+a4

加速阶段的运动过程使用减速阶段的相反运动过程来模拟，加速阶段的速度v2的变化为：

v2＝a1(2tmin-t)³+a2(2tmin-t)²+a3(2tmin-t)+a4

由对称性可以得到减速阶段的运动距离为l/2，则上述的约束条件可以表示为：

3a1tmin²+2a2tmin+a3＝0

结合车辆或移动目标刚进入交叉路口时的速度，进行在上述约束条件下的三次曲线拟合，得到车辆或移动目标的速度预测曲线。

在一个实施例中，根据速度预测曲线，进行两段曲线拟合：第一段曲线的约束条件为tmin处的角度值为45°，tmin处的曲线斜率为1，两段拟合曲线关于点(tmin，45)对称；

在上述约束条件下进行三次曲线拟合，得到车辆或移动目标的横摆角速度预测曲线；

根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线的到车辆或移动目标的初步预测航迹。

在一个实施例中，对初步预测航迹应用扩展卡尔曼滤波，得到车辆或移动目标的预测航迹，其中，根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，得到车辆或移动目标的预测航迹。

在一个实施例中，扩展卡尔曼滤波是指，对于非线性的速度预测曲线和横摆角速度预测曲线，进行局部线性化，在局部应用匀加速运动模型得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，得到车辆或移动目标的预测航迹。将经局部线性化得到的车辆或移动目标的预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，输入卡尔曼滤波基本方程，包括：

将测量噪声和系统过程噪声设置为零均值的高斯白噪声，随机线性离散系统的状态方程和量测方程为：

xk＝φk|k-1xk-1+γk|k-1wk-1

zk＝hkxk+vk

式中：xk是系统的n维系统状态向量；zk是系统的m维量测向量；φk|k-1是系统n×n维一步状态转移矩阵；γk|k-1是n×p维噪声输入矩阵；hk是m×n维量测矩阵；wk-1是p维系统过程噪声；vk是m维量测噪声；

状态估计：

滤波增益矩阵：

一步预测误差：

pk|k-1＝φk|k-1pkφ^tk|k-1+γk|k-1qk-1γ^tk|k-1

估计误差方差阵：

pk＝(i-kkhk)pk|k-1(i-kkhk)^t+kkrkkk^t＝(i-kkhk)pk|k-1

根据上述随机线性离散系统的卡尔曼滤波基本方程，给定初值和p0，根据k时刻的量测值zk，能递推计算出k时刻的状态估计

将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值输入，得到车辆或移动目标的预测航迹。

在一个实施例中，根据各车辆或者移动目标的预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域，如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹，分别计算：

本车辆从当前位置进入航迹交叉区域的时间t1、本车辆从当前位置离开航迹交叉区域的时间t2、目标车辆或者移动对象从当前位置进入航迹交叉区域的时间t3、目标车辆或者移动目标当前位置离开航迹交叉区域的时间t4；

对于本车辆先通过航迹交叉区域的情况，航迹交叉区域占用时间差pet计算为(t3-t2)，pet为正值；

针对目标车辆或者移动对象先通过航迹交叉区域的情况，航迹交叉区域占用时间差pet计算为(t4-t1)，pet为负值；

碰撞时间ttc计算为本车辆从当前位置进入航迹交叉区域的时间t1；

根据碰撞时间ttc以及航迹交叉区域占用时间差pet的绝对值的数值，进行碰撞预警计算。

在一个实施例中，整车控制器依据碰撞预警计算的结果向本车辆发出控制指令，包括发出预警信号、控制车辆减速或者控制车辆制动。

在一个实施例中，导航定位设备基于实时动态载波相位差分技术rtk，导航定位设备包括全球导航卫星系统接收器和惯性测量单元，导航定位设备进行实时厘米级定位。

在一个实施例中，目标捕捉设备包括摄像头和毫米波雷达。

根据本发明的一实施例，提出一种交叉路口防撞预警方法，包括：

车辆进入交叉路口区域，车辆发送由导航定位设备获取的本车辆的位置信息和移动轨迹，同时获取其他车辆或者移动目标的位置信息和移动轨迹；

各车辆和移动目标的位置信息和移动轨迹计算预测航迹；

根据预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域；

如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹，基于航迹交叉区域计算碰撞时间ttc和航迹交叉区域占用时间差pet，根据ttc和pet进行碰撞预警计算；

根据碰撞预警计算的结果对本车辆发出控制指令。

在一个实施例中，车辆装备有车载单元，车载单元包括：车端v2x通信设备、导航定位设备和整车控制器。车端v2x通信设备与其他车端v2x通信设备以及路端v2x通信设备进行通信。导航定位设备基于rtk，导航定位设备获取本车辆的位置信息和移动轨迹。整车控制器与车端v2x通信设备以及导航定位设备连接，整车控制器进行碰撞预警计算并基于碰撞预警计算的结果向本车辆发出控制指令，控制车辆的移动。交叉路口的路侧装有路侧单元，路侧单元包括：路端v2x通信设备和目标捕捉设备。路端v2x通信设备与车端v2x通信设备进行通信。目标捕捉设备捕捉交叉路口区域的移动目标，获取移动目标的位置信息和移动轨迹，移动目标不具有v2x通信设备。车辆进入交叉路口区域时，车辆通过车端v2x通信设备发送由导航定位设备获取的本车辆的位置信息和移动轨迹，同时通过车端v2x通信设备接受其他车辆的位置信息和移动轨迹，以及由目标捕捉设备获取的移动目标的位置信息和移动轨迹。

在一个实施例中，该交叉路口防撞预警方法还包括：显示地图，并在地图上显示本车辆的位置信息和移动轨迹，还在地图上显示交叉路口区域其他车辆以及移动目标的位置信息和移动轨迹，如果整车控制器进行碰撞预警计算后确定本车辆与其他车辆或者移动目标存在碰撞危险，则该显示设备在地图上显示该其他车辆或者移动目标的位置信息和移动轨迹，以及本车辆与该其他车辆或者移动目标的航迹交叉区域。

在一个实施例中，各车辆和移动目标的位置信息和移动轨迹计算预测航迹包括：通过车辆状态预估模型和扩展卡尔曼滤波计算各车辆和移动目标的预测航迹。

在一个实施例中，车辆状态预估模型依据各车辆和移动目标的移动轨迹计算各车辆和移动目标的初步预测航迹，其中，

依据车辆或移动目标的移动轨迹，车辆或移动目标被分为直行轨迹和转弯轨迹；

对于直行轨迹的车辆或移动目标，初步预测航迹为车辆或移动目标保持当前的移动速度，沿当前移动轨迹做匀速直线移动；

对于转弯轨迹的车辆或移动目标，计算初步预测航迹如下：

输入车辆或移动目标的速度和横摆角速度，以及交叉路口的几何形状；

对于转弯轨迹的车辆或移动目标，其在交叉路口的运动过程分为加速和减速两个阶段，分别用三次曲线进行拟合，曲线的拟合系数为a1,a2,a3,a4，拟合时间变量为t，两条曲线的分界点为速度最小点vmin，该时刻的时间点记为tmin，在分界点处的加速度为零，交叉路口的总长度为l：

减速阶段的速度v1的变化为：

v1＝a1t³+a2t²+a3t+a4

加速阶段的运动过程使用减速阶段的相反运动过程来模拟，加速阶段的速度v2的变化为：

v2＝a1(2tmin-t)³+a2(2tmin-t)²+a3(2tmin-t)+a4

由对称性可以得到减速阶段的运动距离为l/2，则上述的约束条件可以表示为：

3a1tmin²+2a2tmin+a3＝0

结合车辆或移动目标刚进入交叉路口时的速度，进行在上述约束条件下的三次曲线拟合，得到车辆或移动目标的速度预测曲线。

在一个实施例中，根据速度预测曲线，进行两段曲线拟合：第一段曲线的约束条件为tmin处的角度值为45°，tmin处的曲线斜率为1，两段拟合曲线关于点(tmin，45)对称；

在上述约束条件下进行三次曲线拟合，得到车辆或移动目标的横摆角速度预测曲线；

根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线的到车辆或移动目标的初步预测航迹。

在一个实施例中，对初步预测航迹应用扩展卡尔曼滤波，得到车辆或移动目标的预测航迹，其中，

根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，得到车辆或移动目标的预测航迹。

在一个实施例中，扩展卡尔曼滤波是指，对于非线性的速度预测曲线和横摆角速度预测曲线，进行局部线性化，在局部应用匀加速运动模型得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，得到车辆或移动目标的预测航迹；

将经局部线性化得到的车辆或移动目标的预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，输入卡尔曼滤波基本方程，包括：

将测量噪声和系统过程噪声设置为零均值的高斯白噪声，随机线性离散系统的状态方程和量测方程为：

xk＝φk|k-1xk-1+γk|k-1wk-1

zk＝hkxk+vk

式中：xk是系统的n维系统状态向量；zk是系统的m维量测向量；φk|k-1是系统n×n维一步状态转移矩阵；γk|k-1是n×p维噪声输入矩阵；hk是m×n维量测矩阵；wk-1是p维系统过程噪声；vk是m维量测噪声；

状态估计：

滤波增益矩阵：

一步预测误差：

pk|k-1＝φk|k-1pkφ^tk|k-1+γk|k-1qk-1γ^tk|k-1

估计误差方差阵：

pk＝(i-kkhk)pk|k-1(i-kkhk)^t+kkrkkk^t＝(i-kkhk)pk|k-1

根据上述随机线性离散系统的卡尔曼滤波基本方程，给定初值和p0，根据k时刻的量测值zk，能递推计算出k时刻的状态估计

将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值输入，得到车辆或移动目标的预测航迹。

在一个实施例中，根据各车辆或者移动目标的预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域，如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹，分别计算：

本车辆从当前位置进入航迹交叉区域的时间t1、本车辆从当前位置离开航迹交叉区域的时间t2、目标车辆或者移动对象从当前位置进入航迹交叉区域的时间t3、目标车辆或者移动目标当前位置离开航迹交叉区域的时间t4；

对于本车辆先通过航迹交叉区域的情况，航迹交叉区域占用时间差pet计算为(t3-t2)，pet为正值；

针对目标车辆或者移动对象先通过航迹交叉区域的情况，航迹交叉区域占用时间差pet计算为(t4-t1)，pet为负值；

碰撞时间ttc计算为本车辆从当前位置进入航迹交叉区域的时间t1；

根据碰撞时间ttc以及航迹交叉区域占用时间差pet的绝对值的数值，进行碰撞预警计算。

在一个实施例中，整车控制器依据碰撞预警计算的结果向本车辆发出控制指令，包括发出预警信号、控制车辆减速或者控制车辆制动。

本发明的交叉路口防撞预警装置和防撞预警方法利用v2x通信技术和高精度导航定位系统，结合设置在路侧的路侧传感装置，能够在车辆进入到交叉路口区域时预先掌握附近的其他车辆或者移动目标的信息，然后根据车辆状态预估模型和扩展卡尔曼滤波计算各车辆和移动目标的预测航迹。根据预测航迹判断是否存在航迹交叉区域，根据航迹交叉区域计算碰撞时间ttc以及航迹交叉区域占用时间差pet，进行防撞预警，具有较高的预警准确性。

附图说明

本发明上述的以及其他的特征、性质和优势将通过下面结合附图和实施例的描述而变的更加明显，在附图中相同的附图标记始终表示相同的特征，其中：

图1揭示了根据本发明的一实施例的交叉路口防撞预警装置的结构框图。

图2揭示了根据本发明的一实施例的交叉路口防撞预警装置中直行轨迹的示意图。

图3揭示了根据本发明的一实施例的交叉路口防撞预警装置中转弯轨迹的示意图。

图4揭示了根据本发明的一实施例的交叉路口防撞预警装置中进行碰撞预警计算的示意图。

图5揭示了根据本发明的一实施例的交叉路口防撞预警方法的执行过程。

具体实施方式

首先参考图1所示，图1揭示了根据本发明的一实施例的交叉路口防撞预警装置的结构框图。该交叉路口防撞预警装置包括：车载单元102和路侧单元104，车载单元102安装在车辆上，路侧单元104安装在交叉路口的路侧。在图1所示的实施例中，示例性地示出了两个车载单元102和一个路侧单元104，以说明车载单元与车载单元、车载单元与路侧单元之间通信路径。需要说明的是，在实际的应用中，车载单元和路侧单元的数量并没有限制，路侧单元的数量可以根据交叉路口的实际情况设置。而车载单元是安装在车辆上，是由进入到交叉路口区域的车辆的数量决定的。

在图1所示的是实例中，车载单元102包括：车端v2x通信设备121、导航定位设备122和整车控制器123。车端v2x通信设备121与其他车端v2x通信设备以及路端v2x通信设备进行通信。导航定位设备122基于rtk，导航定位设备122获取本车辆的位置信息和移动轨迹。在一个实施例中，导航定位设备基于实时动态载波相位差分技术rtk，rtk(real-timekinematic)实时动态载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。这是一种新的常用的卫星定位测量方法，rtk不需要进行后台计算，能够实时得到厘米级的定位精度。在一个实施例中，导航定位设备包括全球导航卫星系统gnss接收器和惯性测量单元imu。利用rtk，导航定位设备进行实时厘米级定位。整车控制器123与车端v2x通信设备121以及导航定位设备122连接，整车控制器123进行碰撞预警计算并基于碰撞预警计算的结果向本车辆发出控制指令，控制车辆的移动。在一个实施例中，整车控制器123依据碰撞预警计算的结果向本车辆发出控制指令，包括发出预警信号、控制车辆减速或者控制车辆制动。

继续参考图1所示，路侧单元104包括：路端v2x通信设备141和目标捕捉设备。路端v2x通信设备141与车端v2x通信设备121进行通信。目标捕捉设备捕捉交叉路口区域的移动目标，获取移动目标的位置信息和移动轨迹，这些移动目标不具有v2x通信设备。在一个实施例中，车端v2x通信设备之间、车端v2x通信设备与路端v2x通信设备之间都通过v2x进行通信。在图1所示的实施例中，车端v2x通信设备之间的v2x通信被标记为v2v，车端v2x通信设备与路端v2x通信设备之间的v2x通信被标记为v2i或者i2v。在一个实施例中，v2x通信采用udp协议，udp数据协议格式包括报文头、数据长度、数据、检验位以及报文尾。rtk数据格式采用gpfpd协议，格式包括报文头、utc时间、航偏角、俯仰角、翻滚角、经纬度、海拔、东北向速度、天向速度、基线长度、卫星数、系统状态、校验以及报文尾。由于进入到交叉路口区域的车辆或者其他移动目标，例如行人或者非机动车并不是都安装有v2x通信设备，对于没有v2x通信设备的车辆或者移动目标，在本发明中都定义为移动目标，即本发明所称的移动目标是不具有v2x通信设备的。路侧单元104通过目标捕捉设备来捕捉交叉路口区域的移动目标，在一个实施例中，目标捕捉设备包括摄像头142和毫米波雷达143。根据交叉路口的实际情况，摄像头142和毫米波雷达143可以布置多组，以避免出现监测盲区。

车辆进入交叉路口区域时，车辆通过车端v2x通信设备发送由导航定位设备获取的本车辆的位置信息和移动轨迹，本车辆的位置信息和移动轨迹以udp组播形式发送。同时通过车端v2x通信设备与通信范围内的其他车辆的车端v2x通信设备以及路侧单元的路端v2x通信设备通信，接受其他车辆的位置信息和移动轨迹，以及由路侧单元的目标捕捉设备获取的移动目标的位置信息和移动轨迹。在一个实施例中，路侧单元的目标捕捉设备会同时获取监测范围内所有目标的位置信息和移动轨迹，然后根据路端v2x通信设备所接收到的车端v2x通信设备发送的信号，从上述的目标中过滤掉已经通过v2x通信确认的目标，这样，留下的目标都是不具备v2x通信设备的移动目标。通过目标捕捉设备的目标捕捉，能够与v2x通信形成冗余，对于v2x通信设备故障或者通信受阻的情况，也能够捕捉到目标的位置信息和移动轨迹。

在一个实施例中，车载单元连接到显示设备。如果车辆本身具有显示设备，例如中控显示器，则车载单元可以连接到中控显示器，如果车辆本身不具有显示设备，则车载单元可以配置一个显示器。通过显示设备显示地图，在一个实施例中，为了配合本发明的厘米级定位，所显示的地图也是达到厘米级分辨率的高精度地图。在地图上显示本车辆的位置信息和移动轨迹，同时还在地图上显示交叉路口区域其他车辆以及移动目标的位置信息和移动轨迹。本车辆的位置信息和移动轨迹由导航定位设备提供，交叉路口区域其他车辆以及移动目标的位置信息和移动轨迹通过车端v2x通信设备获得。

整车控制器依据各车辆和移动目标的位置信息和移动轨迹计算预测航迹，并根据预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域。如果不存在航迹交叉区域，那么就判断为不存在发生碰撞的危险。如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹，基于航迹交叉区域计算碰撞时间ttc和航迹交叉区域占用时间差pet，根据ttc和pet进行碰撞预警计算。在一个实施例中，如果整车控制器进行碰撞预警计算后确定本车辆与其他车辆或者移动目标存在碰撞危险，则通过显示设备在地图上显示该其他车辆或者移动目标的位置信息和移动轨迹，以及本车辆与该其他车辆或者移动目标的航迹交叉区域。

在一个实施例中，整车控制器依据各车辆和移动目标的位置信息和移动轨迹，通过车辆状态预估模型和扩展卡尔曼滤波计算各车辆和移动目标的预测航迹。

整车控制器根据车辆状态预估模型，依据各车辆和移动目标的移动轨迹计算各车辆和移动目标的初步预测航迹，其中，依据车辆或移动目标的移动轨迹，车辆或移动目标被分为直行轨迹和转弯轨迹。图2揭示了根据本发明的一实施例的交叉路口防撞预警装置中直行轨迹的示意图。本发明中所称的直行轨迹是指与本车辆的行驶方向基本垂直的车辆的行驶轨迹。主要是交叉路口中横向行驶的车辆。对于与本车辆处于平行行驶方向的同向行驶车辆或者对向行驶车辆，在正常情况下(按照交通规则行驶)，应当处于不同的车道上，车辆之间不存在路径的交叉，因此本发明对于这种情况未作考虑。在图2中，c1代表本车辆，c2代表目标车辆，ez表示本车辆与目标车辆在行驶航迹上所存在的交叉区域。图3揭示了根据本发明的一实施例的交叉路口防撞预警装置中转弯轨迹的示意图。转弯轨迹是交叉路口常见的行驶路径，也是较难预测的路径，多数交通事故的发生均与转弯路径有关。在图3中，c1代表本车辆，c2代表目标车辆，ez表示本车辆与目标车辆在行驶航迹上所存在的交叉区域。

根据本发明的车辆状态预估模型，对于直行轨迹的车辆或移动目标，初步预测航迹为车辆或移动目标保持当前的移动速度，沿当前移动轨迹做匀速直线移动。

对于转弯轨迹的车辆或移动目标，计算初步预测航迹如下：

输入车辆或移动目标的速度和横摆角速度，以及交叉路口的几何形状。将车辆或移动目标的速度和横摆角速度，以及交叉路口的几何形状作为两个输入变量。

对于转弯轨迹的车辆或移动目标，其在交叉路口的运动过程分为加速和减速两个阶段，减速阶段和加速阶段分别用三次曲线进行拟合。曲线的拟合系数为a1,a2,a3,a4，拟合时间变量为t，减速曲线和加速曲线这两条曲线的分界点为速度最小点vmin，该时刻的时间点记为tmin，在分界点处的加速度为零，交叉路口的总长度为l：

减速阶段的速度v1的变化为：

v1＝a1t³+a2t²+a3t+a4

加速阶段由于受到驾驶员驾驶习惯的影响，存在一定的不确定性，为了简化模型，减少计算量，在本发明中加速阶段的运动过程使用减速阶段的相反运动过程来模拟，因此加速阶段的速度v2的变化为：

v2＝a1(2tmin-t)³+a2(2tmin-t)²+a3(2tmin-t)+a4

由对称性可以得到减速阶段的运动距离为l/2，则上述的约束条件可以表示为：

3a1tmin²+2a2tmin+a3＝0

结合车辆或移动目标刚进入交叉路口时的速度，进行在上述约束条件下的三次曲线拟合，得到车辆或移动目标的速度预测曲线。其中，曲线的拟合系数a1,a2,a3,a4，以及拟合时间变量t可以根据实际需求而确定。关于拟合系数a1,a2,a3,a4以及拟合时间变量t的具体取值，本发明中不做限定。

在得到车辆或移动目标的速度预测曲线后，根据速度预测曲线计算车辆或移动目标的横摆角速度预测曲线。在一个实施例中，根据速度预测曲线，进行两段曲线拟合，两段曲线分别是对应加速阶段和减速阶段：第一段曲线的约束条件为tmin处的角度值为45°，tmin处的曲线斜率为1，两段拟合曲线关于点(tmin，45)对称。在上述约束条件下进行三次曲线拟合，得到车辆或移动目标的横摆角速度预测曲线。

在获得车辆或移动目标的速度预测曲线和横摆角速度预测曲线后，根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线的到车辆或移动目标的初步预测航迹。

对初步预测航迹应用扩展卡尔曼滤波，得到车辆或移动目标的预测航迹。卡尔曼滤波(kalmanfiltering)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。在一个实施例中，根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，得到车辆或移动目标的预测航迹。

因为卡尔曼滤波主要是针对线性系统，而速度预测曲线和横摆角速度预测曲线是非线性的。所以在本发明中，对于速度预测曲线和横摆角速度预测曲线应用扩展卡尔曼滤波算法(ekf)，扩展卡尔曼滤波是指，对于全局呈现非线性的速度预测曲线和横摆角速度预测曲线，进行局部线性化，经过局部线性化处理后的速度预测曲线和横摆角速度预测曲线在局部可以视为是线性的。这样在局部应用匀加速运动模型得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，进行最优估计，得到车辆或移动目标的预测航迹。

在一个实施例中，将经局部线性化得到的车辆或移动目标的预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，输入卡尔曼滤波基本方程，包括：

将测量噪声和系统过程噪声设置为零均值的高斯白噪声，随机线性离散系统的状态方程和量测方程为：

xk＝φk|k-1xk-1+γk|k-1wk-1

zk＝hkxk+vk

式中：xk是系统的n维系统状态向量；zk是系统的m维量测向量；φk|k-1是系统n×n维一步状态转移矩阵；γk|k-1是n×p维噪声输入矩阵；hk是m×n维量测矩阵；wk-1是p维系统过程噪声；vk是m维量测噪声；

状态估计：

滤波增益矩阵：

一步预测误差：

pk|k-1＝φk|k-1pkφ^tk|k-1+γk|k-1qk-1γ^tk|k-1

估计误差方差阵：

pk＝(i-kkhk)pk|k-1(i-kkhk)^t+kkrkkk^t＝(i-kkhk)pk|k-1

根据上述随机线性离散系统的卡尔曼滤波基本方程，给定初值和p0，根据k时刻的量测值zk，能递推计算出k时刻的状态估计

将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值输入，进行最优估计，得到车辆或移动目标的预测航迹。

在对各个车辆以及移动目标分别计算获得其预测航迹后，根据各车辆或者移动目标的预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域。如果不存在航迹交叉区域，则认为本车辆和目标车辆或者移动目标之间不存在碰撞的危险。如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹、以及航迹交叉区域的位置来进行碰撞预警计算。图4揭示了根据本发明的一实施例的交叉路口防撞预警装置中进行碰撞预警计算的示意图。

参考图4所示，需要计算的参数包括：

在存在航迹交叉区域ez的场景中，车辆要安全地通过航迹交叉区域ez，有两种情况：本车辆先于目标对象通过ez区域，或者本车辆后于目标对象通过ez区域。如果本车辆与目标对象通过ez区域的时间存在重叠，则存在碰撞的风险。

两种安全情况的安全条件分别为：

对于本车辆先于目标对象通过ez区域，本车辆驶离ez区域后，目标对象再进入ez区域。

对于本车辆后于目标对象通过ez区域，目标对象驶离ez区域后，本车辆再进入ez区域。

因此，可以定义四个时间对是否产生碰撞危险进行判定，分别是：

本车辆c1从当前位置进入航迹交叉区域ez的时间t1(车头进入航迹交叉区域的近端)、本车辆c1从当前位置离开航迹交叉区域ez的时间t2(车尾离开航迹交叉区域的远端)、目标车辆或者移动对象c2从当前位置进入航迹交叉区域ez的时间t3(车头进入航迹交叉区域的近端)、目标车辆或者移动目标c2当前位置离开航迹交叉区域ez的时间t4(车尾离开航迹交叉区域的远端)。根据这四个参数来计算航迹交叉区域占用时间差pet以及碰撞时间ttc。

对于本车辆c1先通过航迹交叉区域ez的情况，航迹交叉区域占用时间差pet计算为(t3-t2)，pet为正值；

针对目标车辆或者移动对象c2先通过航迹交叉区域ez的情况，航迹交叉区域占用时间差pet计算为(t4-t1)，pet为负值；

碰撞时间ttc计算为本车辆从当前位置进入航迹交叉区域的时间t1；

根据碰撞时间ttc以及航迹交叉区域占用时间差pet的绝对值的数值，进行碰撞预警计算。

在本车辆和目标对象都还未进入ez区域时，每个时刻都对应着一个预估pet。在没发生碰撞的情况下，如果本车辆先离开ez区域，则此场景的预估pet为(t3-t2)，且预估pet为正值；如果目标对象先离开ez区域，则此场景的预估pet为(t4-t1)，且预估pet为负值。如果发生碰撞，预估pet等于0。当预估pet的绝对值越小，代表两车通过路口的时间越接近，发生碰撞的可能性越大。但仅凭预估pet的绝对值，不能确定场景是否真正危险。比如，当t1很大时，即使pet绝对值很小，也不会立刻发生碰撞，因为本车辆到达ez区域的时间还很长，留给驾驶员反应的时间很长。只有当t1及预估pet的绝对值同时小于一定界限时，才会真正存在碰撞危险。因此本发明选择本车辆进入ez区域的时间t1作为ttc，与预估pet共同判定是否存在碰撞危险。

整车控制器依据碰撞预警计算的结果向本车辆发出控制指令，包括发出预警信号、控制车辆减速或者控制车辆制动。在一个实施例中，如果整车控制器进行碰撞预警计算后确定本车辆与其他车辆或者移动目标存在碰撞危险，还通过显示设备在地图上显示该其他车辆或者移动目标的位置信息和移动轨迹，以及本车辆与该其他车辆或者移动目标的航迹交叉区域。

本发明还揭示了一种交叉路口防撞预警方法，图5揭示了根据本发明的一实施例的交叉路口防撞预警方法的执行过程。该交叉路口防撞预警方法，包括：

车辆进入交叉路口区域，车辆发送由导航定位设备获取的本车辆的位置信息和移动轨迹，同时获取其他车辆或者移动目标的位置信息和移动轨迹；

各车辆和移动目标的位置信息和移动轨迹计算预测航迹；

根据预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域；

如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹，基于碰撞时间ttc和航迹交叉区域占用时间差pet，根据ttc和pet进行碰撞预警计算；

根据碰撞预警计算的结果对本车辆发出控制指令。

其中，车辆装备有车载单元，车载单元包括：车端v2x通信设备、导航定位设备和整车控制器。车端v2x通信设备与其他车端v2x通信设备以及路端v2x通信设备进行通信。导航定位设备，导航定位设备基于rtk，导航定位设备获取本车辆的位置信息和移动轨迹。整车控制器，整车控制器与车端v2x通信设备以及导航定位设备连接，整车控制器进行碰撞预警计算并基于碰撞预警计算的结果向本车辆发出控制指令，控制车辆的移动。

交叉路口的路侧装有路侧单元，路侧单元包括：路端v2x通信设备和目标捕捉设备。路端v2x通信设备与车端v2x通信设备进行通信。目标捕捉设备，目标捕捉设备捕捉交叉路口区域的移动目标，获取移动目标的位置信息和移动轨迹，所述移动目标不具有v2x通信设备。

车辆进入交叉路口区域时，车辆通过车端v2x通信设备发送由导航定位设备获取的本车辆的位置信息和移动轨迹，同时通过车端v2x通信设备接受其他车辆的位置信息和移动轨迹，以及由目标捕捉设备获取的移动目标的位置信息和移动轨迹。

各车辆和移动目标的位置信息和移动轨迹计算预测航迹包括：通过车辆状态预估模型和扩展卡尔曼滤波计算各车辆和移动目标的预测航迹。

车辆状态预估模型依据各车辆和移动目标的移动轨迹计算各车辆和移动目标的初步预测航迹，其中，

依据车辆或移动目标的移动轨迹，车辆或移动目标被分为直行轨迹和转弯轨迹；

对于直行轨迹的车辆或移动目标，初步预测航迹为车辆或移动目标保持当前的移动速度，沿当前移动轨迹做匀速直线移动；

对于转弯轨迹的车辆或移动目标，计算初步预测航迹如下：

输入车辆或移动目标的速度和横摆角速度，以及交叉路口的几何形状；

对于转弯轨迹的车辆或移动目标，其在交叉路口的运动过程分为加速和减速两个阶段，分别用三次曲线进行拟合，曲线的拟合系数为a1,a2,a3,a4，拟合时间变量为t，两条曲线的分界点为速度最小点vmin，该时刻的时间点记为tmin，在分界点处的加速度为零，交叉路口的总长度为l：

减速阶段的速度v1的变化为：

v1＝a1t³+a2t²+a3t+a4

加速阶段的运动过程使用减速阶段的相反运动过程来模拟，加速阶段的速度v2的变化为：

v2＝a1(2tmin-t)³+a2(2tmin-t)²+a3(2tmin-t)+a4

由对称性可以得到减速阶段的运动距离为l/2，则上述的约束条件可以表示为：

3a1tmin²+2a2tmin+a3＝0

结合车辆或移动目标刚进入交叉路口时的速度，进行在上述约束条件下的三次曲线拟合，得到车辆或移动目标的速度预测曲线。

根据速度预测曲线，进行两段曲线拟合：第一段曲线的约束条件为tmin处的角度值为45°，tmin处的曲线斜率为1，两段拟合曲线关于点(tmin，45)对称；

在上述约束条件下进行三次曲线拟合，得到车辆或移动目标的横摆角速度预测曲线；

根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线的到车辆或移动目标的初步预测航迹。

对初步预测航迹应用扩展卡尔曼滤波，得到车辆或移动目标的预测航迹，其中，根据车辆或移动目标的速度预测曲线和横摆角速度预测曲线得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，得到车辆或移动目标的预测航迹。扩展卡尔曼滤波是指，对于非线性的速度预测曲线和横摆角速度预测曲线，进行局部线性化，在局部应用匀加速运动模型得到车辆或移动目标的预测速度和预测横摆角速度，将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，得到车辆或移动目标的预测航迹；

将经局部线性化得到的车辆或移动目标的预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值，输入卡尔曼滤波基本方程，包括：

将测量噪声和系统过程噪声设置为零均值的高斯白噪声，随机线性离散系统的状态方程和量测方程为：

xk＝φk|k-1xk-1+γk|k-1wk-1

zk＝hkxk+vk

式中：xk是系统的n维系统状态向量；zk是系统的m维量测向量；φk|k-1是系统n×n维一步状态转移矩阵；γk|k-1是n×p维噪声输入矩阵；hk是m×n维量测矩阵；wk-1是p维系统过程噪声；vk是m维量测噪声；

状态估计：

滤波增益矩阵：

一步预测误差：

pk|k-1＝φk|k-1pkφ^tk|k-1+γk|k-1qk-1γ^tk|k-1

估计误差方差阵：

pk＝(i-kkhk)pk|k-1(i-kkhk)^t+kkrkkk^t＝(i-kkhk)pk|k-1

根据上述随机线性离散系统的卡尔曼滤波基本方程，给定初值和p0，根据k时刻的量测值zk，能递推计算出k时刻的状态估计

将预测速度和预测横摆角速度作为卡尔曼滤波中的虚拟测量值输入，得到车辆或移动目标的预测航迹。

根据各车辆或者移动目标的预测航迹判断本车辆与其他车辆或者移动目标是否存在航迹交叉区域，如果存在航迹交叉区域，则依据本车辆与该车辆或者移动目标的位置信息和移动轨迹，分别计算：

本车辆从当前位置进入航迹交叉区域的时间t1、本车辆从当前位置离开航迹交叉区域的时间t2、目标车辆或者移动对象从当前位置进入航迹交叉区域的时间t3、目标车辆或者移动目标当前位置离开航迹交叉区域的时间t4；

对于本车辆先通过航迹交叉区域的情况，航迹交叉区域占用时间差pet计算为(t3-t2)，pet为正值；

针对目标车辆或者移动对象先通过航迹交叉区域的情况，航迹交叉区域占用时间差pet计算为(t4-t1)，pet为负值；

碰撞时间ttc计算为本车辆从当前位置进入航迹交叉区域的时间t1；

根据碰撞时间ttc以及航迹交叉区域占用时间差pet的绝对值的数值，进行碰撞预警计算。

整车控制器依据碰撞预警计算的结果向本车辆发出控制指令，包括发出预警信号、控制车辆减速或者控制车辆制动。

在一个实施例中，该交叉路口防撞预警方法还包括：

显示地图，并在地图上显示本车辆的位置信息和移动轨迹，还在地图上显示交叉路口区域其他车辆以及移动目标的位置信息和移动轨迹，如果整车控制器进行碰撞预警计算后确定本车辆与其他车辆或者移动目标存在碰撞危险，则该显示设备在地图上显示该其他车辆或者移动目标的位置信息和移动轨迹，以及本车辆与该其他车辆或者移动目标的航迹交叉区域。

该交叉路口防撞预警方法的细节特征与前述的交叉路口防撞预警装置相对应，因此此处就不再重复描述。

本发明的交叉路口防撞预警装置和防撞预警方法通过v2x通信，获取周围车辆的位置和速度信息，相比较于传统利用车载传感器对周围环境进行感知的方法，具有以下优点：

1)定位精度高。rtk可以提供厘米级高精度定位，利用rtk提供的位置和速度信息，通过扩展卡尔曼滤波(ekf)对周围车辆的航迹做比较精准的预测。

2)感知范围大。目前常见的环境感知传感器的探测范围一般在100米左右，而v2x设备的稳定通信范围可以达到300～400米。这就意味着可以在相对较远的距离对危险车辆进行感知和预测，提供更充足的反应时间和采取措施时间。

3)无因障碍物遮挡而产生的探测盲区。在交通路况比较复杂的交叉路口区域，经常因周围车辆或者建筑物的遮挡产生视野盲区。目前的任何一种传感器都无法穿透这类障碍物进行感知。利用v2x通信可以有效地解决这一问题，而对于那些没有装备v2x通信设备的车辆和行人，也可以利用架设在路口的摄像头和毫米波雷达进行探测，然后通过i2v将位置信息传递给目标车辆。

本发明交叉路口防撞预警模型利用车辆状态预估模型和扩展卡尔曼滤波对周围车辆的航迹进行预测，判断自身车辆与其他车辆的航迹是否产生交互区域(ez区域)。在传统ttc的基础上，引入预估pet这一参数，利用ttc和预估pet共同判定是否存在碰撞危险，适用于交叉路口转弯车辆场景。

本发明的交叉路口防撞预警装置和防撞预警方法利用v2x通信技术和高精度导航定位系统，结合设置在路侧的路侧传感装置，能够在车辆进入到交叉路口区域时预先掌握附近的其他车辆或者移动目标的信息，然后根据车辆状态预估模型和扩展卡尔曼滤波计算各车辆和移动目标的预测航迹。根据预测航迹判断是否存在航迹交叉区域，根据航迹交叉区域计算碰撞时间ttc以及航迹交叉区域占用时间差pet，进行防撞预警，具有较高的预警准确性。

上述实施例是提供给熟悉本领域内的人员来实现或使用本发明的，熟悉本领域的人员可在不脱离本发明的发明思想的情况下，对上述实施例做出种种修改或变化，因而本发明的保护范围并不被上述实施例所限，而应该是符合权利要求书提到的创新性特征的最大范围。