大场景3D地图动态加载的导航定位方法、设备及存储介质与流程

本发明涉及定位，具体涉及一种大场景3d地图动态加载的导航定位方法、设备及存储介质。

背景技术：

1、随着slam(simultaneous localization and mapping，即时定位与地图构建)技术的发展，机器人可以通过携带的传感器，如激光雷达、高清相机、imu等在未知环境中完成自身定位和地图构建，基于生成的场景地图，实现导航功能，机器人自主导航能力被广泛应用于在智能巡检领域，替代人工完成较为复杂高危的巡检作业。

2、对于大场景环境下使用slam技术建立的地图文件往往较大，加载该地图实现定位导航会不仅会耗时严重，很难满足实时性要求，而且还会大量占用系统内存，对工控机性能要求较高，因此需要对地图文件进行处理。实际应用中，为了给机器人提供位置和环境信息实现高效的大场景地图加载，目前常见的优化方法主要有：

3、1、激光点云地图压缩：采用提取语义特征并构建语义地图等压缩技术减少地图数据的存储空间，提高加载速度，但是需要大量数据用于训练对应的网络模型，否则预先定义与训练的语义信息可能不会出现，会导致地图特征稀疏，该类方法泛化性能较差；

4、2、缓存技术：将已经加载的地图数据存储到缓存中，以减少重复加载的时间和资源消耗，但是缓存方案没法控制加载地图的延迟，影响导航定位的实时性能。

技术实现思路

1、本发明提出的一种大场景3d地图动态加载的导航定位方法、设备及存储介质，可至少解决背景技术中的技术问题之一。

2、为实现上述目的，本发明采用了以下技术方案：

3、一种大场景3d地图动态加载的导航定位方法，通过计算机设备执行以下步骤，

4、包括以下步骤，

5、构建3d云地图；

6、对可行区域和不可行区域进行识别；

7、根据导航点分割地图；

8、根据机器人实时位姿预加载、加载和释放子地图；

9、重定位并更新地图继续执行任务。

10、进一步地，包括以下步骤，

11、步骤一、构建现场环境的3d点云地图；

12、步骤二、对3d点云地图进行预处理；

13、步骤三、将处理后的3d点云地图转成2d栅格地图；

14、步骤四、在2d栅格地图中添加n个导航点位g1,g2,…,gn，设置阈值threshold，对分割的子地图划分缓冲区域、加载地图区域和直接导航区域，用于预加载、加载地图判断和直接导航，以激光雷达扫描范围r和阈值threshold和的两倍为地图边长map_size，以添加的导航点位为地图中心将整个地图分块并记录每个导航点位对应的子地图编号map1,map2,…,mapn；

15、步骤五、根据导航点位规划路径，通过路径规划算法获取每个导航点位的导航顺序，根据导航点位对应的子地图的地图编号，确定地图加载的顺序；

16、步骤六、重新加载pcd点云地图，并初始化体素地图；

17、步骤七、扫描场景环境生成的地图，对可行区域和不可行区域进行识别；

18、步骤八、计算每个体素栅格最近的障碍物的距离；

19、步骤九、基于先验地图的实时定位采用蒙特卡洛粒子滤波算法的定位策略，蒙特卡洛粒子滤波算法的定位基于给定的先验地图信息与感知状态量，根据状态量采样一组具有权重的随机粒子来表示位姿估计的后验概率，并根据这些粒子与权值来计算修正当前状态量；

20、粒子滤波定位算法采用波束模型和似然域模型结合的方式，通过两种模型算法对实时点云数据帧进行粒子滤波，得到各自的权重和位姿，然后根据各自的权重进行加权计算，进行重采样，得到最终的位姿；

21、步骤十、刚开始获取初始位姿initial_pose，判断机器人初始位置所属的子地图编号mapi，加载包括mapi在内的子地图mapi、mapi+2、mapi+3；

22、步骤十一、开始导航，用粒子滤波算法进行定位，粒子权重计算部分选择加载点云地图以及距离体素地图，直接查找文件中存储的最近障碍物距离值的方式，指定路径下voxelmap_dis.txt文件已存在则直接加载打开用于权重计算，没有则创建生成voxelmap_dis.txt文件；

23、步骤十二、子地图mapi+3加载完且mapi释放结束，用整个地图较大分辨率生成的voxelmap_dis0.txt文件进行重定位，获取机器人当前位姿，并以机器人此时所在子地图mapi+1对应的voxelmap_disi+1.txt文件计算粒子权重，用粒子滤波定位导航；

24、步骤十三、更新子地图mapi、mapi+1和mapi+2，重复步骤十一到步骤十二，继续执行任务，直至任务结束。

