本发明属于交通运输技术领域，具体涉及一种交通灯匹配过程中运动车辆定位滤波及预测方法。
背景技术：
随着现代电子信息技术的发展，智能交通和无人驾驶技术也得到飞速发展。城市交叉路口是城市道路网络的重要节点，也是交通状况最复杂、交通事故频发的道路区域。虽然在交叉路口安装了交通灯用以引导和控制交通，但由于其目标小，加上周围光线、大雨、雾霾等因素的影响，导致驾驶员很难及时感知到其状态信息。
由于目前民用定位系统的精度受环境因素影响很大，甚至会出现漂移现象。针对这些问题，人们研究提出了多种滤波算法，例如卡尔曼滤波算法、非线性滤波算法、差分定位等，这些算法能够提高定位精度，减少误差；粒子滤波算法还能够减少漂移产生。这些算法虽然在一定程度上提高了定位精度，但很难应用于工程实践，而且算法复杂度高，会导致实时性不好，也很难应用于嵌入式系统。
技术实现要素：
针对现有技术中存在的上述问题，本发明提供了一种交通灯匹配过程中运动车辆定位滤波及预测方法，该方法在确定运动车辆驶向交通灯后，综合考虑运动车辆与交通灯之间的距离、运动车辆掉头时间等因素，使得定位设备出现定位误差时通过滤波及预测算法进行修正，实时性好，能够应用于嵌入式系统，容易应用于工程实践。
为此，本发明采用了以下技术方案：
一种交通灯匹配过程中运动车辆定位滤波及预测方法，包括以下步骤：
步骤一，确定车辆运行状态为驶向交通灯；
步骤二，实时采集车辆当前行驶位置定位信息和交通灯信息，并计算运动车辆与交通灯之间的距离；
步骤三，判断驶向交通灯的车辆与交通灯之间的距离是否逐渐变小：若距离逐渐变小，则没有定位误差，把距离值和运动车辆位置定位信息缓存；若距离突然变大，且距离大于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取值为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-lmc0
lng＝2*lng1-lng0
lat＝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)*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为圆周率，这里取3.14159265358979324；r为地球半径，这里取6371004米。
步骤三的具体过程如下：
(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取值为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，则运动车辆状态转换为不定状态并结束，否则结束。
上述第五步中，由于时间间隔为0.2秒，行驶距离较小，运动车辆可以视为匀速行驶，且地球表面可以视为平面，因此，可以通过下列公式更新lmc及点m的经纬度数据(lng,lat)：
lmc＝2*lmc1-lmc0
lng＝2*lng1-lng0
lat＝2*lat1-lat0
其中：lmc0、lmc1分别为存储在队列qd中的交通灯中心点c与当前运动车辆位置点m之间的距离数据；(lng0，lat0)、(lng1，lat1)分别为存储在队列qll中的经纬度数据。
实施例
当运动车辆处于驶向交通灯状态，进行交通灯匹配及驶离交通灯状态判断时，可以采用最大时间滤波，进一步抑制随机定位误差，也可以较好地抑制突发错误。运动车辆处于驶向交通灯状态时，中心点c与运动车辆当前位置点m之间的距离lmc应该逐渐减小。当计算距离lcm突然变大且lcm大于十字路口区域最大边长的一半时，只有两种情况：一是车辆掉头；二是出现定位误差。南春丽等人在《禁左条件下车辆掉头行为仿真模型》(计算机工程与应用，2006年第20期)一文中估算了在不考虑驾驶员特性及其它车道车辆通行的情况下，小型车辆掉头平均时间为2.65秒。定位设备定时采集定位数据的时间间隔为0.2秒。如果车辆掉头，那么需要连续采集大约13组定位数据才能完成掉头，即距离lcm突然变大连续13次时，认为是车辆掉头；距离lmc突然变大但小于13次时，认为是出现定位误差。
以上所述仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则范围之内所作的任何修改、等同替换以及改进等，均应包含在本发明的保护范围之内。