自动割草机多传感器融合定位方法及装置与流程

本技术涉及割草机，尤其涉及一种自动割草机多传感器融合定位方法及装置。

背景技术：

1、自动割草机是一种在割草作业过程中无需人工参与就可完成割草的自动化园林设备。目前，自动割草机主要分为有边界和无边界两种方式。有边界自动割草机通过在工作区域边界采用围线的方式，自动割草机本体结构上安装信号检测传感器检测工作区域边界信号来实现导航和定位。无边界自动割草机采用的定位方式有rtk(real-time kinematic，载波相位差分)技术、视觉slam(simultaneous localizationandmapping，即时定位与建图)技术、激光slam技术等，但是这些技术或其组合的方式存在不足。

2、对于自动割草机工作场景，采用纯rtk定位方式，在空旷、无遮挡区域的工作区域，gnss(globalnavigation satellite system，全球卫星导航系统)模块为“rtk固定解”状态时能够提供厘米级高精度定位，但在树木或建筑物遮挡时，卫星信号被遮挡，产生多径效应引起定位失效。对于空旷、无遮挡的大面积场景，采用视觉或激光slam技术，特征较少或不明显，或自动割草机与特征物体距离较远，引起定位误差大，导致无法提供稳定的高精度定位数据。

3、如何提高基于多传感器融合的自动割草机的定位精度，成为本领域亟需解决的问题。

技术实现思路

1、本技术提供了一种自动割草机多传感器融合定位方法及装置，目的在于提高自动割草机的定位精度。

2、为了实现上述目的，本技术提供了以下技术方案：

3、一种自动割草机多传感器融合定位方法，所述多传感器包括激光雷达、惯性传感器以及卫星定位模块，所述激光雷达用于采集所述自动割草机周围环境的点云数据，所述惯性传感器用于监测所述自动割草机在任意时间点的三轴加速度和三轴角速度，所述卫星定位模块用于提供天线定位数据以及基站定位数据，所述天线定位数据包括预置在所述自动割草机上的天线在任意时间点的天线位置坐标和定位状态，所述基站定位数据包括基站位置坐标，所述方法包括：

4、在所述自动割草机启动后，基于所述基站位置坐标，建立目标坐标系；

5、基于关键时间戳对应的关键天线位置坐标，以及所述三轴加速度，对所述自动割草机的位姿进行初始化，以获得所述多传感器在所述目标坐标系下的位姿；所述关键时间戳为所述定位状态首次变更为第一状态的时间；

6、根据所述激光雷达以及所述卫星定位模块各自的定位效果，获得第一融合位姿；所述激光雷达的定位效果基于所述点云数据所确定；所述卫星定位模块的定位效果基于所述定位状态所确定；

7、根据所述第一融合位姿，以及所述惯性传感器的位姿，确定所述第二融合位姿；

8、基于所述第二融合位姿，计算得到所述自动割草机的实时位姿。

9、可选的，基于关键时间戳对应的关键天线位置坐标，以及所述三轴加速度，对所述多传感器的位姿进行初始化，以获得所述多传感器在所述目标坐标系下的位姿，包括：

10、获得关键时间戳对应的关键天线位置坐标；

11、基于所述关键天线位置坐标，确定所述自动割草机的初始位置坐标，以及所述自动割草机的初始航向角；

12、基于所述三轴加速度，计算得到所述自动割草机的初始姿态角；

13、根据所述初始姿态角、所述初始航向角以及所述初始位置坐标，对所述自动割草机的位姿进行初始化，以获得所述多传感器在所述目标坐标系下的位姿；所述位姿包括姿态角、航向角以及位置坐标。

14、可选的，基于所述点云数据，确定所述激光雷达的定位效果的过程，包括：

15、从所述点云数据中获得多个特征值；所述特征值表征相邻两帧点云之间的数据特征；

16、根据多个所述特征值中的最小值，判定所述点云数据是否发生退化；

17、如果所述点云数据有发生退化，确定所述激光雷达的定位效果为失效；

18、如果所述点云数据未发生退化，确定所述激光雷达的定位效果为有效。

19、可选的，根据所述激光雷达以及所述卫星定位模块各自的定位效果，获得第一融合位姿，包括：

20、在所述激光雷达的定位效果为有效，以及所述卫星定位模块的定位效果为有效的情况下，获得目标位姿数据；所述目标位姿数据包括多个目标时间戳，以及对应的卫星定位模块的位姿；所述目标时间戳为所述定位状态处于所述第一状态的时间；

21、基于对应的目标时间戳符合预设条件的卫星定位模块的位姿，确定第一位置坐标；所述预设条件为：目标时间戳与所述点云数据的采集时间之间的时间差符合阈值；

22、利用gtsam因子图，对所述第一位置坐标以及所述激光雷达的位姿进行优化，以获得第一融合位姿。

23、可选的，根据所述激光雷达以及所述卫星定位模块各自的定位效果，获得第一融合位姿，包括：

