一种GNSS跳变数据剔除算法的制作方法

一种gnss跳变数据剔除算法
技术领域
1.本发明涉及无人驾驶的建图和定位技术领域。


背景技术：

2.随着无人驾驶技术的快速发展，无人驾驶对人类生活的影响越来越大。建图与定位能力是无人驾驶汽车能够运行的前提条件，是自动驾驶技术的重要组成部分，建图精确与否对定位的准确度有重要影响。目前，建图主要是通过多传感器融合技术对激光雷达、imu(惯性测量单元)和gnss(全球导航卫星系统)等传感器数据进行融合，来建立出精确的地图，并在无人驾驶车辆运行时通过融合数据提供精确的定位。
3.其中，gnss利用接收机被动地接受来自卫星的数据结算出经纬度信息，得到绝对定位数据，在差分模式下可达到厘米级定位精度，在多传感器融合建图中，gnss的位姿数据作为点云数据进行建图的直接约束，对建图效果有着直接影响。但是gnss数据可能收到外界环境的影响导致数据产生跳变，如容易受到树荫等遮挡的影响产生跳变，gnss数据出现跳变时，若直接使用存在跳变的gnss数据进行融合建图，则将导致点云地图出现错误约束，建立出错误的地图。若建图错误，则多传感器融合定位也无从谈起。


技术实现要素：

