CN109523830B - 基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法 - Google Patents

Abstract

本发明涉及一种基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，利用车载定位装置采集的频率为5hz、精度为厘米级的实时定位信息，根据邻近时刻多个点的车辆位置、速度、方向角信息，拟合车辆行驶轨迹，预测目标时刻车辆位置，并根据轮廓冲突法，判断如果车辆仍按当前速度、方向角行驶，目标时刻车辆是否与邻近车辆存在碰撞风险。较传统的通过车载传感器进行车‑车距离判断及碰撞预警的方法，本发明所提出的方法可有效避免车辆运行速度较高、恶劣气象、视线条件不良等情况引起的碰撞预警失效；具有算法简便、成本较低、易于推广的特点。

Description

基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法

技术领域

本发明涉及车辆高智慧辅助驾驶技术领域，具体涉及一种基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法。

背景技术

随着公路建设事业蓬勃发展和汽车保有量迅速膨胀，交通安全形势正面临着前所未有的挑战。交通系统是由人、车、路、环境构成的复杂系统，但这四要素中，“人”是导致道路交通事故发生的最主要因素。美国印第安那大学对交通事故原因的调查研究显示，85％的交通事故都与驾驶员自身及其行为特性直接相关。另有研究显示，如果驾驶员能够提前1秒感知到危险并及时采取预防措施，大约40％的正面碰撞事故、60％的与路面状况相关的事故和70％的追尾事故可以避免发生。因此，对驾驶员进行风险预警可有效降低事故发生概率。

近年来，高智慧辅助驾驶甚至无人驾驶技术已成为研究热点，车辆主动防撞系统更是其核心内容。目前采用的主流方法，是在车辆上加装雷达、激光、微波、视觉等多类传感器，融合多源数据进行距离识别、风险判识。但此类方法具有以下不足：第一，各类传感器均有适用条件限制，如车辆运行速度较高、雨雾等恶劣气象、道路转弯半径较小而造成的视线条件不良等，均会对传感器识别距离的准确性造成影响；第二，若要实现较精确的距离判断，需安装多种传感器相互配合，数据融合算法复杂，因此成本过高，推广难度较大。

普通的卫星导航定位精度约在5-10米，无法用于车辆防撞预警。只有依托北斗导航系统高精度定位技术，才可获取高频、高精度车辆位置信息，进而拟合高准确度的车辆轨迹，并进行车辆位置预测和碰撞风险判识。而基于此方法进行车辆防撞预警，国内外还鲜有研究。

发明内容

本发明的目的是提供一种基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，基于高频(5HZ)、高精度(厘米级)的车辆定位信息，进行车辆主动防撞预警距离的计算，实现有效的碰撞预警。

本发明所采用的技术方案为：

基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，其特征在于：

包括以下步骤：

利用车载定位装置采集的频率为5hz、精度为厘米级的实时定位信息，根据邻近时刻多个点的车辆位置、速度、方向角信息，拟合车辆行驶轨迹，预测目标时刻车辆位置，并根据轮廓冲突法，判断如果车辆仍按当前速度、方向角行驶，目标时刻车辆是否与邻近车辆存在碰撞风险，并根据发生轮廓冲突的目标时刻确定碰撞预警等级。

具体包括以下步骤：

步骤1，建立路侧北斗连续运行基准站参考系统，车载定位装置接收该系统播发的高精度差分改正数，并以1秒5次的频率进行解算，生成厘米级的高精度车辆定位数据；

步骤2，利用多个邻近点的根据车辆高频位置、速度、方向角信息，拟合车辆运动轨迹，车辆轨迹被分为两类：直线和圆曲线；

步骤3，选取目标预警时刻，利用车辆连续的高频位置、速度、方位角信息，预测本时刻至目标时刻车辆沿预测轨迹的位移及目标时刻车辆位置；

