一种移动机器人多传感器融合双层滤波定位方法与流程

本发明属于移动机器人定位，尤其涉及一种移动机器人多传感器融合双层滤波定位方法。

背景技术：

1、随着经济的发展和科技的不断进步，移动机器人的应用日益广泛，在复杂的室内外环境中，准确的定位是移动机器人移动的前提，各种依靠单一传感器的定位方法存在不可避免的缺陷且环境适应性差，gnss在高楼林立的复杂城市中因为信号受到遮挡会导致定位不准确，依靠imu获取位姿信息进行定位，容易产生累计误差，也会影响定位精度。多传感器融合定位技术能够有效解决上述问题，但在融合过程中容易出现由于某些传感器数据处理耗时长，导致数据时间不同步，输出的定位数据有误差的问题，进而降低了定位精度。

技术实现思路

1、本发明的目的在于提供一种移动机器人多传感器融合双层滤波定位方法，旨在解决多传感器融合中出现的由于某些传感器数据处理耗时长，导致数据时间不同步，输出的定位数据有误差，从而降低了定位精度的问题。

2、一方面，本发明提供了一种移动机器人多传感器融合双层滤波定位方法，所述方法包括以下步骤：

3、步骤一、获取imu数据并将其存入第一数组，获取轮速计数据并将其存入第二数组；

4、步骤二、通过计算获取激光雷达里程计的数据作并将其存入第三数组；

5、步骤三、判断第三数组是否为空，若是，则执行步骤六，若否，则执行步骤四；

6、步骤四、将所述第一数组、所述第二数组和所述第三数组中各数据的时间戳一一对齐，由第一滤波器分别根据所述第一数组中的数据进行位姿预测和所述第二数组以及所述第三数组中的数据进行位姿更新，输出定位数据，丢弃三个所述数组中的数据；

7、步骤五、由第二滤波器根据第一滤波器的参数进行初始化，根据所述第一数组中的所述imu数据进行预测，获得当前时刻位姿并将其实时输出；

8、步骤六、由第二滤波器根据所述第一数组中新来的一帧imu数据预测位姿，获得当前时刻位姿并将其实时输出。

9、进一步地，所述激光里程计的获取包括如下步骤：

10、由组合导航系统对所述imu数据和rtk数据进行融合，获得组合导航里程计；

11、获取激光雷达点云帧的时间戳，通过所述时间戳前后的所述组合导航里程计的数据使用时间插值的方法计算位姿并将其作为ndt算法的初始位姿，对激光雷达点云帧与高精点云地图使用ndt算法进行点云配准,获得激光雷达里程计。

12、进一步地，所述第一滤波器和第二滤波器均基于误差模型及运动模型计算根据所述imu数据获得imu预测位姿。

13、进一步地，还包括设置标志位，通过所述标志位判断所述第二滤波器执行步骤五还是步骤六。

14、进一步地，所述步骤二中所述初始位姿的获取包括以下具体步骤：

15、将所述组合导航里程计的数据存入数组a中；

16、从所述数组a中找出大于激光雷达点云时间戳tl的最近一帧组合导航里程计的位姿pn，其时间戳为tn，小于激光雷达点云时间戳tl的最近一帧组合导航里程计的位姿pc，其时间戳为tc，根据时间差值求解比例v1和v2，然后按照比例求解位姿，获得对应激光雷达时间戳tl的初始位姿，计算公式如下所示：

17、

18、

19、pi＝pc*v1+pn*v2  (3)

20、其中，pi为对应激光雷达时间戳tl的初始位姿。

21、进一步地，所述第一滤波器和所述第二滤波器均用采用误差状态卡尔曼滤波算法，所述误差状态卡尔曼滤波算法包括预测和更新。

22、另一方面，本文还提供了一种移动机器人多传感器融合双层滤波定位装置，所述装置包括：

23、第一数据获取模块，用于获取imu数据和轮速计数据，并分别存入第一数组和第二数组；

24、第二数据获取模块，用于通过计算获取激光雷达里程计，并将所述激光里程计的数据存入第三数组，获取激光雷达里程计的具体步骤如下：

25、由组合导航系统对imu数据和rtk数据进行融合，获得组合导航里程计；

26、获取激光雷达点云帧的时间戳，通过所述时间戳前后的所述组合导航里程计的数据使用时间插值的方法计算位姿并将其作为ndt算法的初始位姿，对激光雷达点云帧与高精点云地图使用ndt算法进行点云配准,获得激光雷达里程计；

27、判断模块，用于是否获取到激光雷达里程计的数据；

28、第一数据处理模块，用于将所述imu数据、激光雷达里程计的数据以及轮速计数据的时间戳一一对齐，由第一滤波器分别根据所述imu数据进行位姿预测和所述激光雷达里程计以及轮速计数据进行位姿更新，弃使用过的三个数组中的数据；由第二滤波器根据第一滤波器的参数进行初始化，根据第一数组中的imu数据进行预测，获得当前时刻位姿；

29、第二数据处理模块，用于由第二滤波器根据第一数组中新来的一帧imu数据进行预测，获得当前时刻位姿；

30、执行模块，用于将第一数据处理模块或第二数据处理模块获得的当前时刻位姿实时输出。

31、本发明的有益效果：本发明针对多传感器融合定位算法中多传感器融合中出现的由于某些传感器数据处理耗时长，导致数据时间不同步，输出的定位数据有误差的问题，提出了一种移动机器人多传感器融合双层滤波定位方法，通过设置第一滤波器和第二滤波器，对激光雷达里程计的数据发生的延时进行处理，确保数据时间同步，实时输出当前时刻定位位姿，解决了传感器数据处理耗时过长导致数据时间不同步的问题，提高了定位精度。

技术特征：

1.一种移动机器人多传感器融合双层滤波定位方法，其特征在于，所述方法包括：

2.根据权利要求1所述的移动机器人多传感器融合双层滤波定位方法，其特征在于，所述激光里程计的获取包括如下步骤：

3.根据权利要求1所述的移动机器人多传感器融合双层滤波定位方法，其特征在于，所述第一滤波器和第二滤波器均基于误差模型及运动模型计算根据所述imu数据获得imu预测位姿。

4.根据权利要求1所述的移动机器人多传感器融合双层滤波定位方法，其特征在于，还包括设置标志位，通过所述标志位判断所述第二滤波器执行步骤五还是步骤六。

5.根据权利要求2所述的移动机器人多传感器融合双层滤波定位方法，其特征在于，所述步骤二中所述初始位姿的获取包括以下具体步骤：

6.根据权利要求1-5中任一所述的移动机器人多传感器融合双层滤波定位方法，其特征在于，所述第一滤波器和所述第二滤波器均用采用误差状态卡尔曼滤波算法，所述误差状态卡尔曼滤波算法包括预测和更新。

7.一种移动机器人多传感器融合双层滤波定位装置，其特征在于，包括以下模块：

技术总结
本发明适用移动机器人技术领域，提供了一种移动机器人多传感器融合双层滤波定位方法，本方法的目的在于通过设置第一滤波器和第二滤波器，对激光雷达里程计的数据发生的延时进行处理，确保数据时间同步，实时输出当前时刻定位位姿，解决了传感器数据处理耗时过长导致数据时间不同步的问题，提高了定位精度。

技术研发人员：邱仁高
受保护的技术使用者：深圳市普蓝机器人有限公司
技术研发日：
技术公布日：2024/1/13