25、又一方面，本发明还公开一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如上述方法的步骤。

26、再一方面，本发明还公开一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如上方法的步骤。

27、由上述技术方案可知，本发明的大场景3d地图动态加载的导航定位方法与现有技术相比较，有益效果在于：

28、1、本发明在动态加载大场景环境地图时，采用动态分块加载的方式，通过设置缓冲区域预加载子地图，降低加载地图的延迟时间，实现实时加载的目的，达到实际应用需求，通过验证本发明可以适用于面积约为100万平方米的大场景环境。

29、2、本发明考虑点云地图的分布特性，通过识别可行区域和不可行区域对点云地图进行处理，去除无效点云的空间占用，进一步降低点云地图的复杂性，减小地图文件的大小。

30、3、本发明在使用蒙特卡洛粒子滤波定位时，将距离每个体素栅格的最近障碍物的距离值算出并记录在txt文本文件中，便于后续粒子滤波定位计算权重时直接查找，减小粒子权重计算的时间，提升定位效率。

技术特征：

1.一种大场景3d地图动态加载的导航定位方法，其特征在于，包括以下步骤，

2.根据权利要求1所述的大场景3d地图动态加载的导航定位方法，其特征在于：包括以下步骤，

3.根据权利要求2所述的大场景3d地图动态加载的导航定位方法，其特征在于：步骤三中将处理后的3d点云地图转成2d栅格地图，包括将接收到的点云信息以“/map”话题的形式发布，用map_server来接收“/map”话题，生成2d栅格地图，保存用于导航的map.pgm和map.yaml文件，其中2d栅格地图的具体信息都存在这个pgm格式的文件中，yaml文件中存储了pgm文件的路径、分辨率、机器人初始位置这些信息，用于确定机器人的位置。

4.根据权利要求2所述的大场景3d地图动态加载的导航定位方法，其特征在于：

5.根据权利要求2所述的大场景3d地图动态加载的导航定位方法，其特征在于：步骤七包括在体素地图初始化完成后做可行区域和不可行区域的识别，已知导航点位顺序的情况下，根据规划的路径，得到该路径占用的每个体素栅格，连接相邻两个导航点gi和gi+1，其中i＝1,2,…,n；以路径规划相邻两个导航点连线占用的体素栅格中心为垂足做垂直于该连线的垂线，标记该垂线相对于垂足两边接触到的第一个已占用的栅格序号，标记的所有占用栅格即为可行区域与不可行区域的边界；将pcd点云地图做可行区域和不可行区域处理后，仅保留边界的栅格占用，更新整个环境的pcd点云地图。

6.根据权利要求2所述的大场景3d地图动态加载的导航定位方法，其特征在于：步骤八中采用八叉树的近邻搜索方法，将搜索到的距离当前体素栅格最近的障碍物的距离值存储在数组中，创建voxelmap_dis.txt文本文件将该距离值存储到数组的同时写入文本文件中的方案，该方案在后续粒子滤波定位权重计算时通过文本文件直接查找距离值，将整张地图用较大分辨率生成用于重定位的voxelmap_dis0.txt文件，同时将每个导航点对应分割的子地图用较小的分辨率生成用于导航过程中粒子滤波定位的voxelmap_disi.txt(i＝1,2,…,n)文件。

7.根据权利要求2所述的大场景3d地图动态加载的导航定位方法，其特征在于：所述步骤十一也可以选择仅加载点云地图，实时通过八叉树近邻搜索方法获取距离粒子当前所在栅格内最近障碍物的距离值，省去前期文件读写和加载操作；

8.根据权利要求2所述的大场景3d地图动态加载的导航定位方法，其特征在于：步骤一、构建现场环境的3d点云地图，根据机器人所携带的传感器，控制机器人运动，选用相关的slam算法，包括faster-lio、orb-slam3、r3live这些生成3d点云地图。

9.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如权利要求1至8中任一项所述方法的步骤。

10.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至8中任一项所述方法的步骤。

技术总结
本发明的一种大场景3D地图动态加载的导航定位方法、设备及存储介质，包括以下步骤，构建3D云地图；对可行区域和不可行区域进行识别；根据导航点分割地图；根据机器人实时位姿预加载、加载和释放子地图；重定位并更新地图继续执行任务。本发明在动态加载大场景环境地图时，采用动态分块加载的方式，通过设置缓冲区域预加载子地图，降低加载地图的延迟时间，实现实时加载的目的，达到实际应用需求，通过验证本发明可以适用于面积约为100万平方米的大场景环境。

技术研发人员：夏磊,褚衍超,章海兵,汪中原,刘闹
受保护的技术使用者：合肥科大智能机器人技术有限公司
技术研发日：
技术公布日：2024/3/17