本发明涉及定位，尤其涉及基于双滤波器的多传感器融合定位方法、装置及设备。
背景技术：
1、自主移动机器人是一种高度智能化的设备，能够通过内置的传感器、执行器和控制系统实现在复杂环境中的自主导航和移动。这类机器人广泛应用于工业自动化、服务行业、医疗辅助、农业以及探索和救援等领域。
2、进行定位时，经常需要融合各种传感器和信号源的数据，如gps(英文：globalpositioning system，全球定位系统)和雷达，尽管gps在室外开阔场景下可以为自主移动机器人提供高精度的定位能力，但在受遮蔽的场所，如树木茂盛的区域或高楼大厦周边，gps信号容易受到干扰或完全丢失，导致定位失效。此外，当采用ndt(normaldistributions transform，正态分布变换)配准时，由于ndt配准是通过计算点云之间的变换关系来实现精确对齐，因此，当数据集较大时，配准过程耗时较长，这在实时导航中可能导致机器人出现回退现象，影响其稳定性和可靠性。
3、因此，仍需对现有的融合定位方法做改进，以解决现有技术进行ndt配准时容易因为时延导致回退的问题。
技术实现思路
1、有鉴于此，本发明的目的是提供基于双滤波器的多传感器融合定位方法、装置及设备，以解决现有技术进行ndt配准时容易因为时延导致回退的问题。
2、为实现上述目的，本发明提供一种基于双滤波器的多传感器融合定位方法，包括如下步骤：
3、步骤s1，获取点云以及存放在第一缓存区中的批量imu数据；
4、步骤s2，判断位于所述第一缓存区头部的所述imu数据，时间戳是否小于所述点云的时间戳；将距离所述点云最近的所述imu数据作为第一目标数据；
5、步骤s3，若不小于所述点云的时间戳，则
6、计算所述第一目标数据对应的第一imu预测值，并根据所述第一imu预测值、所述点云以及预存的点云地图进行配准，得到配准位姿；
7、根据所述第一imu预测值、所述配准位姿以及预设的第一滤波器进行数据融合，得到第一当前位姿；
8、步骤s4，若小于所述点云的时间戳，则
9、根据位于头部的所述imu数据以及预设的运动状态方程进行运算，得到对应时刻状态量；
10、根据所述对应时刻状态量以及预设的第二滤波器进行预测，得到预测状态量，作为下一轮预测的基准；
11、释放完成预测的所述imu数据，并跳转至步骤s2进行再次判断。
12、在一些实施例中，还包括：
13、步骤s5，获取存放在第二缓存区中的批量所述imu数据；
14、步骤s6，判断是否有gps数据；将距离所述gps数据最近的所述imu数据作为第二目标数据；
15、步骤s7，若有所述gps数据，则计算所述第二目标数据对应的第二imu预测值；根据所述第二imu预测值、所述gps数据以及预设的第三滤波器进行数据融合，得到第二当前位姿；
16、步骤s8，根据所述第一当前位姿、所述第二当前位姿以及预设的最终位姿生成规则，得到最终的当前位姿。
17、在一些实施例中，步骤s6之后，还包括：若没有所述gps数据，则
18、根据位于头部的所述imu数据以及所述运动状态方程进行运算，得到所述对应时刻状态量；
19、根据所述对应时刻状态量以及预设的第四滤波器进行预测，得到所述预测状态量，作为下一轮预测的基准；
20、释放完成预测的所述imu数据，并跳转至步骤s6进行再次判断。
21、在一些实施例中，所述第一滤波器至所述第四滤波器，至少有一者为无损卡尔曼滤波器。
22、在一些实施例中，步骤s8中，所述最终位姿生成规则为：
23、基于所述第一当前位姿，计算δxyl、δyl，并基于所述第二当前位姿，计算δxyg、δyg；其中，δxyl为基于雷达检测时当前时刻与上一时刻在坐标系平面内的位置变化量，δyl为基于雷达检测时当前时刻与上一时刻的航向角旋转变化量，δxyg为基于gps检测时当前时刻与上一时刻在坐标系平面内的位置变化量，δyg为基于gps检测时当前时刻与上一时刻的航向角旋转变化量；
24、对δxyl、δxyg进行差值运算，得到δxy；对δyl、δyg进行差值运算，得到δy；
25、判断δxy、δy是否符合预设条件；
26、若不符合所述预设条件，则根据所述初始的第一当前位姿、所述初始的第二当前位姿以及预存的修正公式进行修正，得到修正后的当前位姿，作为所述最终的当前位姿。
27、在一些实施例中，所述预设条件为：δxy≤0.1米并且δy≤0.04rad。
28、在一些实施例中，步骤s1之前，还包括：
29、步骤s9，判断imu数据频率是否大于第一预设值；将两帧所述点云的时间间隔作为基准时段，所述基准时段由选取时段及非选取时段组成，选取时段与基准时段的比值为第二预设值；
30、步骤s10，若大于所述第一预设值，则仅将选取时段上的所述imu数据存入所述第一缓存区。
31、本发明还提供一种多传感器融合定位装置，包括：
32、获取单元，用于获取点云以及存放在第一缓存区中的批量imu数据；
33、判断单元，用于判断位于所述第一缓存区头部的所述imu数据，时间戳是否小于所述点云的时间戳；将距离所述点云最近的所述imu数据作为第一目标数据；
34、第一处理单元，用于在不小于所述点云的时间戳时，计算所述第一目标数据对应的第一imu预测值，并根据所述第一imu预测值、所述点云以及预存的点云地图进行配准，得到配准位姿；根据所述第一imu预测值、所述配准位姿以及预设的第一滤波器进行数据融合，得到第一当前位姿；
35、第二处理单元，若小于所述点云的时间戳，则根据位于头部的所述imu数据以及预设的运动状态方程进行运算，得到对应时刻状态量；根据所述对应时刻状态量以及预设的第二滤波器进行预测，得到预测状态量，作为下一轮预测的基准；释放完成预测的所述imu数据，并跳转至判断时间戳的步骤进行再次判断。
36、本发明还提供一种多传感器融合定位设备，包括存储器和至少一个处理器，所述存储器中存储有指令；所述至少一个处理器调用所述存储器中的所述指令，以使得所述多传感器融合定位设备执行如上所述的基于双滤波器的多传感器融合定位方法。
37、本发明还提供一种计算机可读存储介质，存储有指令，所述指令被处理器执行时实现如上所述的基于双滤波器的多传感器融合定位方法。
38、与现有技术相比，本发明的有益效果在于：通过获取点云以及存放在第一缓存区中的批量imu数据；进而，判断位于所述第一缓存区头部的所述imu数据，时间戳是否小于所述点云的时间戳，将距离所述点云最近的所述imu数据作为第一目标数据；若不小于所述点云的时间戳，则计算所述第一目标数据对应的第一imu预测值，并根据所述第一imu预测值、所述点云以及预存的点云地图进行配准，得到配准位姿，根据所述第一imu预测值、所述配准位姿以及预设的第一滤波器进行数据融合，得到第一当前位姿；若小于所述点云的时间戳，则根据位于头部的所述imu数据以及预设的运动状态方程进行运算，得到对应时刻状态量，根据所述对应时刻状态量以及预设的第二滤波器进行预测，得到预测状态量，作为下一轮预测的基准，释放完成预测的所述imu数据，并跳转至时间戳判断步骤进行再次判断。通过前述方法，进行配准时，imu预测值提供了一个初步的位姿估计，这个估计值非常接近真实的位姿，这样在进行点云配准时，可以减少搜索空间，显著减少计算量，提高配准的效率。并且，imu数据的高频率和实时性，使得在配准之前，设备可以依靠imu预测值进行初步定位，确保设备在整个过程中始终保持较好的定位精度和实时性，避免由于时延导致的定位不准问题。
1.一种基于双滤波器的多传感器融合定位方法，其特征在于，包括如下步骤：
2.如权利要求1所述的基于双滤波器的多传感器融合定位方法，其特征在于，还包括：
3.如权利要求2所述的基于双滤波器的多传感器融合定位方法，其特征在于，步骤s6之后，还包括：若没有所述gps数据，则
4.如权利要求3所述的基于双滤波器的多传感器融合定位方法，其特征在于，所述第一滤波器至所述第四滤波器，至少有一者为无损卡尔曼滤波器。
5.如权利要求2所述的基于双滤波器的多传感器融合定位方法，其特征在于，步骤s8中，所述最终位姿生成规则为：
6.如权利要求5所述的基于双滤波器的多传感器融合定位方法，其特征在于，所述预设条件为：δxy≤0.1米并且δy≤0.04rad。
7.如权利要求1所述的基于双滤波器的多传感器融合定位方法，其特征在于，步骤s1之前，还包括：
8.一种多传感器融合定位装置，其特征在于，包括：
9.一种多传感器融合定位设备，其特征在于，包括存储器和至少一个处理器，所述存储器中存储有指令；所述至少一个处理器调用所述存储器中的所述指令，以使得所述多传感器融合定位设备执行如权利要求1-7中任一项所述的基于双滤波器的多传感器融合定位方法。
10.一种计算机可读存储介质，存储有指令，其特征在于，所述指令被处理器执行时实现如权利要求1-7中任一项所述的基于双滤波器的多传感器融合定位方法。
