割草机及其全局重定位方法与流程

本发明涉及机器人定位与导航，尤其涉及一种割草机全局重定位方法及割草机。

背景技术：

1、割草机已应用于住宅庭院、公园绿地及高尔夫球场等场景。割草机在实际运行中可能因人为搬运、车轮打滑、碰撞、长时间休眠后唤醒或传感器短时失效而丢失全局定位，上述情形通常被称为机器人绑架问题。定位丢失后，若不能可靠恢复全局位姿，机器人将无法安全地继续路径规划及割草作业。

2、针对全局重定位问题，现有方案可采用粒子滤波或基于全局点云描述子的候选检索。粒子滤波方法需要在较大的作业区域中维持大量粒子，计算量较大，且草坪环境中的重复纹理和相似局部结构容易造成粒子退化。基于scancontext等全局描述子的方案能够较快获得候选关键帧，但现有相似度度量容易受到描述子幅值变化影响；此外，割草机被搬运后航向角不确定，同一位置在不同朝向下的描述子还会发生扇区错位，导致匹配失效。

3、同时，候选检索所得位姿通常仍具有一定的平移和旋转误差。迭代最近点算法对配准初值较为敏感，正态分布变换虽然具有相对较大的收敛域，但单独使用时难以同时兼顾收敛范围和最终精度。在草坪这类弱几何特征且局部结构重复的环境中，仅依据描述子最高分候选或配准算法内部适应度，容易接受错误位置。

4、因此，需要一种能够利用离线地图数据库和当前单帧激光点云，在任意朝向下快速筛选多个候选，通过具有较大收敛域的粗配准与高精度精配准逐级求解，并利用独立几何一致性指标剔除误匹配的全局重定位方案。

技术实现思路

1、基于此，有必要针对上述技术问题，提供一种割草机全局重定位方法及割草机能够快速、准确且可靠地恢复割草机的全局位姿，同时降低错误重定位的概率。

2、第一方面，本技术提供了一种割草机全局重定位方法，其特征在于，包括以下步骤：

3、s1，获取预先构建的地图数据库以及割草机的当前扫描点云，地图数据库包括作业区域的全局点云地图、多个参考点云帧的全局描述子及对应全局位姿；

4、s2，根据当前扫描点云生成查询全局描述子，对查询全局描述子与各参考点云帧的全局描述子进行非负归一化，并使待比较的两个全局描述子沿角度维度产生多个相对循环位移，分别计算各相对循环位移下的分布重叠相似度，以最大分布重叠相似度及其对应的相对循环位移表征查询全局描述子与相应参考点云帧的匹配结果；

5、s3，根据匹配结果确定多个候选参考点云帧，并根据各候选参考点云帧的全局位姿和对应的相对循环位移确定多个候选初始位姿；

6、s4，针对各候选初始位姿，从全局点云地图中确定局部子地图，分别对当前扫描点云和局部子地图进行体素滤波降采样，并以相应候选初始位姿为初值，对降采样后的当前扫描点云与局部子地图依次进行粗配准和精配准，得到多个候选配准位姿；

7、s5，基于按照各候选配准位姿变换后的当前扫描点云与全局点云地图之间的空间邻近关系，计算独立于粗配准和精配准的内部评价值的几何一致性得分，依据几何一致性得分从多个候选配准位姿中确定目标位姿，并在目标位姿满足与割草机当前定位状态对应的接受条件时输出目标位姿。

8、在一些实施方式中，步骤s1包括：

9、s11，控制割草机遍历作业区域，融合激光雷达、里程计和惯性测量单元的数据进行同步定位与建图，获得所述全局点云地图以及各点云帧的全局位姿；

10、s12，根据相邻点云帧之间的移动距离和/或转动角度筛选参考点云帧，生成各参考点云帧的全局描述子，并将全局描述子与相应全局位姿绑定；

11、s13，对全局点云地图进行降采样，将其与参考点云帧的全局描述子及全局位姿存储为地图数据库，并在执行全局重定位时加载地图数据库。

12、在一些实施方式中，查询全局描述子和参考点云帧的全局描述子均为极坐标网格描述子；步骤s2中生成查询全局描述子包括：

13、s21，将点云中各点的水平方位角映射至[0,2π)，并剔除水平距离不在预设有效距离区间内的点；

14、s22，沿水平距离方向划分多个环区，并沿水平方位角方向划分多个扇区；

15、s23，以落入各环区与扇区所限定网格内的点的最大高度作为相应网格的编码值，生成描述子矩阵。