步骤4，车辆位置坐标实质是车外天线安装点位的坐标及方向角，根据天线点位和车辆外轮廓的位置关系，计算目标时刻车辆实际外轮廓顶点坐标；

步骤5，考虑驾驶员反应延迟，为避免避撞失效，根据不同车型区别设置纵、横向冗余值，将车辆实际外轮廓顶点坐标转化为计算外轮廓顶点坐标；

步骤6，选取目标时刻，预测在该时刻他车的计算外轮廓顶点坐标是否在自车四边形之内或之上，若是，则判断为目标时刻两车会发生冲突。

步骤7，根据预警的目标时刻，相应确定预警等级。

步骤1具体为：

构建包括RTK基准站、网络传输系统、数据处理系统的路侧北斗连续运行基准站参考系统；RTK基准站需建设三个组成三角网，车辆运行路径需包含于三角网内；车辆定位装置由车外天线和车内接收机构成，接收机主要元部件包括北斗卫星接收板卡、4G无线通信模块、主控单元、电源管理单元、异步通信模块。

步骤2具体为：

首先选取邻近三点位置坐标，判断三点是否共线，若共线，则行车轨迹为直线，若不共线，则行车轨迹为圆曲线；

若行车轨迹为圆曲线，为提高精度，采用多个邻近点拟合车辆的行车轨迹，5≤n≤10，具体思路为，运用最小二乘法，以邻近点与轨迹圆心的距离与轨迹半径的差的平方和为目标函数：

其中：(x_i，y_i)为邻近点坐标，(x_c，y_c)和r分别为轨迹圆心和半径；

采用最速下降法，求得目标函数Q最小时(x_c，y_c)和r的值，从而获得车辆圆曲线行车轨迹。

步骤3具体为：

计算i时刻的加速度，以i时刻的位置、速度、方向角、加速度推算至目标时刻t的位移l；

将目标时刻选取为：i时刻后的1s、2s、3s；

若车辆行驶轨迹为直线，目标时刻坐标(x_t，y_t)预测方法为：

其中，α为车辆i时刻方向角；

若车辆沿圆曲线轨迹行进，目标时刻坐标(x_t，y_t)预测方法为：

其中，φ为车辆位移对应的圆心角：

步骤4具体为：

接收机输出的高精度车辆定位数据，实际上是安装在车辆上的天线的位置数据；为提高车辆轮廓计算精度，安装天线时，应测量天线安装点位与车辆各外轮廓边缘的距离，并以天线安装位置为原点，以车辆轴向为x轴构建车辆局部坐标系，并采用旋转平移公式得到直行车辆外轮廓在实用坐标系下的坐标计算公式；车辆沿直线轨迹运动时，以i时刻的车辆方位角α为旋转角，沿圆曲线轨迹运动时，需对旋转角进行校正；

α₀＝α+Δα

其中，f为圆曲线偏向，左偏时取1，右偏时取-1；d为定位点至汽车后轴的距离；r为车辆轨迹圆曲线半径。

步骤5具体为：

对于小型车：纵向冗余值取5m，横向冗余值取0.25m；

对于大型车：纵向冗余值取10m，横向冗余值取0.5m。

步骤6具体为：

目标时刻车辆外轮廓顶点坐标通过上述步骤计算得出，每个车辆被作为一个四边形考虑，判断自车与他车轮廓是否会发生冲突，被转化为判断两四边形是否相交；一个四边形被认为由两个三角形组成，若两个四边形相交，则一个四边形的顶点会出现在另一四边形的任一三角形之内或之上；因此，问题进一步转化为判断他车某顶点P(x₀,y₀) 与自车任一三角形ΔABC的位置关系；具体方法为：

其中，s(ABC)为ΔABC的面积，其他类似；

当G＞0时，点P(x₀,y₀)位于三角形ΔABC之内，

当G＝0时，点P(x₀,y₀)位于三角形ΔABC之上，

当G＜0时，点P(x₀,y₀)位于三角形ΔABC之外；

