二维栅格地图实时更新显示方法、电子设备、存储介质与流程

1.本发明属于地图技术领域，具体涉及一种二维栅格地图实时更新显示方法及装置。


背景技术：

2.二维栅格地图作为移动机器人学中导航地图的主要表示方法之一，通过将环境大小划分成一系列的平面栅格，其中每一栅格给定一个可能值，表示该栅格被障碍物占据的概率，从而构建当前环境的栅格地图，用于指导机器人行走。一般根据机器人获取关键帧的实时点云图对栅格地图进行实时更新，所以要得到全面而准确的二维栅格地图，前提是首先获取准确的每帧点云帧的位姿，整套技术过程也称slam(同步定位与建图)。
3.针对超大场景生成超大栅格地图，存在两个明显的工程问题：
4.(1)实时更新问题，由于slam在未发生闭环修正(检测是否来过该位置)前总是存在累积误差，会导致之前生成的二维栅格地图存在误差。当发生闭环修正整个位姿后，栅格地图需要重新生成，当地图变得很多大，比如公园场景(2公里x2公里)，需要实时更新二维栅格地图就变得困难；
5.(2)保存大栅格占据概率地图占用的内存过大问题，按每个栅格代表0.05m，则表示2公里*2公里的场景，地图尺寸最少需要40000*40000个float类型栅格，需要消耗5.96g内存。
6.现有的室外巡逻机器人通常工作在园区、街区、广场、公园等大场景。建图过程中实时生成显示栅格地图，有利于扫图人员生成一张准确的栅格图。目前方法在面对超大栅格地图占内存大，在闭环修正位姿后实时刷新栅格地图问题分别采用：加大工控机的内存大小；加大工控机运算能力或直接离线生成栅格图。上述方法提高了机器人生产成本并降低了用户体验，并非一种优的选择。


技术实现要素：

