一种基于KD树动态避障的方法及其存储介质、电子设备与流程

本发明涉及自动控制与机器人导航，尤其涉及一种基于kd树动态避障的方法及其存储介质、电子设备。

背景技术：

1、现有的机器人主要通过代价地图的方式进行感知、规控。代价地图最核心的原理是将地图离散化为一个栅格地图，并为栅格地图中的每一个栅格分配一个代价值。栅格地图的思想是将一个工作环境按照预设的分辨率在横向和纵向上切割为很多个栅格，在同一个栅格内的障碍物占用情况被认为是均匀统一的。这样就可以用有限多个栅格表示一个有限大小的平面上的障碍物情况。机器人动态避障，即通过预判未来某个轨迹点位姿是否可能碰撞障碍物，从而达到避障的效果。传统栅格地图的动态避障方法是将预设的机器人footprint(机器人footprint为机器人足迹，又称碰撞箱，是一个由坐标点组成的多边形)转置到栅格地图中的机器人预设轨迹点位姿；依次判断机器人footprint所在的所有栅格是否有障碍物，如果任意一栅格有障碍物，则机器人碰撞障碍物。机器人动态避障精确度受栅格地图的分辨率影响，如果栅格地图的分辨率低，则栅格地图中的栅格数量少，即每个栅格面积大，则在判断碰撞障碍物时会产生较大误差(栅格内的障碍物占用情况默认是全部占用)，分辨率越低，误差越大，若提升栅格地图的分辨率，栅格地图数据量会呈指数增加，将消耗极大的存储空间，并带来相当大的计算复杂度，使得一般的cpu处理器难以应对；另外栅格地图中障碍物占用的栅格和安全区域的栅格和空间开销是一样大的，造成较多性能浪费。

2、专利号为cn 111381245 b的一种激光slam自适应分辨率栅格地图构建方法：初始自定义高中低三种分辨率的大小，分别设为rh、rm和rl，自定义参数a、b、n；采集激光雷达点云信息；检测点云信息中的直线段和弧线段；统计检测出的直线段和弧线段的总数量c1以及未被包含进直线段和弧线段的点云数量c2；先根据c1和c2的值自动设置地图分辨率r*，再根据点云距离信息进行调整，得到地图分辨率r；更新地图；上述技术方案通过激光雷达采集的数据判断当前环境障碍物的密集程度，再结合障碍物到机器人的距离信息，以此来实时地调整地图的分辨率，应用上述栅格地图可以提高机器人动态避障精确度的同时，可以相对有效地降低存储空间、节省计算资源，解决栅格地图中障碍物占用的栅格和安全区域的栅格时间和空间开销是一样大的，造成较多性能浪费的问题；但是上述栅格地图还是消耗较大的存储空间，计算复杂度也比较高。

技术实现思路

1、为此，需要提供一种基于kd树动态避障的方法，解决传统栅格地图精度低、占用储存空间高，判断机器人footprint所在的所有栅格是否有障碍物时间长的问题。

2、为实现上述目的，本发明提供了一种基于kd树动态避障的方法，其包括以下步骤：

3、将机器人获得的传感器点云处理后合并生成kd树形点云图；

4、将预设的机器人footprint转置到在kd树形点云图中机器人预设轨迹点位姿；

5、使用kd树形点云图中的半径搜索和/或k近邻搜索机器人footprint中心附近的云点，获得搜索出的点云；

6、判断搜索出的点云是否在机器人footprint内部；若在，则说明机器人碰撞障碍物；否则机器人不会碰撞障碍物。

7、进一步地，所述将机器人获得的传感器点云处理后合并生成kd树形点云图，包括以下步骤：

8、将机器人获得的传感器的多个点云进行滤波；

9、将滤波后的多个点云合并形成点云集合；

10、将点云集合导入kd树，生成树形结构点云图。

11、进一步地，所述kd树形点云图中的半径搜索的搜索中心为机器人footprint中心，搜索半径为机器人footprint中心距离机器人footprint边缘的最远距离。

12、进一步地，在获得搜索出的点云，判断搜索出的点云是否在机器人footprint内部前，还包括以下步骤：

13、统计搜索出的点云的个数；

14、判断搜索出的点云的个数是否等于0，若等于0，则说明机器人不会碰撞障碍物。

15、进一步地，在判断搜索出的点云的个数不等于0时，

16、还包括从搜索出的点云中获得距离机器人中心距离接近的点云，判断距离机器人中心距离接近的点云和机器人中心距离是否小于或等于机器人footprint边缘距离机器人footprint中心的最近距离，若小于或等于则说明机器人碰撞障碍物。

17、进一步地，所述判断搜索出的点云是否在机器人footprint内部时，使用pnpoly算法判定搜索出的点云是否在机器人footprint内部，包括以下步骤：

18、从判断搜索出的点云中获得机器人footprint边缘距离机器人footprint中心的最近距离至机器人footprint边缘距离机器人footprint中心的最近距离的点云；

19、将获得的点云引出一条射线，统计这条射线与机器人footprint的交点个数，

20、若交点个数为奇数，则说明点云在机器人footprint内部；否则说明点云在机器人footprint外部。

21、进一步地，在获得搜索出的点云，判断搜索出的点云是否在机器人footprint内部前，还包括以下步骤：

22、从搜索出的点云中获得距离机器人中心距离接近的点云，判断距离机器人中心距离接近的点云和机器人中心距离是否小于或等于机器人footprint边缘距离机器人footprint中心的最近距离，若小于或等于则说明机器人碰撞障碍物。

23、进一步地，所述判断搜索出的点云是否在机器人footprint内部时，使用pnpoly算法判定搜索出的点云是否在机器人footprint内部，包括以下步骤：

24、搜索出的点云引出一条射线，统计这条射线与机器人footprint的交点个数，

25、若交点个数为奇数，则说明点云在机器人footprint内部；否则说明点云在机器人footprint外部。

26、存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述基于kd树动态避障的方法的步骤。

27、电子设备，其包括存储介质、处理器，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述基于kd树动态避障的方法的步骤。

28、区别于现有技术，上述技术方案通过机器人获得的传感器点云生成kd树形点云图，相比生成栅格地图的时间，生成kd树形点云图的时间大大缩短，优化了空间开销；且kd树形点云图的精度为float类型的精度，大约为6-7位有效数字，相较传统栅格地图精度提升了3-4个数量级；将预设的机器人footprint转置到在kd树形点云图中机器人当前位姿或预设轨迹点位姿，可以通过kd树形点云图搜索机器人footprint的附近点云，获得搜索出的点云，通过判断搜索出的点云是否在机器人footprint内，判断机器人是否会碰撞障碍物；相比传统栅格地图，搜索机器人footprint的附近点云的时间也明显缩短；可以帮助机器人实现更精准的动态避障，大大提高了机器人的通过性和安全性。