[发明专利]一种基于权重的轨迹纠偏算法

权利要求书

1.一种基于权重的轨迹纠偏算法，其特征在于，利用轨迹点滞后运动过程，获取有效权重点，再通过轨迹点与权重点的最大距离和相隔时间来比对，轨迹的运行过程中，有效权重点只有一个，代表的是一段有效轨迹的稳定点，如果出现疑似偏移点的点，则再生成一个，代表新的这个疑似偏移点为开始的点的权重，如果这个新的疑似偏移点之后的5个点都没有偏移，则说明这段轨迹有效，则加入有效轨迹点中并更新权重点。

2.根据权利要求1所述的一种基于权重的轨迹纠偏算法，其特征在于，所述权重点w1的计算公式为w1＝p1*0.2+p2*0.8，其中p1前一个权威点，p2为新的轨迹点。

3.根据权利要求1所述的一种基于权重的轨迹纠偏算法，其特征在于，在判断偏移点的距离的时候按照公交车辆最大速度V(m/s)、按t(s)的时间定位间隔来计算，权重点V*t之外就算作偏移点；在有一段时间没有成功获取定位点的时候先计算两点间隔时间再计算最大偏移点范围。

4.一种基于权重的轨迹纠偏方法，其特征在于，包括以下步骤：

(1)传入坐标p，判断是否为第一次传入，若为第一次传入，则设置当前坐标点为权重点，将坐标加入到坐标w1TempList集合；返回进入下一循环；

(2)若不是第一次传入，则判断其速度是否小于公交最大车速V；若小于，返回进入下一循环；

(3)当速度大于等于最大车速V；则判断w2是否为null；

(4)当w2为null时，则计算w1与当前定位点p的时间差，并得到最大距离，比较p与权重点w1的距离是否超过最大偏移距离；若超过最大偏移距离，则将p设置为w2，并将p点存入坐标缓存列表w2TempList；返回进入下一循环；

(5)若没有超过最大偏移距离，将p加入到坐标集合w1TempList，更新w1权重点；当w1TempList中的数据量大于3时，将w1TempList中的数据放入finaList，并将w1TempList清空；当w1TempList中的数据量不大于3时，返回进入下一循环；

(6)当w2不是null时，计算w2与当前定位点p的时间差，并得到最大距离，比较p与权重点w2的距离是否超过最大偏移距离；若超过最大偏移距离，清空w2TempList，将p点设置为w2，并将p点存入缓存列表w2TempList；返回进入下一循环；

(7)若没有超过最大偏移距离，将p加入w2TempList，更新为w2权重点；当w2TempList集合数量不大于5时，返回进入下一循环；

(8)当w2TempList集合数量大于5时，判断w1所代表的轨迹点数是否大于5，若大于5，将w1TempList中的数据放入finaList，并清空w1TempList，将w2TempList集合中的数据放入到finaList；同时清空w2TempList集合，更新w1的权重点为w2的值，将W2的值设为null；返回进入下一循环；

(9)当w1所代表的轨迹点数不大于5，将w2TempList集合中的数据放入到finaList；同时清空w2TempList集合，更新w1的权重点为w2的值，将W2的值设为null；则返回进入下一循环。