4.本发明的目的在于提供一种剔除因环境因素导致的gnss跳变数据的一种gnss跳变数据剔除算法，以应对gnss数据跳变不可完全避免的情况下对跳变数据进行剔除处理。
5.为实现上述目的，本发明的技术方案是：一种gnss跳变数据剔除算法，算法步骤如下：
6.步骤1、接收传感器传传来的gnss数据，以不受干扰的正确的gnss数据作为第一个gnss数据，即起始数据，初始跳变误差记为error，error为三维向量，分别对应x、y、z三维坐标，error的模长则为|error|，初始|error|＝0；然后进入步骤2；
7.步骤2、继续接收传感器传来的gnss数据，最新接收的数据作为当前时刻数据记为now_data，now_data前未被舍弃的上一时刻数据记为old_data；然后进入步骤3；
8.步骤3：计算now_data相对于old_data的位姿变换，并得到相对移动向量记为d,d的模长为相对移动距离即为|d|；然后进入步骤4；
9.步骤4：若|d|大于预设定的阈值a，则记录error＝error+d，否则不进行其他操作；然后进入步骤5；
10.步骤5：若模长|error|大于预设定的阈值b，则判断当前gnss数据now_data出现了跳变，舍弃该数据，回到步骤2；
11.若模长|error|小于或等于预设定的阈值b，则判断当前gnss数据now_data正常，将error清零避免产生累计误差，回到步骤2；
12.由此，实现循环处理。
13.应用于自动驾驶设备时，若自动驾驶设备运动较快，阈值a和阈值b同比增大，若自
动驾驶设备运动较慢，阈值a和阈值b同比减小。
14.通过采用上述技术方案，本发明的有益效果是：上述技术方案的一种gnss跳变数据的剔除算法，通过判断相邻两个时刻和历史时刻的gnss数据变化大小，判断当前的gnss数据是否属于跳变数据并进行剔除，从而得到可靠的gnss数据。gnss数据可用于自动驾驶中的多传感器融合地图构建及定位，剔除跳变数据后的gnss数据能够更精确的帮助进行点云地图构建与定位。本发明有利于构建自动驾驶所需的3d点地图及实时定位。
附图说明
15.图1是本发明涉及的一种gnss跳变数据位姿示意图。
16.图2是实际采集的存在gnss跳变数据的gnss数据示意图；
17.图3是图2的同环境采集通过采用本发明的一种gnss跳变数据剔除算法处理得到的gnss数据示意图。
具体实施方式
18.为了进一步解释本发明的技术方案，下面通过具体实施例来对本发明进行详细阐述。
19.这里先说明一下本实施例中所述的gnss数据均为已经经过坐标转换的数据，即对应x、y、z三维坐标的三维向量。
20.本发明一种gnss跳变数据剔除算法，算法步骤如下：
21.步骤1、接收传感器传传来的gnss数据，以不受干扰的正确的gnss数据作为第一个gnss数据，即起始数据，初始跳变误差记为error，error为三维向量，分别对应x、y、z三维坐标，error的模长则为|error|，初始|error|＝0；然后进入步骤2；
22.该步骤1中对gnss跳变数据的剔除处理以第一个数据作为正确参照，判断后续数据是否产生跳变；
23.步骤2、继续接收传感器传来的gnss数据，最新接收的数据作为当前时刻数据记为now_data，now_data前未被舍弃的上一时刻数据记为old_data；然后进入步骤3；
24.步骤3：计算now_data相对于old_data的位姿变换(旋转+平移变换)，并得到相对移动向量记为d(由old_data坐标点指向now_data坐标点的向量),d的模长为相对移动距离即为|d|；然后进入步骤4；
25.步骤4：若|d|大于预设定的阈值a，则记录error＝error+d(两向量相加)，否则不进行其他操作；然后进入步骤5；
26.步骤5：若模长|error|大于预设定的阈值b，则判断当前gnss数据now_data出现了跳变，舍弃该数据，回到步骤2；
27.若模长|error|小于或等于预设定的阈值b，则判断当前gnss数据now_data正常，将error清零避免产生累计误差，回到步骤2；
28.由此，实现循环处理。
29.根据上面的方法步骤下面结合具体的实施例方式描述一下本发明的算法应用，如图1所示，每个箭头代表一个gnss数据位姿，其中3号和4号箭头表示产生跳变的gnss数据位姿，是应该剔除的gnss跳变数据，1号箭头为正常gnss数据位姿，以1号箭头为初始位姿，也
就是第一个gnss数据，即起始数据，进行跳变数据处理。
30.设连续两个位姿的相对距离在不产生跳变的情况下不大于阈值a，如1号位姿跟2号位姿；设连续两个位姿的相对距离在均产生同样跳变的情况下不大于阈值a，如3号位姿跟4号位姿；设连续两个位姿的相对距离在只有其中一个位姿跳变的情况下大于阈值b，如2号位姿跟3号位姿。
31.首先接收到1号位姿，此时只有一个位姿，初始误差|error|＝0，继续接收下一个位姿；然后接收到2号位姿，此时2号位姿为当前时刻数据now_data，1号位姿为上一时刻位姿old_data；接着计算2号位姿now_data相对于1号位姿old_data的位姿变换(旋转+平移变换)，并得到相对移动向量d(由old_data坐标点指向now_data坐标点的向量),d的模长为相对移动距离|d|；因为相对移动距离|d|不大于阈值a，因此视1号位姿和2号位姿均未产生跳变，或均产生了同样的跳变，因此不对error进行累加，目前跳变误差的模长|error|为0，不大于阈值b，进行清零动作，仍为0；第一对位姿处理结束；
32.继续循环处理下一对，继续接收到3号位姿，此时3号位姿为当前时刻数据now_data，2号位姿为上一时刻位姿old_data；接着计算3号位姿now_data相对于2号位姿old_data的位姿变换(旋转+平移变换)，并得到相对移动向量d；因为3号位姿开始产生跳变，因此相对移动距离|d|大于阈值a，视3号位姿与2号位姿之间产生了跳变，因此记录3号位姿对应的跳变误差error＝error+d(两向量相加)，目前跳变误差的模|error|大于阈值b，因此视3号位姿为跳变位姿，进行舍弃；第二对位姿处理结束；
33.继续循环处理下一对，继续接收到4号位姿，此时4号位姿为当前时刻数据now_data，3号位姿为上一时刻位姿old_data；接着计算4号位姿now_data相对于3号位姿old_data的位姿变换(旋转+平移变换)，并得到相对移动向量d；因为相对移动距离|d|不大于阈值a，因此视3号位姿和4号位姿均未产生跳变，或均产生了同样的跳变，因此不对error进行累加，目前跳变误差的模|error|大于阈值b，因此视4号位姿为跳变位姿，进行舍弃；第三对位姿处理结束；
34.继续循环处理下一对，继续接收到5号位姿，此时5号位姿为当前时刻数据now_data，4号位姿为上一时刻位姿old_data；接着计算5号位姿now_data相对于4号位姿old_data的位姿变换(旋转+平移变换)，并得到相对移动向量d；因为4号位姿为跳变位姿，而5号位姿为正常位姿，因此相对移动距离|d|大于阈值a，视5号位姿与4号位姿之间产生了跳变，因此记录5号位姿对应的跳变误差error＝error+d(两向量相加)，目前跳变误差的模|error|不大于阈值b(2、3号位姿之间的相对变换与4、5号位姿之间的相对变换相抵消)，对error进行清零动作；第四对位姿处理结束；
35.由此便完成了对5个位姿组成的gnss数据的跳变数据剔除处理，成功剔除了3号和4号跳变位姿，保留了1、2、5号正常位姿，1、2、5号位姿可以供给多传感器融合建图算法或融合定位算法进行正常使用。
36.其中，阈值a用以判断两个位姿之间是否超过正常变换，阈值b用以判断某个位姿是否为跳变位姿，阈值大小根据具体情况依经验进行取值，如在采集数据时，无人驾驶汽车运动较快，则两个gnss位姿之间的正常变换较大，阈值可取大些，避免将正常误差视为跳变，反之则可以取得较小些。
37.如图2所示为实际测试采集得gnss数据，其中部分数据因采集设备处于树荫等遮
蔽物下产生了跳变；如图3所示为经过本发明算法处理后的gnss数据，剔除了跳变数据，保留了正常数据得出的处理结果。
38.上述实施例和图式并非限定本发明的产品形态和式样，任何所属技术领域的普通技术人员对其所做的适当变化或修饰，皆应视为不脱离本发明的专利范畴。