24、在所述激光雷达的定位效果为有效，以及所述卫星定位模块的定位效果为失效的情况下，基于所述激光雷达的位姿，确定为第一融合位姿。

25、可选的，根据所述激光雷达以及所述卫星定位模块各自的定位效果，获得第一融合位姿，包括：

26、在所述激光雷达的定位效果为失效，以及所述卫星定位模块的定位效果为有效的情况下，获得目标位姿数据；所述目标位姿数据包括多个目标时间戳，以及对应的卫星定位模块的位姿；所述目标时间戳为所述定位状态处于所述第一状态的时间；

27、基于最晚目标时间戳对应的卫星定位模块的位姿，确定第一航向角以及第二位置坐标；

28、利用gtsam因子图，对所述第二位置坐标进行优化，以获得第三位置坐标；

29、基于所述最晚目标时间戳对应的惯性传感器的位姿，确定对应的第一姿态角；

30、基于所述第一姿态角、所述第一航向角以及所述第三位置坐标，确定第一融合位姿。

31、可选的，根据所述第一融合位姿，以及所述惯性传感器的位姿，确定所述第二融合位姿，包括：

32、基于所述第一融合位姿作为观测量，利用gtsam因子图，对所述惯性传感器的零偏进行优化，以获得目标监测数据；所述目标监测数据包括所述惯性传感器在优化时间内的多个位姿；所述优化时间包括以第一时间点为起始直至第二时间点为结束的时间；所述第一时间点为零偏完成优化的结束时间，所述第二时间点为所述惯性传感器的当前系统时间；

33、对所述目标监测数据进行imu预积分，以获得所述惯性传感器的位姿队列；

34、基于所述惯性传感器的位姿队列，确定对应的位姿增量；

35、基于所述第一融合位姿，以及所述位姿增量，计算得到所述第二融合位姿。

36、一种自动割草机多传感器融合定位装置，所述多传感器包括激光雷达、惯性传感器以及卫星定位模块，所述激光雷达用于采集所述自动割草机周围环境的点云数据，所述惯性传感器用于监测所述自动割草机在任意时间点的三轴加速度和三轴角速度，所述卫星定位模块用于提供天线定位数据以及基站定位数据，所述天线定位数据包括预置在所述自动割草机上的天线在任意时间点的天线位置坐标和定位状态，所述基站定位数据包括基站位置坐标，所述装置包括：

37、坐标系确定单元，用于在所述自动割草机启动后，基于所述基站位置坐标，建立目标坐标系；

38、位姿初始化单元，用于基于关键时间戳对应的关键天线位置坐标，以及三轴加速度，对所述自动割草机的位姿进行初始化，以获得所述多传感器在所述目标坐标系下的位姿；所述关键时间戳为所述定位状态首次变更为第一状态的时间；

39、第一位姿融合单元，用于根据所述激光雷达以及所述卫星定位模块各自的定位效果，获得所述第一融合位姿；所述激光雷达的定位效果基于所述点云数据所确定；所述卫星定位模块的定位效果基于所述定位状态所确定；

40、第二位姿融合单元，用于根据所述第一融合位姿，以及所述惯性传感器的位姿，确定所述第二融合位姿；

41、位姿计算单元，用于基于所述第二融合位姿，计算得到所述自动割草机的实时位姿。

42、一种存储介质，所述存储介质包括存储的程序，其中，所述程序被处理器运行时执行所述的自动割草机多传感器融合定位方法。

43、一种自动割草机，包括多传感器以及处理器，其中，所述多传感器包括激光雷达、惯性传感器以及卫星定位模块，所述激光雷达用于采集所述自动割草机周围环境的点云数据，所述惯性传感器用于监测所述自动割草机在任意时间点的三轴加速度和三轴角速度，所述卫星定位模块用于提供天线定位数据以及基站定位数据，所述天线定位数据包括预置在所述自动割草机上的天线在任意时间点的天线位置坐标和定位状态，所述基站定位数据包括基站位置坐标，所述处理器用于执行所述的自动割草机多传感器融合定位方法。

44、本技术提供的技术方案，在自动割草机启动后，基于基站位置坐标，建立目标坐标系。基于关键时间戳对应的关键天线位置坐标，以及三轴加速度，对自动割草机的位姿进行初始化，以获得多传感器在目标坐标系下的位姿。根据激光雷达以及卫星定位模块各自的定位效果，获得第一融合位姿。根据第一融合位姿，以及惯性传感器的位姿，确定第二融合位姿。基于第二融合位姿，计算得到自动割草机的实时位姿。本技术可根据第一融合位姿以及第二融合位姿，确定自动割草机的实时位姿，且第一融合位姿参考了激光雷达以及卫星定位模块各自的定位效果，从而能够解决因为遮挡导致gnss定位失效引起自动割草机无法正常工作的问题，同时在无需建立高精度特征地图、工作场景切换过程中无需停机的情况下为自动割草机提供高精度定位。