一种基于路标的定位数据筛选方法与流程

本发明属于机器人地图的定位信息处理领域，具体涉及一种基于路标的定位数据筛选方法。

背景技术：

1、在相关技术领域的普通技术人员可以理解的基础上，机器人在执行slam(simultaneous localization and mapping)全称即时定位与地图构建或并发建图与定位的过程中，让机器人在未知的环境中，完成定位（localization)，建图(mapping)和路径规划(navigation)。每次将周围环境的特征以地图的形式保存起来，作为历史地图。机器人下次遍历同一环境之前，可预先把历史地图显示出来。则会出现历史地图和当前地图的关系，即两幅图存在一个转换关系，且历史地图需要一个定位过程。根据历史地图定位出的位姿与当前地图的位姿，确定历史地图与当前地图的转换关系，以实现将历史地图旋转平移，使其与当前地图重合。

2、目前，主流的slam技术应用有两种，分别是激光slam(基于激光雷达lidar 来建图导航)和视觉slam(vslam,基于单/双目摄像头视觉建图导航)，由于传感器的误差以及周围环境光照因素的影响，所以视觉或激光对移动机器人的自身定位或路标的定位都会存在误差；如何从目前存在误差或错误的机器人地图定位信息中找到正确的定位信息成为需要解决的技术问题。

技术实现思路

1、针对如何从目前存在误差或错误的机器人地图定位信息中找到正确的定位信息的技术问题，本发明公开一种基于路标的定位数据筛选方法，先后从定位数据的对比、过滤和筛选方面出发，可以从一堆定位数据中查找到最正确的定位数据以服务于机器人地图；具体的技术方案如下：

2、一种基于路标的定位数据筛选方法，所述定位数据筛选方法包括：步骤1、根据路标对应的特征点的集中度，确定定位成功的路标，再将定位成功的路标的定位数据标记为目标定位数据；步骤2、机器人每遍历一个目标定位数据，则筛选出落入该目标定位数据的预设位移范围内的目标定位数据，再对筛选出的目标定位数据进行角度差过滤，然后机器人获得参考定位数据组及其内部存在的目标定位数据；步骤3、机器人对参考定位数据组内的目标定位数据进行平均值处理，获得平均定位数据，然后从参考定位数据组内的目标定位数据中，选择相对于平均定位数据产生的位姿差异程度最小的目标定位数据设置为最优定位数据。

3、进一步地，所述定位数据包括同一个路标在历史地图内的位姿与当前地图内的位姿之间的转换关系；或所述定位数据包括历史地图的地图坐标系与当前地图的地图坐标系之间的转换关系；路标是配置为用于机器人定位的参考物或机器人地图内的参考坐标位置；所述转换关系由旋转角度和坐标偏移量表示，或该转换关系由旋转角度换算出的旋转矩阵、以及由坐标偏移量换算出的平移向量表示；每个路标对应一个定位数据，每个定位数据对应一个旋转角度和一个坐标偏移量；机器人将一个目标定位数据的预设位移范围配置为：以该目标定位数据为中心定位数据，将坐标偏移量与该中心定位数据的坐标偏移量在各个坐标轴方向上的差值的绝对值小于或等于预设坐标差值阈值的定位数据组成的集合。

4、进一步地，在所述步骤2中，每当机器人筛选出坐标偏移量落入对应的目标定位数据的预设位移范围内的一个目标定位数据，则将筛选出的目标定位数据标记为待匹配定位数据，并对坐标偏移量落入同一个预设位移范围内的目标定位数据进行计数，并在该预设位移范围内对该待匹配定位数据的旋转角度作一次累加，直至机器人筛选完坐标偏移量落入该待匹配定位数据内的所有目标定位数据，获得角度累加值和坐标偏移量落入该预设位移范围内的目标定位数据的数量，再计算该角度累加值与坐标偏移量落入该预设位移范围内的目标定位数据的数量的比值，得到平均角度值。