采用遍历方法，将他车四个顶点与自车任一三角形的位置关系进行判断，若在其之上或之内，表明在目标时刻辆车会发生碰撞。

步骤7具体为：

选取目标时刻是防碰撞预警的关键，目标时刻过近会导致驾驶员反应不及，过远又会由于预测误差过大而造成误报；设置不同预警等级，并分别选取3s、2s、1s作为碰撞预警目标时刻；

若未来3s车辆外轮廓会发生冲突，为三级碰撞预警，即最低；若未来2s车辆外轮廓会发生冲突，为二级预警，即中等；若未来1s 车辆外轮廓会发生冲突，为一级预警，即最高。

本发明具有以下优点：

(1)有别于常规的在车辆上加装激光、雷达、视觉等多种传感器进行碰撞预警的方法，本发明利用高频率、高精度的车辆定位信息，进行车辆实时轨迹预测，并通过轮廓冲突法进行车辆碰撞风险判识，在数据获取途径、碰撞判断方法上均有创新。

(2)较常规的加装传感器的方法，本发明所提出的方法可有效避免车辆运行速度较高、恶劣气象、视线条件不良等情况引起的碰撞预警失效，并具有算法简便、成本较低、易于推广的特点。

附图说明

图1为本发明实施例的流程图；

图2为本发明实施例提供的车辆外轮廓顶点计算局部坐标系示意图；

图3为本发明实施例提供的轨迹拟合及目标时刻位置预测示意图；

图4为本发明实施例提供的预警条件示意图。

具体实施方式

下面结合具体实施方式对本发明进行详细的说明。

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。同样的，附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

本发明涉及一种基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法。图1，图2提供了轨迹预测及碰撞预警方法的步骤和流程图。显然，获取高频、高精度的车辆实时定位信息是本发明提出的方法的基础。本发明利用车载定位装置采集的频率为5hz、精度为厘米级的实时定位信息，根据邻近时刻多个点的车辆位置、速度、方向角信息，拟合车辆行驶轨迹，预测目标时刻车辆位置，并根据轮廓冲突法，判断如果车辆仍按当前速度、方向角行驶，目标时刻车辆是否与邻近车辆存在碰撞风险。流程为：路侧北斗连续运行基准站参考系统(简称CORS系统)，并安装车载天线及接收机，获取高频、高精度车辆定位信息；根据邻近点坐标，利用最小二乘法建立目标函数并用最速下降法求解，拟合车辆运行轨迹；确定目标时刻，计算车辆位移和目标时刻位置；计算目标时刻车辆外轮廓顶点实际坐标；考虑冗余值，计算目标时刻车辆外轮廓顶点计算坐标；利用外轮廓冲突法，判识自车与他车碰撞风险；根据目标时刻碰撞风险状态确定碰撞预警等级。

本发明的方法中，采集高频、高精度车辆实时定位信息分为两步：

步骤1：构建路侧CORS系统，CORS系统由三个RTK基准站、网络传输系统、数据处理系统组成。为确保定位精度，三个基准站组成的三角网应覆盖车辆行驶路径；为确保数据传输实时性，经测试，网络传输采用4G网络进行数据播发。基准站接收实时卫星数据，解算基准站覆盖区域内的卫星定位信息的误差改正数，并将误差改正数发给车载接收机，即可实现厘米级定位。

步骤2：为驶入CORS系统覆盖路段的车辆安装车载天线和接收机。车载天线用吸盘固定于驾驶位上方车辆顶部，车载接收机放置于车内，每个车载接收机有各自的ID。接收机接收基准站播发的改正数，以5hz的频率解算实时定位，并可通过4G 传输定位信息至后台服务器。由于后续步骤需由定位数据推算车辆外轮廓，因此需知道车辆外廓尺寸及车载天线与外廓的相对位置。为便于操作，将车辆按照《JTG B01-2014公路工程技术标准》进行车型分类，各车型设计外廓尺寸如表所示。并测量天线安装点位与车辆前侧、左侧外轮廓的距离，输入接收机并与该接收机ID对应。

