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所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法,其特征在于:步骤四的具体过程如下:(1)当CN>N时,则视作该运动车辆掉头;
(2)当CN≤N时,则视作当前定位出现误差,根据前面相邻两点与中心点C的距离,预测M点与C点的距离LMC;
(3)根据前面相邻两点的经纬度,预测M点经纬度(Lng,Lat)。
6.根据权利要求5所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法,其特征在于:所述N为整型常数;其计算公式为: 这里TD表示在不考虑驾驶员特性及其它车道车辆通行的情况下,小型车辆掉头的平均时间;ΔT为定位设备采集定位信息的时间间隔。
7.根据权利要求6所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法,其特征在于:这里TD取值为2.65秒,定位设备采用5Hz的采集数据频率,即ΔT=0.2秒,由此计算得到N=13次。
8.根据权利要求7所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法,其特征在于:计算两点距离LMC时采用最大时间滤波算法进行滤波。
9.根据权利要求8所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法,其特征在于:所述最大时间滤波算法的具体步骤如下:第一步,获取车辆当前位置点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,则运动车辆状态转换为不定状态并结束,否则结束。
10.根据权利要求9所述的一种交通灯匹配过程中运动车辆定位滤波及预测方法,其特征在于:第五步中通过下列公式更新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中的经纬度数据。