5、进一步地，所述步骤2包括：对于每个目标定位数据，机器人依次筛选出坐标偏移量落入对应的预设位移范围内的目标定位数据，再将机器人在同一个目标定位数据的预设位移范围内筛选出的所有目标定位数据组成一个定位组，并将该定位组内的目标定位数据标记为待匹配定位数据，并对待匹配定位数据的旋转角度进行累加处理，再在累加完处于同一个定位组内所有的待匹配定位数据的旋转角度后，获得角度累加值，然后对最新获得的角度累加值作平均值处理，获得属于该定位组的平均角度值，然后在该定位组内，机器人比较待匹配定位数据的旋转角度与当前获得的平均角度值，再依据比较结果，从该定位组内剔除旋转角度不满足预设角度差条件的待匹配定位数据；当机器人在每个定位组内都剔除旋转角度不满足预设角度差条件的待匹配定位数据后，从现存的所有定位组当中选择待匹配定位数据的数量最多的定位组设置为所述参考定位数据组。

6、进一步地，在所述步骤2中，所述对筛选出的目标定位数据进行角度差过滤的方法包括：每当机器人获得属于一个定位组的平均角度值，则在该定位组内依次控制每个待匹配定位数据的旋转角度与该平均角度值作差，获得对应待匹配定位数据的角度差值；每当获得的角度差值的绝对值大于预设角度阈值，则从该定位组内剔除该角度差值对应的待匹配定位数据，并确定从该定位组内剔除旋转角度不满足预设角度差条件的待匹配定位数据，并将剔除待匹配定位数据后的定位组更新为所述定位组；机器人在每个定位组内都对待匹配定位数据进行角度差过滤之后，从最新获得的所有的定位组当中，选择待匹配定位数据的数量最多的定位组设置为所述参考定位数据组，并获得参考定位数据组内的目标定位数据。

7、进一步地，每个目标定位数据包括坐标偏移量和旋转角度；在所述步骤3中，所述机器人对参考定位数据组内的目标定位数据进行平均值处理，获得平均定位数据的方法包括：在同一个参考定位数据组内，机器人对目标定位数据在x轴方向上的坐标偏移量进行累加计算，获得该参考定位数据组内的x轴坐标偏移量累加结果，再对该x轴坐标偏移量累加结果求平均值，获得x轴坐标平均偏移量；机器人还对目标定位数据在y轴方向上的坐标偏移量进行累加计算，获得该参考定位数据组内的y轴坐标偏移量累加结果，再对该y轴坐标偏移量累加结果求平均值，获得y轴坐标平均偏移量；机器人还对目标定位数据的旋转角度进行累加计算，获得该参考定位数据组内的旋转角度累加结果，再对该旋转角度累加结果求平均值，获得平均旋转角度。

8、进一步地，在步骤3中，所述从参考定位数据组内的目标定位数据中，选择相对于平均定位数据产生的位姿差异程度最小的目标定位数据设置为最优定位数据的方法包括：在同一个参考定位数据组内，对于每个目标定位数据，机器人计算该目标定位数据在x轴方向上的坐标偏移量与所述x轴坐标平均偏移量的差值的绝对值并将该差值的绝对值标记为第一平移统计差值，机器人计算该目标定位数据在y轴方向上的坐标偏移量与所述y轴坐标平均偏移量的差值的绝对值并将该差值的绝对值标记为第二平移统计差值，机器人计算该目标定位数据的旋转角度与所述平均旋转角度的差值的绝对值并将该差值的绝对值标记为统计角度差值，然后将第一平移统计差值、第二平移统计差值与统计角度差值的和值配置为该目标定位数据相对于平均定位数据产生的统计距离；其中，平均定位数据包括x轴坐标平均偏移量、y轴坐标平均偏移量和平均旋转角度；在同一个参考定位数据组内，机器人将相对于平均定位数据产生的统计距离最小的目标定位数据设置为最优定位数据，再使用最优定位数据所包括的旋转角度和坐标偏移量表示历史地图的地图坐标系与当前地图的地图坐标系之间的转换关系。

