【技术实现步骤摘要】
本专利技术涉及地图定位，尤其涉及一种基于多传感器融合的地面机器人重定位方法及装置。
技术介绍
1、随着slam（simultaneous localization and mapping，同时定位与地图构建）技术的发展，通过融合激光雷达，imu，gps等多传感器进行建图定位，是智能无人车自动导航的重要基础。现有的普通导航地图一般为道路级地图，包含地图信息少，精度为米级。而基于高精度地图的定位可以精确到10-20cm级别，可以为自动驾驶提供更全面的数据基础。
2、现有的基于激光雷达的高精度地图定位方法，在未获得先验位置姿态数据的情况下，由于激光点云的稀疏性，很难完成自主的全局定位，鲁棒性差，易受传感器误差影响，容易出现无法重定位或重定位结果差距大的情况。通常基于激光雷达的高精地图定位方式，采用搭载3d激光雷达的设备进行数据采集，再结合采集到的数据与高精地图匹配，通常采用icp算法（iterative closest point）或ndt（normal distributions transform）算法进行计算，获得机器人的位置姿态。由于该定位结果由地图匹配计算得来，高精地图一般面积较大，不可避免会存在检索所需计算量大，相似场景多的情况，导致无法定位、定位出错，或者系统负载过大导致计算卡顿，无法实时运行。为了避免系统负载过大的情况出现，通常会降低地图匹配的速率，并结合里程计进行高频定位结果输出。如通过ekf算法（extendedkalman filter，扩展卡尔曼滤波算法）融合激光雷达数据和imu传感器数据输出高频定
技术实现思路
1、本专利技术的目的在于克服现有技术中的不足，提供一种基于多传感器融合的地面机器人重定位方法及装置，解决
技术介绍
中采用单传感器方式进行重定位操作存在的各种技术问题。
2、为达到上述目的，本专利技术是采用下述技术方案实现的：
3、第一方面，本专利技术提供了一种基于多传感器融合的地面机器人重定位方法，包括：
4、获取视觉相机数据、激光雷达数据以及imu数据；
5、基于视觉相机数据和激光雷达数据分别进行视觉重定位和激光重定位；
6、若视觉重定位和/或激光重定位成功，则根据视觉相机数据和/或激光雷达数据计算地面机器人的第一位置姿态估计值和/或第二位置姿态估计值；
7、基于激光雷达数据和imu数据通过eskf算法，计算地面机器人的第三位置姿态估计值；
8、获取上一时刻的地面机器人的位置姿态估计值，从预先生成的高精地图数据中获取临近的m个时刻的激光点云数据拼接生成局部地图；
9、基于激光雷达数据和局部地图计算地面机器人的第四位置姿态估计值；
10、基于第一位置姿态估计值、第二位置姿态估计值、第三位置姿态估计值以及第四位置姿态估计值，融合获取位置姿态最终值。
11、可选的，所述获取视觉相机数据、激光雷达数据以及imu数据还包括对视觉相机数据、激光雷达数据以及imu数据按其时间戳进行时间对齐。
12、可选的，若视觉重定位成功且激光重定位未成功，则根据视觉相机数据计算地面机器人的第一位置姿态估计值；
13、若视觉重定位未成功且激光重定位成功，则根据激光雷达数据计算地面机器人的第二位置姿态估计值；
14、若视觉重定位成功且激光重定位成功，则根据视觉相机数据和激光雷达数据分别计算地面机器人的第一位置姿态估计值和第二位置姿态估计值；
15、若视觉重定位未成功且激光重定位未成功，则重复执行以下步骤直至视觉重定位和/或激光重定位成功：
16、基于下一时刻的视觉相机数据和激光雷达数据分别进行激光重定位和视觉重定位。
17、可选的，所述视觉重定位包括：
18、从目标时刻的视觉相机数据中提取目标特征点，并将目标特征点聚类成不同种类的单词；
19、将单词的种类和出现次数相结合转化为bow描述子；
20、基于bow描述子遍历预构建的bow描述子库，查找所有具有相同单词种类的视觉图像数据，计算bow相似度得分；
21、将bow相似度得分最高的视觉图像数据对应的激光点云数据和目标时刻的激光雷达数据进行gicp配准；
22、若配准分数满足预设的视觉配准分数阈值，则视觉重定位成功；
23、根据视觉重定位成功对应的激光点云数据和激光雷达数据计算第一位置姿态估计值；
24、所述激光重定位包括：
25、根据目标时刻的激光雷达数据，计算sc描述子；
26、基于sc描述子遍历构建的sc描述子库，查找最近似的n个激光点云数据，分别计算sc相似度得分；
27、将sc相似度得分最高的激光点云数据和目标时刻的激光雷达数据进行gicp配准；
28、若配准分数满足预设的激光配准分数阈值，则激光重定位成功；
29、根据激光重定位成功对应的激光点云数据和激光雷达数据计算第二位置姿态估计值；
30、其中，所述sc描述子库和bow描述子库的构建过程包括：
31、读取预先生成的高精地图数据，对每一时刻的视觉图像数据进行bow描述子计算并保存描述子至bow描述子库；对每一时刻的激光点云数据进行sc描述子计算并保存描述子至sc描述子库。
32、可选的，所述第三位置姿态估计值的计算过程包括：
33、构建地面机器人在k时刻的估计状态量：
34、；
35、式中，分别为地面机器人在k时刻的位置、速度、姿态、加速度计偏置量、陀螺仪偏置量以及重力加速度；
36、对估计状态量对时间求导，获取imu数据的离散误差递推公式，并基于离散误差递推公式组成eskf算法中预测过程中的雅可比矩阵和，并构建预测方程：
37、；
38、；
39、式中，为k时刻相对于k-1时刻的预测状态量误差，为预测状态量误差对应的预测协方差矩阵，为预测协方差矩阵的修正值；
40、将k-1时刻至k时刻之间的imu数据带入预测方程进行递推，获取预测状态量误差和预测协方差矩阵；
41、构建预测方程的观测方程：
42、；
43、式中，为观测数据，为观测噪声，为观测噪声的协方差矩阵，为抽象函数；
44、进一步计算卡尔曼增益：
45、；
46、；
47、式中，为中间变量；
48、基于卡尔曼增益计算预测状态量误差和预测协方差矩阵的修正值：
49、；
50、；
51、基于修正值修正估计状态量，并从修正后的估计状态量中提取位置和姿态作为第三估计值。
52、可选的，所述拼接生成局部地图还包括：
53、对局部地图进本文档来自技高网...
【技术保护点】
1.一种基于多传感器融合的地面机器人重定位方法，其特征在于，包括：
2.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，所述获取视觉相机数据、激光雷达数据以及IMU数据还包括对视觉相机数据、激光雷达数据以及IMU数据按其时间戳进行时间对齐。
3.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，
4.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，
5.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，所述第三位置姿态估计值的计算过程包括：
6.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，所述拼接生成局部地图还包括：
7.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，所述基于激光雷达数据和局部地图计算地面机器人的第四位置姿态估计值包括：
8.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，所述融合获取位置姿态最终值包括：
<...【技术特征摘要】
1.一种基于多传感器融合的地面机器人重定位方法，其特征在于，包括：
2.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，所述获取视觉相机数据、激光雷达数据以及imu数据还包括对视觉相机数据、激光雷达数据以及imu数据按其时间戳进行时间对齐。
3.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，
4.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，
5.根据权利要求1所述的基于多传感器融合的地面机器人重定位方法，其特征在于，所述第三位置姿态估计值的计算过程包括：
<...【专利技术属性】
技术研发人员：沈嘉成，李峰岳，李高铭，曾元一，张小涛，赖善炎，陈海鹏，杨宗林，张馨月，尚德龙，周玉梅，
申请(专利权)人：中科南京智能技术研究院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。