车辆类型	总长(m)	总宽(m)
			小客车	6	1.8
大型客车	13.7	2.55
			铰接客车	18	2.5
载重汽车	12	2.5
			铰接列车	18.1	2.55

车辆轨迹预测分为以下三步：

步骤1：用包含当前时刻的邻近三点坐标(x₁，y₁)、(x₂，y₂)及 (x₃，y₃)进行行车轨迹判断。

Δ＝x₁₂y₁₃-x₁₃y₁₂

x₁₂＝x₁-x₂

y₁₂＝y₁-y₂

x₁₃＝x₁-x₃

y₁₃＝y₁-y₃

当Δ＝0时，则三点共线，车辆轨迹符合直线；

当Δ≠0时，则三点构成圆曲线，车辆轨迹符合圆曲线。

f＝sign(x₁y₂-x₂y₁+x₂y₃-x₃y₂+x₃y₁-x₁y₃)

当f＝1时，三点逆时针分布，为左偏圆曲线；当f＝-1时，三点顺时针分布，为右偏圆曲线。

步骤2：为提高行车轨迹拟合精度，采用多点拟合圆曲线的方法对轨迹进行拟合。

以7点拟合为例，选取当前时刻及当前时刻前的6个邻近点，坐标分别为(x₁,y₁)至(x₇,y₇)，建立目标函数

令Q最小，用最速下降法求解(x_c,y_c)和r，便可得知车辆圆曲线轨迹。

步骤3：预测当前时刻至目标时刻的位移，以及目标时刻的位置。

所述目标时刻，是指若车辆保持当前时刻的轨迹、速度、方向角不变，判断在未来某一时刻是否会与相关车辆发生碰撞。目标时刻选取是轨迹预测和碰撞预警的关键，过近会导致驾驶员反应不及，过远又会由于预测误差过大而造成误报。根据已有研究基础，一般驾驶人反应时间0.7-1.5s，系统处理时间 0.3-1秒，因此目标时刻不应小于1s，宜大于2.5s。《高速公路交通管理办法》规定：“在正常情况下，当车速达到100公里每小时时，车间间距应达到100米以上；当车速达到70公里每小时时，车间间距应达到70米以上。”此规定中的安全距离相对应的安全时间为3.6s。据以上，本发明分别选取3s、2s、 1s作为目标时刻，对车辆碰撞风险进行判识。因此，在预测位移、位置时，需对当前时刻后1s、2s、3s分别进行预测。

利用当前时刻i的速度、加速度计算位移，计算方法为：

若车辆运动轨迹为直线，用位移以及当前时刻i点位的坐标、方向角推算目标时刻的坐标，计算方法为：

若车辆运动轨迹为圆曲线，用位移以及当前时刻i点位的轨迹半径、圆心角、方向角推算目标时刻的坐标，计算方法为：

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

车辆碰撞预警方法分为以下五步：

步骤1：根据图3建立车辆局部坐标系，并根据方向角将局部坐标系转化为实用坐标系，计算车辆外轮廓顶点坐标。 L,R,F,B通过不同车型设计尺寸和测量出的天线安装点位据轮廓的距离可知，具体方法如下：

车辆沿圆曲线轨迹运行时，需对α₀进行校正，公式如下：

α＝α₀+Δα

其中，f—圆曲线偏向，左偏时取1，右偏时取-1；

d—定位点至汽车后轴的距离；

r—车辆轨迹圆曲线半径。

步骤2：考虑容错性，对L,R,F设置冗余值。考虑大型车较小型车对安全距离要求更高，故区别设置。