9、进一步地，在所述步骤1中，所述根据路标对应的特征点的集中度，确定定位成功的路标的方法包括：当机器人连续多次判断到同一个路标处于定位一致状态后，判断该路标对应的特征点的集中度是否大于预设密集度阈值，是则确定该路标对应的特征点分布集中，否则确定该路标对应的特征点分布不集中并确定该路标是定位成功的路标；当机器人判断到当前一个路标对应的特征点分布集中，且存在分布不集中的特征点所属的路标与所述当前一个路标处于定位一致状态时，确定所述当前一个路标是定位成功的路标。

10、进一步地，机器人配置每个路标由至少两个特征点表示；并配置密集计数值；每当机器人检测到用于表示同一个路标的两个特征点的欧式距离小于预设距离阈值，则对密集计数值计数一次；在机器人不重复地检测完用于表示同一个路标的任意两个特征点的欧式距离后，判断密集计数值与机器人累计的总检测次数的比值是否大于所述预设密集度阈值，是则确定该路标对应的特征点分布集中，否则确定该路标对应的特征点分布不集中；其中，机器人将密集计数值与机器人累计的总检测次数的比值配置为所述集中度。

11、进一步地，机器人为每个路标配置一个索引号，以区分不同的路标；机器人判断到同一个索引号对应的路标先后获得的定位数据之间的差异满足预设误差条件时，确定该索引号对应的路标处于定位一致状态；直至同一个索引号对应的路标被判断到处于定位一致状态的次数达到预设次数后，开始判断路标对应的特征点的集中度是否大于预设密集度阈值。

12、进一步地，机器人为每个路标配置一个索引号，以区分不同的路标；当机器人判断到一个索引号对应的路标的定位数据与另一个索引号对应的路标的定位数据之间的差异满足预设误差条件时，确定该两个索引号对应的路标处于定位一致状态。

13、进一步地，路标的定位数据包括坐标偏移量和旋转角度；两个路标的定位数据的差异、或同一个路标在不同时间下的定位数据的差异包括x轴方向上的坐标偏移量的差值、y轴方向上的坐标偏移量的差值以及旋转角度的差值；机器人检测两个定位数据的差异的过程中，当机器人检测到x轴方向上的坐标偏移量的差值小于第一预设平移误差值，且y轴方向上的坐标偏移量的差值小于第二预设平移误差值，且旋转角度的差值小于预设角度误差值时，确定该两个定位数据的差异满足预设误差条件。

14、进一步地，所述步骤1还包括：机器人将目定位数据依次存入线性存储空间；在机器人将所有目标定位数据都存入线性存储空间后，若机器人检测到线性存储空间内存储的目标定位数据的数量小于预设数量阈值时，将线性存储空间存储的目标定位数据配置为所述参考定位数据组内的目标定位数据，然后执行所述步骤3；预设数量阈值是小于线性存储空间的最大数据容量。

15、本发明的有益技术效果在于：机器人通过配置一个目标定位数据的预设位移范围，来搜索与该目标定位数据比较接近的其余目标定位数据，再对这些搜索到的目标定位数据的旋转角度作平均值处理，以滤除旋转角度偏离平均值较大的目标定位数据，从而排除指向误差较大的路标；然后能够将剩余的目标定位数据的数量最多的一组数据集合筛选出来，成为一组优化的定位数据的集合，以便于从中选取更加具有代表性的有效数据。则机器人在该组优化的定位数据的集合内，分别算出其平移量和旋转量的平均值，选取出与相应的平均值的欧式距离最接近的定位数据作为最终的历史地图与当前地图转换关系，减少集中分布的定位数据存在的定位误差，实现从一堆定位数据中查找到最正确的定位数据以服务于机器人地图的转换和机器人定位。