7.本技术的一个目的是提供一种二维栅格地图实时更新显示方法的新技术方案，能够有利于二维栅格地图的实时更新显示。
8.本发明的第一方面，提供了一种二维栅格地图实时更新显示方法，包括以下步骤：s1、定义变量，所述变量包括动态矩阵变量、查找二叉树类型变量、数组类型变量和类型变量，其中，定义所述动态矩阵变量表示二维栅格地图，并将所述矩阵变量的矩阵元素初始化，代表没有被遍历；定义所述查找二叉树类型变量，以用于存储建图过程中的栅格概率子图；定义所述数组类型变量以保存所有帧的激光点云和对应的激光位姿；定义所述类型变量表示子图序列号，并初始化；s2、判断是否发生了闭环修正；s3、如果未发生闭环修正，则生产栅格图；s4、如果发生了闭环修正，则进行闭环修正处理。
9.根据本发明的一个实施例，步骤s1中，所述动态矩阵变量为eigen::matrix《uint8》动态矩阵变量，所述二叉树类型变量为std::map查找二叉树类型变量，所述数组类型变量
为std::vector数组类型变量，所述类型变量为uint8类型变量。
10.根据本发明的一个实施例，步骤s1中，将所述矩阵变量的矩阵元素全部初始化为127，定义所述类型变量表示子图序列号，并初始化为0。
11.根据本发明的一个实施例，所述std::vector数组类型变量的数量为两个，两个所述std::vector数组类型变量中的一个用于保存所有帧的激光点云，两个所述std::vector数组类型变量中的另一个用于保存所述激光点云对应的所述激光位姿。
12.根据本发明的一个实施例，步骤s2中，查询全局回环修正标志位是否置位。
13.根据本发明的一个实施例，步骤s3包括：s31、通过回调函数接收连续预设帧所述激光点云的数据和对应的所述激光位姿，通过所述点云数据和所述激光位姿计算生成一个栅格概率子图，并且填充对应的所述栅格地图上的占据概率：s32、在计算完成后，将所述栅格概率子图和所述子图序列号同时保存在所述查找二叉树类型变量中；s33、将所述激光点云的数据和对应的所述激光位姿分别保存在所述数组类型变量中；s34、将所述子图序列号加1。
14.根据本发明的一个实施例，步骤s31中的所述预设帧为10帧。
15.根据本发明的一个实施例，步骤s4包括：s41、遍历对比闭环修正前的所述激光位姿与闭环修正后的所述激光位姿，找到已经被改变了的激光点云位姿帧，并找到所述激光点云位姿帧对应的栅格概率子图n；s42、将栅格概率子图n经对应的激光点云帧和对应闭环修正后位姿重新生成栅格概率子图n’；s43、将所述栅格地图先减去栅格概率子图n的栅格概率数据，再加上栅格概率子图n’的栅格概率数据，完成所述栅格地图的更新；s44、通过闭环修正后的所述激光位姿替换闭环修正前的所述激光位姿，激光位姿保存在对应的所述数组类型变量中；通过闭环修正后的栅格概率子图替换闭环修正前的栅格概率子图，栅格概率子图保存在对应的所述数组类型变量中。
16.本发明的第二方面，还提供了一种电子设备，包括：处理器和存储器，在所述存储器中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器执行上述任一项实施例所述的方法。
17.本发明的第三方面，还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行上述任一项实施例所述的方法。
18.根据本发明实施例的方法，能够在不提高系统硬件成本下能做到更加流畅地实时刷新超大栅格地图的目的。
19.通过以下参照附图对本技术的示例性实施例的详细描述，本技术的其它特征及其优点将会变得清楚。
附图说明
20.被结合在说明书中并构成说明书的一部分的附图示出了本技术的实施例，并且连同其说明一起用于解释本技术的原理。
21.图1是本发明的一实施例的二维栅格地图实时更新显示方法的流程图；
22.图2是本发明的又一实施例的二维栅格地图实时更新显示方法的流程图；
23.图3是本发明实施例的电子设备的工作原理图。
24.附图标记：
25.电子设备100；
26.存储器110；操作系统111；应用程序112；
27.处理器120；网络接口130；输入设备140；硬盘150；显示设备160。
具体实施方式
28.现在将参照附图来详细描述本技术的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本技术的范围。
29.以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本技术及其应用或使用的任何限制。
30.对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。
31.在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。
32.应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
33.下面首先结合附图具体描述根据本发明实施例的二维栅格地图实时更新显示方法。
34.如图1和图2所示，根据本发明实施例的二维栅格地图实时更新显示方法，包括以下步骤：
35.s1、定义变量，变量包括动态矩阵变量、查找二叉树类型变量、数组类型变量和整型类型变量。也就是说，步骤s1为变量定义以及初始化的步骤。
36.其中，一方面，定义动态矩阵变量表示二维栅格地图，并将矩阵变量的矩阵元素初始化，代表没有被遍历，栅格占据情况未知。可选地，动态矩阵变量为eigen::matrix《uint8》类型动态矩阵变量。并且，可以将矩阵变量的矩阵元素全部初始化为127。
37.换言之，定义eigen::matrix《uint8》动态矩阵变量(occ_map_)表示二维栅格地图，并将矩阵元素全部初始化为127，代表没有被遍历，栅格占据情况未知。
38.又一方面，定义一个查找二叉树类型变量，以用于表示建图过程中的栅格占据概率子图，可选地，栅格占据概率子图变量为std::map查找二叉树类型变量。也就是说，定义std::map查找二叉树类型变量(occ_submap_)存储建图过程中所有的栅格概率子图。
39.再一方面，定义两个数组类型变量以保存所有帧的激光点云和对应的激光位姿。可选地，数组类型变量为std::vector数组类型变量。
40.根据本发明的一个实施例，std::vector数组类型变量的数量为两个，两个std::vector数组类型变量中的一个用于保存所有帧的激光点云，两个std::vector数组类型变量中的另一个用于保存激光点云对应的激光位姿。也就是说，分别定义两个std::vector数组类型变量(lidar_scan_,lidar_pose_)来保存所有帧的激光点云和对应的激光位姿。
41.再一方面，定义整型类型变量表示子图序列号，并初始化。可选地，整型变量为uint8类型变量。其中，可以定义整型变量表示子图序列号，并初始化为0。
42.s2、判断是否发生了闭环修正。
43.其中，步骤s2中，查询全局回环修正标志位是否置位。换言之，步骤s2为判断闭环修正是否发生的步骤，查询全局回环修正标志位是否置位，如果回环修正发生则跳转到步骤s4，否则进入步骤s3。
44.s3、如果未发生闭环修正，则正常生产栅格图。
45.在本发明的一些具体实施方式中，步骤s3包括：
46.s31、通过回调函数接收连续预设帧激光点云的数据和对应的激光位姿，通过点云数据和激光位姿计算生成一个栅格概率子图，并且填充对应的栅格地图上的占据概率。
47.可选地，步骤s31中的预设帧为10帧。
48.s32、在计算完成后，将栅格概率子图和子图序列号同时保存在查找二叉树类型变量中。
49.s33、将激光点云的数据和对应的激光位姿分别保存在数组类型变量中。
50.s34、将子图序列号加1。
51.也就是说，步骤s3为生产栅格图的步骤，具体地，通过回调函数接收连续10帧激光点云的数据和对应的激光位姿，通过点云数据和激光位姿计算生成一个栅格概率子图，并且填充对应栅格地图(occ_map_)上占据概率，计算完成后，可以执行以下操作：
52.a)将栅格概率子图和submap_index_子图序列号同时保存在occ_submap_中；
53.b)将激光点云的数据和对应的激光位姿分别保存在lidar_scan,lidar_pose中；
54.c)将子图序列号submap_index_加1。
55.s4、如果发生了闭环修正，则利用保存的栅格子图快速更新整个栅格图。
56.其中，步骤s4可以包括：
57.s41、遍历对比闭环修正前的激光位姿与闭环修正后的激光位姿，找到已经被改变了的激光点云位姿帧，并找到激光点云位姿帧对应的栅格概率子图n。
58.s42、将栅格概率子图n经对应的激光点云帧和对应闭环修正后位姿重新生成栅格概率子图n’。
59.s43、将栅格地图先减去栅格概率子图n的栅格概率数据，再加上栅格概率子图n’的栅格概率数据，完成栅格地图的更新。
60.s44、通过闭环修正后的激光位姿替换闭环修正前的激光位姿，激光位姿保存在对应的数组类型变量中；通过闭环修正后的栅格概率子图替换闭环修正前的栅格概率子图，栅格概率子图保存在对应的数组类型变量中。
61.换言之，步骤s4为闭环修正处理的步骤，具体可以包括如下步骤：
62.a)遍历对比闭环修正前激光位姿(lidar_pose_)与闭环修正后激光位姿，找到已经被改变了的激光点云位姿帧，并找到该帧所对应的栅格概率子图n；
63.b)将栅格概率子图n，经对应的激光点云帧(lidar_scan_)和对应闭环修正后位姿重新生成栅格概率子图n
‘
；
64.c)将栅格地图(occ_map_)先减去栅格概率子图n的栅格概率数据，再加上栅格概率子图n’的栅格概率数据，完成栅格地图(occ_map_)的更新。其中栅格概率子图n是闭环修正前的子图，栅格概率子图n
‘
是闭环修正后重新计算的子图。
65.d)更新变量：把闭环修正后的激光位姿替换闭环修正前激光位姿，激光位姿保存
在变量(lidar_pose_)中；把闭环修正后的栅格概率子图替换闭环修正前的栅格概率子图，栅格概率子图保存在变量(occ_submap_)。
66.总而言之，根据本发明实施例的方法，采用树子图结构，树子图指由连续k(k＝10)帧激光点云计算的栅格概率值保存为一个子图形式(子图也用树结构来保存，其中子图树节点索引是栅格地图中行列号(row,col),子图树节点保存的是该栅格的占据概率)，建图过程中产生的所有子图用二叉查找树保存，其中树节点索引序号用子图号表示(i＝1.2.3....)表示，树节点内容存生成的子图。
67.这样处理带来好处是把栅格地图备份成子图的形式，在面对建图过程闭环发生时，只更新被改变的子图，其它子图不变，由之前整个栅格图需要重新计算到只需要计算对应少量子图，随着栅格地图越来越大，闭环引起的子图变化个数也将保证在一定数量里面。也就是闭环更新栅格图计算量由o(n)变成o(n),前者n是全部子图数量，后者n是闭环引起需要更新的子图数量，基本在一个定值上波动，解决建图过程中发生闭环修正更新地图能实时。
68.通过将二维栅格地图中栅格占据概率值用uint8类型替换float类型，将原占内存降低到四分之一的内存大小；由于占据栅格图每个栅格的占据概率大小是浮点型，值域[0，1]表示，可知float32的值域占用4个字节，表示范围在[-3.4e383.4e38]，可以看到用float32类型来表达概率值，造成很大的浪费。又由于uint8类型表示范围在[0255]，那么其实可以考虑用整数来表达概率值：
[0069]
n＝ρ*256
ꢀꢀꢀ
(1)
[0070]
ρ＝n*1/256
ꢀꢀꢀ
(2)
[0071]
其中，式(1)、(2)中，ρ为概率值，n为保存的整数值。
[0072]
现有技术中，针对超大栅格地图实时更新显示问题的目前解决方法。缺点包括加大工控机的远算能力及内存大小，虽然能一定程度解决问题，但是提高了机器人的bom成本；离线建图从根本上避免了超大栅格地图实时更新的问题，但是用户交互变差，难出一张全面准确的栅格地图，比如难保证某一区域是否被完整扫过？回环修正够不够？
[0073]
而根据本发明实施例的方法，一方面，通过用uint8类型代替float32类型保存每个栅格占据概率，将把二维栅格地图占用内存大小降低到四分之一；又一方面，将每连续10帧点云生成子图，所有的子图为方便查找采用二叉树结构类型保存，在面对建图过程闭环发生时，只更新被改变的子图，其它子图不变，大大提升栅格地图更新实时性。
[0074]
另外，本发明实施例还提供了一种计算机存储介质，所述计算机存储介质包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现上述任一所述的方法。
[0075]
也就是说，所述计算机存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行上述任一所述的方法。
[0076]
如图3所示，本发明实施例提供了一种电子设备100，包括存储器110和处理器120，所述存储器110用于存储一条或多条计算机指令，所述处理器120用于调用并执行所述一条或多条计算机指令，从而实现上述任一所述的方法。
[0077]
也就是说，电子设备100包括：处理器120和存储器110，在所述存储器110中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器120执行上述任一所述的方法。
[0078]
进一步地，如图3所示，电子设备100还包括网络接口130、输入设备140、硬盘150、和显示设备160。
[0079]
上述各个接口和设备之间可以通过总线架构互连。总线架构可以是可以包括任意数量的互联的总线和桥。具体由处理器120代表的一个或者多个中央处理器(cpu)，以及由存储器110代表的一个或者多个存储器的各种电路连接在一起。总线架构还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其它电路连接在一起。可以理解，总线架构用于实现这些组件之间的连接通信。总线架构除包括数据总线之外，还包括电源总线、控制总线和状态信号总线，这些都是本领域所公知的，因此本文不再对其进行详细描述。
[0080]
所述网络接口130，可以连接至网络(如因特网、局域网等)，从网络中获取相关数据，并可以保存在硬盘150中。
[0081]
所述输入设备140，可以接收操作人员输入的各种指令，并发送给处理器120以供执行。所述输入设备140可以包括键盘或者点击设备(例如，鼠标，轨迹球(trackball)、触感板或者触摸屏等。
[0082]
所述显示设备160，可以将处理器120执行指令获得的结果进行显示。
[0083]
所述存储器310，用于存储操作系统运行所必须的程序和数据，以及处理器120计算过程中的中间结果等数据。
[0084]
可以理解，本发明实施例中的存储器110可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(rom)、可编程只读存储器(prom)、可擦除可编程只读存储器(eprom)、电可擦除可编程只读存储器(eeprom)或闪存。易失性存储器可以是随机存取存储器(ram)，其用作外部高速缓存。本文描述的装置和方法的存储器110旨在包括但不限于这些和任意其它适合类型的存储器。
[0085]
在一些实施方式中，存储器110存储了如下的元素，可执行模块或者数据结构，或者他们的子集，或者他们的扩展集：操作系统111和应用程序112。
[0086]
其中，操作系统111，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序112，包含各种应用程序，例如浏览器(browser)等，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序112中。
[0087]
上述处理器120，当调用并执行所述存储器110中所存储的应用程序和数据，具体的，可以是应用程序112中存储的程序或指令时，将第一集合和第二集合中的一者分散发送至所述第一集合和第二集合中的另一者所分布的节点，其中，所述另一者分散存储于至少两个节点；并根据所述第一集合的节点分布和所述第二集合的节点分布，分节点地进行交集处理。
[0088]
本发明上述实施例揭示的方法可以应用于处理器120中，或者由处理器120实现。处理器120可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器120中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器120可以是通用处理器、数字信号处理器(dsp)、专用集成电路(asic)、现成可编程门阵列(fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件，可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直
接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器110，处理器120读取存储器110中的信息，结合其硬件完成上述方法的步骤。
[0089]
可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(asic)、数字信号处理器dsp)、数字信号处理设备(dspd)、可编程逻辑设备(pld)、现场可编程门阵列(fpga)、通用处理器、控制器、微控制器、微处理器、用于执行本技术所述功能的其它电子单元或其组合中。
[0090]
对于软件实现，可通过执行本文所述功能的模块(例如过程、函数等)来实现本文所述的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。
[0091]
具体地，处理器120还用于读取所述计算机程序，执行上述任一所述的方法。
[0092]
在本技术所提供的几个实施例中，应该理解到，所揭露方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
[0093]
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理包括，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
[0094]
上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述收发方法的部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(read-only memory，简称rom)、随机存取存储器(random access memory，简称ram)、磁碟或者光盘等各种可以存储程序代码的介质。
[0095]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。