公路设计中，停车视距为一重要的设计指标，由三部分组成，包括驾驶员在反应时间内车辆行驶的距离、开始制动至停车的制动距离和安全距离。这里的安全距离，就是一个冗余值，反映的是驾驶员心理可接受的车辆停止后车与车间的纵向距离，该值一般取5～10m。本发明中的前方冗余值参考该值进行设置，小型车前方冗余值取5m，大型车前方冗余值取10m。

侧向安全距离取值依据为，一般车行道为3.75m，小型车总宽为1.8m，假设车辆沿中心线行驶，相邻车道并排行驶的车辆压车道线时，驾驶员会有紧迫感，此时辆车侧向距离为 0.975m，取该值的一半作为安全距离，即0.5m，考虑到辆车均侧向均设置冗余，因此以0.25m作为侧向冗余值。考虑大型车对安全距离要求更高，参考纵向冗余取值情况，取小型车冗余值的一倍作为大型车侧向冗余值，即0.5m。

步骤3：对步骤1中的L,R,F进行修正后，重新计算目标时刻车辆轮廓顶点坐标。

步骤4：如图4所示，判断目标时刻两车轮廓的位置关系。具体方法为：以

为判别函数，G≥0 时，两车轮廓有冲突，即发生碰撞；G＜0时无碰撞风险。用计算机中的遍历算法，依次判断A车各顶点和B车各三角形的位置关系。

步骤5：输出风险等级，如下表。

目标时刻	G≥0	G＜0
			3s	三级预警(低)	不预警
2s	二级预警(中)	不预警
			1s	一级预警(高)	不预警

本发明的内容不限于实施例所列举，本领域普通技术人员通过阅读本发明说明书而对本发明技术方案采取的任何等效的变换，均为本发明的权利要求所涵盖。

Claims (6)

1.基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，其特征在于：

包括以下步骤：

利用车载定位装置采集的频率为5hz、精度为厘米级的实时定位信息，根据邻近时刻多个点的车辆位置、速度、方向角信息，拟合车辆行驶轨迹，预测目标时刻车辆位置，并根据轮廓冲突法，判断如果车辆仍按当前速度、方向角行驶，目标时刻车辆是否与邻近车辆存在碰撞风险，并根据发生轮廓冲突的目标时刻确定碰撞预警等级；

具体包括以下步骤：

步骤1，建立路侧北斗连续运行基准站参考系统，车载定位装置接收该系统播发的高精度差分改正数，并以1秒5次的频率进行解算，生成厘米级的高精度车辆定位数据；

步骤2，利用多个邻近点的根据车辆高频位置、速度、方向角信息，拟合车辆运动轨迹，车辆轨迹被分为两类：直线和圆曲线；

步骤3，选取目标预警时刻，利用车辆连续的高频位置、速度、方位角信息，预测本时刻至目标时刻车辆沿预测轨迹的位移及目标时刻车辆位置；

步骤4，车辆位置坐标实质是车外天线安装点位的坐标及方向角，根据天线点位和车辆外轮廓的位置关系，计算目标时刻车辆实际外轮廓顶点坐标；

步骤5，考虑驾驶员反应延迟，为避免避撞失效，根据不同车型区别设置纵、横向冗余值，将车辆实际外轮廓顶点坐标转化为计算外轮廓顶点坐标；

步骤6，选取目标时刻，预测在该时刻他车的计算外轮廓顶点坐标是否在自车四边形之内或之上，若是，则判断为目标时刻两车会发生冲突；

步骤7，根据预警的目标时刻，相应确定预警等级；

步骤3具体为：

计算i时刻的加速度，以i时刻的位置、速度、方向角、加速度推算至目标时刻t的位移l；

将目标时刻选取为：i时刻后的1s、2s、3s；

若车辆行驶轨迹为直线，目标时刻坐标(x_t，y_t)预测方法为：

其中，α为车辆i时刻方向角；

若车辆沿圆曲线轨迹行进，目标时刻坐标(x_t，y_t)预测方法为：