16、在一些实施方式中，步骤s2包括：将全局描述子的各分量进行非负化并归一化为非负分布，以巴氏系数作为分布重叠相似度；对描述子矩阵的各环分别沿扇区方向执行循环移位，各环采用相同的整数扇区偏移量t，并使t遍历0至s-1，其中s为扇区总数；以及根据取得最大巴氏系数的扇区偏移量按照2πt/s确定查询点云相对于参考点云帧的航向角补偿量。

17、在一些实施方式中，步骤s3包括：按照最大分布重叠相似度由高到低对多个参考点云帧排序，选择排序靠前的k个参考点云帧作为候选参考点云帧；和/或，选择最大分布重叠相似度满足预设相似度条件的参考点云帧作为候选参考点云帧。

18、在一些实施方式中，步骤s4中确定候选子地图包括：

19、s410，根据候选初始位姿将全局点云地图中的地图点变换至割草机的车体坐标系；

20、s420，保留径向距离处于激光雷达有效探测范围内且水平方位角处于激光雷达水平视场角范围内的地图点；对保留的地图点进行稀疏采样，获得与相应候选初始位姿对应的候选子地图。

21、在一些实施方式中，步骤s4还包括：

22、s430，采用第一体素滤波参数对当前扫描点云pscan进行体素滤波降采样，并采用第二体素滤波参数对局部子地图mk进行体素滤波降采样，得到降采样后的当前扫描点云和降采样后的局部子地图，其中，第一体素滤波参数和第二体素滤波参数相互独立；

23、s440，以第k个候选初始位姿为初值，基于降采样后的当前扫描点云和局部子地图执行正态分布变换配准，得到粗配准位姿tndt,k，并以粗配准位姿tndt,k为初值，基于当前扫描点云和局部子地图执行迭代最近点配准，得到第k个候选配准位姿ticp,k。

24、在一些实施方式中，步骤s5包括：

25、s51，基于全局点云地图预先构建并复用静态最近邻搜索数据结构；

26、s52，将当前扫描点云按照各候选配准位姿变换至地图坐标系，并查询变换后各点到全局点云地图的最近邻距离；

27、s53，将最近邻距离小于预设距离阈值的点确定为内点，以内点数量占参与验证的点的总数量的比例作为几何一致性得分。

28、在一些实施方式中，步骤s5中的接受条件包括：当割草机处于未初始化状态时，将几何一致性得分与第一阈值比较；当割草机处于已初始化状态时，将几何一致性得分与高于第一阈值的第二阈值比较；仅在几何一致性得分大于与当前定位状态对应的阈值时更新割草机的全局位姿；当割草机处于已初始化状态且连续预设帧数未获得满足第二阈值的目标位姿时，判定定位丢失并切换至重定位等待状态，针对后续扫描点云重复执行步骤s2至s5。

29、第二方面， 本技术提供了一种割草机，包括激光雷达、处理器和存储器，存储器中存储有预先构建的地图数据库及能够由处理器执行的计算机程序，地图数据库包括作业区域的全局点云地图、多个参考点云帧的全局描述子及对应全局位姿，其特征在于，处理器执行计算机程序时被配置为：获取激光雷达采集的当前扫描点云，根据当前扫描点云生成查询全局描述子；对查询全局描述子与各参考点云帧的全局描述子进行非负归一化，使待比较的两个全局描述子沿角度维度产生多个相对循环位移，并计算各相对循环位移下的分布重叠相似度，以确定多个候选初始位姿；针对各候选初始位姿，从全局点云地图中确定局部子地图，分别对当前扫描点云和局部子地图进行不同尺度的体素滤波以构建点云金字塔，并依次基于粗配准层点云进行粗配准和基于精配准层点云进行精配准，得到多个候选配准位姿；以及基于按照各候选配准位姿变换后的当前扫描点云与全局点云地图之间的空间邻近关系计算几何一致性得分，依据几何一致性得分确定目标位姿，并在目标位姿满足与割草机当前定位状态对应的接受条件时输出目标位姿。

30、上述割草机全局重定位方法及割草机通过先将非负描述子归一化为离散分布再计算巴氏系数，降低整体幅值变化对场景相似度的影响；通过遍历扇区循环移位显式补偿水平旋转，并将最佳偏移量用于候选初始航向估计，使被搬运后的任意朝向重定位成为可能；通过top-k候选保留、动态视场子地图和ndt-icp级联配准，兼顾候选召回率、配准收敛域、最终精度和嵌入式计算效率；通过基于全局地图静态最近邻结构计算内点比例，以独立几何一致性指标剔除重复草坪环境下的误匹配；通过离线数据库预构建、在线只读加载、定位状态感知阈值及失败回退机制，提高产品运行的确定性和安全性。