交通灯匹配过程中运动车辆定位滤波及预测方法技术

本发明专利技术公开了一种交通灯匹配过程中运动车辆定位滤波及预测方法，包括以下步骤：确定车辆运行状态为驶向交通灯；实时采集车辆当前行驶位置定位信息和交通灯信息，并计算运动车辆与交通灯之间的距离；判断该距离是否逐渐变小：若距离逐渐变小，则没有定位误差，把距离值和定位信息缓存；若距离突然变大，且距离大于L，则连续累计次数CN加1，转入下一步；判断出现定位误差后进行滤波及预测，并修正距离值和定位数据。该方法在确定运动车辆驶向交通灯后，综合考虑运动车辆与交通灯之间的距离、运动车辆掉头时间等因素，使得定位设备出现定位误差时通过滤波及预测算法进行修正，实时性好，能够应用于嵌入式系统，容易应用于工程实践。

【技术实现步骤摘要】
交通灯匹配过程中运动车辆定位滤波及预测方法
本专利技术属于交通运输
，具体涉及一种交通灯匹配过程中运动车辆定位滤波及预测方法。
技术介绍
随着现代电子信息技术的发展，智能交通和无人驾驶技术也得到飞速发展。城市交叉路口是城市道路网络的重要节点，也是交通状况最复杂、交通事故频发的道路区域。虽然在交叉路口安装了交通灯用以引导和控制交通，但由于其目标小，加上周围光线、大雨、雾霾等因素的影响，导致驾驶员很难及时感知到其状态信息。由于目前民用定位系统的精度受环境因素影响很大，甚至会出现漂移现象。针对这些问题，人们研究提出了多种滤波算法，例如卡尔曼滤波算法、非线性滤波算法、差分定位等，这些算法能够提高定位精度，减少误差；粒子滤波算法还能够减少漂移产生。这些算法虽然在一定程度上提高了定位精度，但很难应用于工程实践，而且算法复杂度高，会导致实时性不好，也很难应用于嵌入式系统。
技术实现思路
针对现有技术中存在的上述问题，本专利技术提供了一种交通灯匹配过程中运动车辆定位滤波及预测方法，该方法在确定运动车辆驶向交通灯后，综合考虑运动车辆与交通灯之间的距离、运动车辆掉头时间等因素，使得定位设备出现定位误差时通过滤波及预测算法进行修正，实时性好，能够应用于嵌入式系统，容易应用于工程实践。为此，本专利技术采用了以下技术方案：一种交通灯匹配过程中运动车辆定位滤波及预测方法，包括以下步骤：步骤一，确定车辆运行状态为驶向交通灯；步骤二，实时采集车辆当前行驶位置定位信息和交通灯信息，并计算运动车辆与交通灯之间的距离；步骤三，判断驶向交通灯的车辆与交通灯之间的距离是否逐渐变小：若距离逐渐变小，则没有定位误差，把距离值和运动车辆位置定位信息缓存；若距离突然变大，且距离大于L，则连续累计次数CN加1，转入下一步；步骤四，判断出现定位误差后进行滤波及预测，并修正距离值和定位数据。进一步地，采用GPS等定位设备获取运动车辆当前行驶位置的定位信息，主要包括经纬度数据；采用无线通信模块获取交通灯终端广播的交通灯信息，包括交通灯区域中心点经纬度、交通灯秒数和颜色等信息；所述交通灯区域为矩形或正方形，取矩形长边或正方形边长的一半为L。进一步地，步骤二的具体过程如下：(1)采集车辆当前行驶位置的定位信息和交通灯信息；(2)从车辆当前行驶位置M的定位信息解析出车辆当前行驶位置M的经纬度数据；(3)从交通灯信息中解析出交通灯区域中心点C的经纬度数据；(4)通过M和C点的经纬度数据，计算出M点与C点之间的距离LMC，表达式如下：LMC＝R*arccos[sin(y1*PI/180)*sin(y2*PI/180)+cos(x1*PI/180)*cos(x2*PI/180)-cos(x2*PI/180-x1*PI/180)]其中：x1为M点的经度，y1为M点的纬度；x2为C点的经度，y2为C点的纬度；PI为圆周率；R为地球半径。进一步地，步骤三的具体过程如下：(1)比较当前距离LMC与前一次计算的距离LMC1大小；(2)若LMC≤LMC1，则队列QD出队一个距离数据，并把LMC存储队列QD；队列QLL出队一组经纬度数据，并把当前M点经纬度(Lng，Lat)存储队列QLL；其中QD为长度为2的M点到C点的距离值缓冲队列，QLL为长度为2的M点经纬度缓冲队列。(3)若LMC>LMC1，且LMC大于L，则连续累计次数CN加1。进一步地，步骤四的具体过程如下：(1)当CN>N时，则视作该运动车辆掉头；(2)当CN≤N时，则视作当前定位出现误差，根据前面相邻两点与中心点C的距离，预测M点与C点的距离LMC；(3)根据前面相邻两点的经纬度，预测M点经纬度(Lng，Lat)。进一步地，所述N为整型常数；其计算公式为：这里TD表示在不考虑驾驶员特性及其它车道车辆通行的情况下，小型车辆掉头的平均时间；ΔT为定位设备采集定位信息的时间间隔。具体地，这里TD取值为2.65秒，定位设备采用5Hz的采集数据频率，即ΔT＝0.2秒，由此计算得到N＝13次。作为一种优选，计算两点距离LMC时采用最大时间滤波算法进行滤波。进一步地，所述最大时间滤波算法的具体步骤如下：第一步，获取车辆当前位置点M经纬度数据(Lng,Lat)；第二步，计算交通灯中心点C与当前运动车辆位置点M之间的距离LMC；第三步，从长度为2的距离LMC缓冲队列QD中出队一个数据LMC1；第四步，对LMC与LMC1的大小进行比较：若LMC不大于LMC1，则连续累计次数CN清零；若LMC大于LMC1，则进行第五步；第五步，更新LMC及点M的经纬度数据(Lng,Lat)；第六步，将新LMC入队列QD；第七步，从长度为2的经纬度数据缓冲队列QLL中出队一个数据，并将新经纬度数据(Lng,Lat)入队；第八步，连续累计次数CN加1；第九步，判断CN是否小于13，若CN不小于13，则运动车辆状态转换为不定状态并结束，否则结束。进一步地，第五步中通过下列公式更新LMC及点M的经纬度数据(Lng,Lat)：LMC＝2*LMC1-LMC0Lng＝2*Lng1-Lng0Lat＝2*Lat1-Lat0其中：LMC0、LMC1分别为存储在队列QD中的交通灯中心点C与当前运动车辆位置点M之间的距离数据；(Lng0，Lat0)、(Lng1，Lat1)分别为存储在队列QLL中的经纬度数据。与现有技术相比，本专利技术的有益效果是：(1)不受周围环境因素影响，算法复杂度低，实时性好。(2)综合考虑了运动车辆与交通灯之间的距离、运动车辆掉头时间等因素，使得定位设备出现定位误差时通过滤波及预测算法进行修正。(3)定位精度满足使用需求，能够应用于嵌入式系统，容易应用于工程实践。附图说明图1是本专利技术所提供的一种交通灯匹配过程中运动车辆定位滤波及预测方法的流程图。图2是最大时间滤波算法流程图。具体实施方式下面结合附图以及具体实施例来详细说明本专利技术，其中的具体实施例以及说明仅用来解释本专利技术，但并不作为对本专利技术的限定。如图1所示，本专利技术公开了一种交通灯匹配过程中运动车辆定位滤波及预测方法，包括以下步骤：步骤一，确定车辆运行状态为驶向交通灯；步骤二，实时采集车辆当前行驶位置定位信息和交通灯信息，并计算运动车辆与交通灯之间的距离；步骤三，判断驶向交通灯的车辆与交通灯之间的距离是否逐渐变小：若距离逐渐变小，则没有定位误差，把距离值和运动车辆位置定位信息缓存；若距离突然变大，且距离大于L，则连续累计次数CN加1，转入下一步；步骤四，判断出现定位误差后进行滤波及预测，并修正距离值和定位数据。这里采用GPS等定位设备获取运动车辆当前行驶位置的定位信息，主要包括经纬度数据；采用无线通信模块获取交通灯终端广播的交通灯信息，包括交通灯区域中心点经纬度、交通灯秒数和颜色等信息；所述交通灯区域为矩形或正方形，取矩形长边或正方形边长的一半为L。步骤二的具体过程如下：(1)采集车辆当前行驶位置的定位信息和交通灯信息；(2)从车辆当前行驶位置M的定位信息解析出车辆当前行驶位置M的经纬度数据；(3)从交通灯信息中解析出交通灯区域中心点C的经纬度数据；(4)通过M和C点的经纬度数据，计算出M点与C点之间的距离LMC，表达式如下：LMC＝R*arccos[sin(y1*PI/180)本文档来自技高网...

【技术保护点】
一种交通灯匹配过程中运动车辆定位滤波及预测方法，其特征在于：包括以下步骤：步骤一，确定车辆运行状态为驶向交通灯；步骤二，实时采集车辆当前行驶位置定位信息和交通灯信息，并计算运动车辆与交通灯之间的距离；步骤三，判断驶向交通灯的车辆与交通灯之间的距离是否逐渐变小：若距离逐渐变小，则没有定位误差，把距离值和运动车辆位置定位信息缓存；若距离突然变大，且距离大于L，则连续累计次数CN加1，转入下一步；步骤四，判断出现定位误差后进行滤波及预测，并修正距离值和定位数据。

【技术特征摘要】
1.一种交通灯匹配过程中运动车辆定位滤波及预测方法，其特征在于：包括以下步骤：步骤一，确定车辆运行状态为驶向交通灯；步骤二，实时采集车辆当前行驶位置定位信息和交通灯信息，并计算运动车辆与交通灯之间的距离；步骤三，判断驶向交通灯的车辆与交通灯之间的距离是否逐渐变小：若距离逐渐变小，则没有定位误差，把距离值和运动车辆位置定位信息缓存；若距离突然变大，且距离大于L，则连续累计次数CN加1，转入下一步；步骤四，判断出现定位误差后进行滤波及预测，并修正距离值和定位数据。2.根据权利要求1所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法，其特征在于：采用GPS等定位设备获取运动车辆当前行驶位置的定位信息，主要包括经纬度数据；采用无线通信模块获取交通灯终端广播的交通灯信息，包括交通灯区域中心点经纬度、交通灯秒数和颜色等信息；所述交通灯区域为矩形或正方形，取矩形长边或正方形边长的一半为L。3.根据权利要求1或2所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法，其特征在于：步骤二的具体过程如下：(1)采集车辆当前行驶位置的定位信息和交通灯信息；(2)从车辆当前行驶位置M的定位信息解析出车辆当前行驶位置M的经纬度数据；(3)从交通灯信息中解析出交通灯区域中心点C的经纬度数据；(4)通过M和C点的经纬度数据，计算出M点与C点之间的距离LMC，表达式如下：LMC＝R*arccos[sin(y1*PI/180)*sin(y2*PI/180)+cos(x1*PI/180)*cos(x2*PI/180)-cos(x2*PI/180-x1*PI/180)]其中：x1为M点的经度，y1为M点的纬度；x2为C点的经度，y2为C点的纬度；PI为圆周率；R为地球半径。4.根据权利要求3所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法，其特征在于：步骤三的具体过程如下：(1)比较当前距离LMC与前一次计算的距离LMC1大小；(2)若LMC≤LMC1，则队列QD出队一个距离数据，并把LMC存储队列QD；队列QLL出队一组经纬度数据，并把当前M点经纬度(Lng，Lat)存储队列QLL；其中QD为长度为2的M点到C点的距离值缓冲队列，QLL为长度为2的M点经纬度缓冲队列。(3)若LMC>LMC1，且LMC大于L，则连续累计次数CN加1。5.根据权利要求4所述的一种交通灯匹配过程中运动车辆定位滤...

【专利技术属性】
技术研发人员：张涛，陈宇峰，向郑涛，张全亮，车凯，简炜，江学焕，贾蓉，周鹏，高瞻，严汝康，董章，郁洋，罗依麟，
申请(专利权)人：湖北汽车工业学院，
类型：发明
国别省市：湖北,42