其中，φ为车辆位移对应的圆心角：

步骤4具体为：

接收机输出的高精度车辆定位数据，实际上是安装在车辆上的天线的位置数据；为提高车辆轮廓计算精度，安装天线时，应测量天线安装点位与车辆各外轮廓边缘的距离，并以天线安装位置为原点，以车辆轴向为x轴构建车辆局部坐标系，并采用旋转平移公式得到直行车辆外轮廓在实用坐标系下的坐标计算公式；车辆沿直线轨迹运动时，以i时刻的车辆方位角α为旋转角，沿圆曲线轨迹运动时，需对旋转角进行校正；

α₀＝α+Δα

其中，f为圆曲线偏向，左偏时取1，右偏时取-1；d为定位点至汽车后轴的距离；r为车辆轨迹圆曲线半径。

2.根据权利要求1所述的基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，其特征在于：

步骤1具体为：

构建包括RTK基准站、网络传输系统、数据处理系统的路侧北斗连续运行基准站参考系统；RTK基准站需建设三个组成三角网，车辆运行路径需包含于三角网内；车辆定位装置由车外天线和车内接收机构成，接收机主要元部件包括北斗卫星接收板卡、4G无线通信模块、主控单元、电源管理单元、异步通信模块。

3.根据权利要求2所述的基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，其特征在于：

步骤2具体为：

首先选取邻近三点位置坐标，判断三点是否共线，若共线，则行车轨迹为直线，若不共线，则行车轨迹为圆曲线；

若行车轨迹为圆曲线，为提高精度，采用多个邻近点拟合车辆的行车轨迹，5≤n≤10，具体思路为，运用最小二乘法，以邻近点与轨迹圆心的距离与轨迹半径的差的平方和为目标函数：

其中：(x_i，y_i)为邻近点坐标，(x_c，y_c)和r分别为轨迹圆心和半径；

采用最速下降法，求得目标函数Q最小时(x_c，y_c)和r的值，从而获得车辆圆曲线行车轨迹。

4.根据权利要求3所述的基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，其特征在于：

步骤5具体为：

对于小型车：纵向冗余值取5m，横向冗余值取0.25m；

对于大型车：纵向冗余值取10m，横向冗余值取0.5m。

5.根据权利要求4所述的基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，其特征在于：

步骤6具体为：

目标时刻车辆外轮廓顶点坐标通过上述步骤计算得出，每个车辆被作为一个四边形考虑，判断自车与他车轮廓是否会发生冲突，被转化为判断两四边形是否相交；一个四边形被认为由两个三角形组成，若两个四边形相交，则一个四边形的顶点会出现在另一四边形的任一三角形之内或之上；因此，问题进一步转化为判断他车某顶点P(x₀,y₀)与自车任一三角形ΔABC的位置关系；具体方法为：

其中，s(ABC)为ΔABC的面积，其他类似；

当G＞0时，点P(x₀,y₀)位于三角形ΔABC之内，

当G＝0时，点P(x₀,y₀)位于三角形ΔABC之上，

当G＜0时，点P(x₀,y₀)位于三角形ΔABC之外；

采用遍历方法，将他车四个顶点与自车任一三角形的位置关系进行判断，若在其之上或之内，表明在目标时刻辆车会发生碰撞。

6.根据权利要求5所述的基于高频高精度定位信息的车辆轨迹预测及碰撞预警方法，其特征在于：

步骤7具体为：

选取目标时刻是防碰撞预警的关键，目标时刻过近会导致驾驶员反应不及，过远又会由于预测误差过大而造成误报；设置不同预警等级，并分别选取3s、2s、1s作为碰撞预警目标时刻；

若未来3s车辆外轮廓会发生冲突，为三级碰撞预警，即最低；若未来2s车辆外轮廓会发生冲突，为二级预警，即中等；若未来1s车辆外轮廓会发生冲突，为一级预警，即最高。

Images