CN116954203A - 栅格地图构建方法、装置、设备及存储介质 - Google Patents

Abstract

本申请公开了一种栅格地图构建方法、装置、设备及存储介质。本申请提供的技术方案，包括：获取激光雷达采集的激光数据和激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和传感数据对应的第二采集位姿；根据激光数据和对应的第一采集位姿生成栅格地图，并在生成栅格地图的过程中更新第一采集位姿；基于更新后的第一采集位姿对第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至栅格地图中。通过上述技术手段，解决了现有技术中激光雷达视野盲区的障碍物无法精准记录在栅格地图中的问题，提高移动机器人的导航规划效率。

Description

栅格地图构建方法、装置、设备及存储介质

技术领域

本申请涉及移动机器人技术领域，尤其涉及一种栅格地图构建方法、装置、设备及存储介质。

背景技术

栅格地图是移动机器人进行避障移动时的重要依据之一，栅格地图记录有障碍物的位置，移动机器人可根据自身位于栅格地图中的位置以及栅格地图中障碍物的位置进行路径规划，以在没有障碍物的区域进行移动。目前构建栅格地图时，通过2D激光雷达进行障碍物扫描，根据2D激光雷达扫描到的激光数据生成对应的栅格地图。

针对目前的栅格地图构建方法，发明人发现2D激光雷达存在视野盲区，2D激光雷达不能将位于视野盲区的障碍物添加到栅格地图中。这使得移动机器人进行路径规划时，花费较多时间在障碍物检测和避障上，导致移动机器人的导航规划效率较低，进而影响移动机器人的工作效率。

发明内容

本申请提供一种栅格地图构建方法、装置、设备及存储介质，解决了现有技术中激光雷达视野盲区的障碍物无法精准记录在栅格地图中的问题，提高移动机器人的导航规划效率，保证移动机器人的工作效率。

第一方面，本申请提供了一种栅格地图构建方法，包括：

获取激光雷达采集的激光数据和所述激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和所述传感数据对应的第二采集位姿；

根据所述激光数据和对应的第一采集位姿生成栅格地图，并在生成所述栅格地图的过程中更新所述第一采集位姿；

基于更新后的第一采集位姿对所述第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至所述栅格地图中。

第二方面，本申请提供了一种栅格地图构建装置，包括：

位姿获取模块，被配置为获取激光雷达采集的激光数据和所述激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和所述传感数据对应的第二采集位姿；

第一构建模块，被配置为根据所述激光数据和对应的第一采集位姿生成栅格地图，并在生成所述栅格地图的过程中更新所述第一采集位姿；

第二构建模块，被配置为基于更新后的第一采集位姿对所述第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至所述栅格地图中。

第三方面，本申请提供了一种栅格地图构建设备，包括：

一个或多个处理器；存储装置，存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的栅格地图构建方法。

第四方面，本申请提供了一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行如第一方面所述的栅格地图构建方法。

本申请在根据激光数据构建栅格地图时根据惯性测量单元和/或里程计采集的运动数据确定激光数据的采集位姿，根据激光数据和对应的采集位姿构生成栅格地图，并基于栅格地图和激光数据优化采集位姿，减小惯性测量单元和/或里程计带来误差，根据优化的采集位姿更新栅格地图，以构建精度更高的栅格地图。根据优化后的激光数据的采集位姿对传感数据的采集位姿进行更新，以精准确定传感数据位于栅格地图中的位置。将传感数据添加至栅格地图中以生成融合有激光数据和传感数据的栅格地图，异构传感器采集的传感数据可以反馈移动机器人周围的低矮障碍物和负向障碍物的分布情况，使得栅格地图不仅记录有激光雷达采集到的障碍物，还记录有异构传感器采集到的低矮障碍物和负向障碍物，填补了激光雷达的采集盲区，提高了栅格地图的精度和准确度。当移动机器人基于高精度的栅格地图进行导航规划时，可根据栅格地图记录的障碍物区域精准快速规划处避障路线，提高导航路径的规格效率，保证移动机器人的工作效率。

附图说明

图1是本申请实施例提供的一种栅格地图构建方法的流程图；

图2是本申请实施例提供的确定数据的采集位姿的流程图；

图3是本申请实施例提供的移动机器人按照移动路线进行移动的示意图；

图4是本申请实施例提供的连续多帧激光数据的第一示意图；

图5是本申请实施例提供的连续多帧激光数据的第二示意图；

图6是本申请实施例提供的更新栅格地图的流程图；

图7是本申请实施例提供的解算第一优化模型的流程图；

图8是本申请实施例提供的移动路线的示意图；

图9是本申请实施例提供的解算第二优化模型的流程图；

图10是本申请实施例提供的将传感数据添加至栅格地图的流程图；

图11是本申请实施例提供的一种栅格地图构建装置的结构示意图；

图12是本申请实施例提供的一种栅格地图构建设备的结构示意图。

具体实施方式

为了使本申请的目的、技术方案和优点更加清楚，下面结合附图对本申请具体实施例作进一步的详细描述。可以理解的是，此处所描述的具体实施例仅仅用于解释本申请，而非对本申请的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本申请相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。

本申请的说明书和权利要求书中的术语“第一”、“第二”等是用于区别类似的对象，而不用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施，且“第一”、“第二”等所区分的对象通常为一类，并不限定对象的个数，例如第一对象可以是一个，也可以是多个。此外，说明书以及权利要求中“和/或”表示所连接对象的至少其中之一，字符“/”，一般表示前后关联对象是一种“或”的关系。

本实施例中提供的栅格地图构建方法可以由栅格地图构建设备执行，该栅格地图构建设备可以通过软件和/或硬件的方式实现，该栅格地图构建设备可以是两个或多个物理实体构成，也可以是一个物理实体构成。例如栅格地图构建设备可以是移动机器人本身，可以是移动机器人的处理器。

栅格地图构建设备安装有嵌入式系统，栅格地图构建设备可以基于嵌入式系统安装有可以执行栅格地图构建方法的应用程序，因此，栅格地图构建设备也可以是应用程序本身。

为便于理解，本实施例以移动机器人为执行栅格地图构建方法的主体为例，进行描述。

在一实施例中，移动机器人通过2D激光雷达采集激光数据，以及通过惯性测量单元或里程计采集移动机器人的运动数据。根据运动数据确定移动机器人采集激光数据时的位置和姿态，并根据移动机器人采集激光数据时的位置和姿态将激光数据添加至栅格地图中以构建出栅格地图。但是2D激光雷达只能扫描到和2D激光雷达等高的障碍物，造成2D激光雷达存在视觉盲区，如低于2D激光雷达安装高度的低矮障碍物和负向障碍物无法被2D激光雷达扫描到。相应的，视觉盲区的障碍物无法添加至栅格地图，因此移动机器人进行路径规划时，需要花费较多时间在障碍物检测和避障上，导致移动机器人的导航规划效率较低，进而影响移动机器人的工作效率。

为解决上述问题，本申请实施例提供了一种栅格地图构建方法，以填补2D激光雷达的视觉盲区内障碍物至栅格地图中。

图1给出了本申请实施例提供的一种栅格地图构建方法的流程图。参考图1，该栅格地图构建方法具体包括：

S110、获取激光雷达采集的激光数据和激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和传感数据对应的第二采集位姿。

其中，第一采集位姿是指2D激光雷达采集激光数据时移动机器人的位置和姿态，第二采集位姿是指异构传感器采集传感数据时移动机器人的位置和姿态。异构传感器是指移动机器人安装的除2D激光雷达以外的感知传感器，包括深度相机、红外传感器和超声波传感器中的至少一个，传感数据可以是深度图像，红外数据和超声波数据中的至少一个。移动机器人还安装有惯性测量单元(IMU)和/或里程计，通过惯性测量单元采集移动机器人的加速度数据，通过里程计采集移动机器人的速度数据。可通过惯性测量单元的加速度数据和速度数据，根据移动机器人的初始位姿确定出移动机器人在移动过程中的各个时间节点的位置和姿态。本实施例根据移动机器人激光雷达采集激光数据和异构传感器采集传感数据的时间节点，基于加速度数据和速度数据确定出移动机器人采集激光数据和传感数据时的位置和姿态。

在一实施例中，如果移动机器人控制2D激光雷达和异构传感器同步采集激光数据和传感数据，则激光数据和传感数据的采集位姿相同，可只确定其中一种数据的采集位姿，另一数据的采集位姿也可得知。

在另一实施例中，如果移动机器人控制2D激光雷达和异构传感器按照各自的频率采集激光数据和传感数据，则需要分别确定出各种数据的采集位姿。在该实施例中，图2是本申请实施例提供的确定数据的采集位姿的流程图。如图2所示，该确定数据的采集位姿的步骤具体包括S1101-S1103：

S1101、通过激光雷达在第一时间节点采集激光数据，并根据惯性测量单元和/或里程计采集的运动数据确定移动机器人在第一时间节点的第一位姿信息。

示例性的，规划移动机器人的移动路线，根据移动路线控制移动机器人进行移动并在移动过程中按照一定频率控制2D激光雷达连续采集多帧激光数据，并根据控制频率确定各帧激光数据的采集时间节点，第一时间节点即为激光数据的采集时间节点，第一位姿信息即为移动机器人在第一时间节点时的位姿。移动机器人移动过程中，通过惯性测量单元和/或里程计按照实时采集移动机器人的运动数据，该运动数据包括加速度数据和速度数据。根据移动机器人的初始位姿和运动数据推算出移动机器人在第一时间节点的第一位姿信息。其中，移动机器人的初始位姿可通过全球定位系统确定。图3是本申请实施例提供的移动机器人按照移动路线进行移动的示意图。如图3所示，在时间节点T0时移动机器人11位于移动路线12起点，通过全球定位系统确定移动机器人在时间节点T0时的初始位姿。移动机器人按照移动路线移动，并在第一时间节点T1和T1’时控制2D激光雷达采集激光数据，T1和T1’之间的时间差为2D激光雷达的采集周期。示例性的，根据T0和T1之间惯性测量单元和里程计采集的运动数据，可确定移动机器人从T0到T1的位姿偏差，根据该位姿偏差和T0时的初始位姿，确定出T1时的第一采集位姿。同理可推得T1’和后续各个第一时间节点的第一采集位姿。

S1102、通过异构传感器在第二时间节点采集传感数据，根据第二时间节点与最接近的第一时间节点之间的运动数据，确定对应两个时间节点之间的位姿变化量。

示例性的，移动机器人移动过程中按照一定频率控制异构传感器连续采集多帧的传感数据，并根据控制频率确定各帧传感数据的采集时间节点，第二时间节点即为传感数据的时间节点。参考图3，移动机器人在第二时间节点T2和T2’时控制异构传感器采集传感数据，T2和T2’之间的时间差为异构传感器的采集周期。T1和T1’是T2前后的两个第一时间节点，比较T2-T1和T1’-T2，如果T2-T1<T1’-T2，则T1为T2最接近的第一时间节点，如果T2-T1>T1’-T2，则T1’为T2最接近的第一时间节点。假设T1为T2最接近的第一时间节点，则可根据T1到T2之间的运动数据确定移动机器人从T1到T2的位姿偏差，根据该位姿偏差和T1的第一采集位姿可确定出T2的第二采集位姿。其中，移动机器人从T1到T2的位姿偏差即为对应时间节点之间的位姿变化量。同理可推得T2’的第二采集位姿以及后续第二时间节点的第二采集位姿。

S1103、将最接近的第一时间节点的第一位姿信息和对应的位姿变化量设置为第二时间节点的第二位姿信息。

需要说明的，由于运动数据在叠加推算出的移动机器人的位姿存在较大的误差，在后续构建栅格地图时会基于激光数据优化对应的第一采集位姿。因此本实施例在获取各个第二时间节点的第二位姿信息时，对应保存的是最接近的第一时间节点的第一采集位姿和与第一时间节点之间的位姿变化量，以便后续根据最接近的第一时间节点更新后的第一采集位姿和对应的位姿变化量对应更新第二采集位姿。例如，T2的第二位姿信息保存有T1的第一采集位姿以及T1和T2之间的位姿偏差。

S120、根据激光数据和对应的第一采集位姿生成栅格地图，并在生成栅格地图的过程中更新第一采集位姿。

示例性的，根据2D激光雷达与移动机器人之间的外参，将激光数据转换到机器人坐标系下。移动机器人的位姿可理解为机器人坐标系与世界坐标系的齐次变换矩阵，可通过激光数据对应的第一采集位姿，将机器人坐标系下的激光数据转换到世界坐标系下。根据激光数据在世界坐标系下的坐标，对应生成栅格地图中的避障点云。图4是本申请实施例提供的连续多帧激光数据的第一示意图。如图4所示，T1、T1’和T1”是三个连续的第一时间节点，移动机器人在T1、T1’和T1”时均采集到障碍点A、障碍点B和障碍点C的激光数据，并根据激光雷达外参和T1、T1’和T1”的第一采集位姿，确定出障碍点A、障碍点B和障碍点C在世界坐标系下的三维坐标，并获取到三维坐标中的横轴坐标和纵轴坐标。根据障碍点A、障碍点B和障碍点C的横轴坐标和纵轴坐标，在栅格地图的相应位置处添加避障点云。如果T1、T1’和T1”的第一采集位姿是准确的，即第一采集位姿是真实的机器人坐标系和世界坐标系之间的齐次变换矩阵，那么基于T1、T1’和T1”的第一采集位姿和对应的激光数据确定出的障碍点A的三维坐标是相同，即基于T1、T1’和T1”的激光数据生成栅格地图时，对于同一障碍点可以添加在栅格地图中的同一位置，其他障碍点同理。图5是本申请实施例提供的连续多帧激光数据的第二示意图。如图5所示，如果T1、T1’和T1”的第一采集位姿存在误差，则会导致基于T1、T1’和T1”的第一采集位姿和对应的激光数据确定出的障碍点A的三维坐标是不同。因此针对运动数据推算出的采集位姿存在的误差影响栅格地图精度的问题，在构建栅格地图的过程基于栅格地图中已有的障碍点坐标和待加入栅格地图的激光数据构建残差模型，以根据残差模型的最优解更新采集位姿。

在该实施例中，图6是本申请实施例提供的更新栅格地图的流程图。如图6所示，该更新栅格地图的步骤具体包括S1201-S1204：

S1201、将第一采集位姿对应的激光数据生成对应的子图，将子图与最新生成的栅格地图进行匹配以构建第一优化模型，解算第一优化模型并对第一采集位姿进行更新，更新后的第一采集位姿为第一优化模型的最优解。

示例性的，通过第一采集位姿和激光雷达外参将激光数据转换到世界坐标系下，并将世界坐标系下的激光数据投影至世界坐标系下的XY平面内，生成对应的子图。其中，子图可理解为第一采集位姿的激光数据对应生成的局部栅格地图。第一优化模型可理解为子图与最新生成的栅格地图进行匹配以构建出的残差模型。在该实施例中，在构建栅格地图时会按照激光数据的采集顺序，依次将激光数据加入栅格地图的构建中，并在构建过程中基于最新生成的栅格地图优化当前加入构建栅格地图的激光数据的第一采集位姿。例如，假设子图P是基于T1的第一采集位姿生成的，此时最新生成的栅格地图Q是基于T1以前的第一时间节点采集的激光数据构建的，将子图P与栅格地图Q进行匹配并构建出残差模型，解算残差模型得到最优解，并将最优解更新为T1的第一采集位姿。

但是对于子图与栅格地图进行匹配构建得到的残差模型，在解算时较为复杂解算速度较低，影响构图效率。对此，本实施例依次将子图与从低分辨率到高分辨率的栅格地图进行匹配构建对应的残差模型，解算低分辨率对应的残差模型的最优解并将最优解作为高分辨率对应的残差模型的先验约束值。由于低分辨率对应的残差模型的解算速度快，以及有先验约束值可以有效提高残差模型的解算速度，总体上提高了第一采集位姿的优化效率和栅格地图的构建效率。在该实施例中，图7是本申请实施例提供的解算第一优化模型的流程图。如图7所示，该解算第一优化模型的步骤具体包括S12011-S12012：

S12011、将最新生成的栅格地图转换为多种分辨率的栅格地图，从低分辨率到高分辨率依次将各个分辨率的栅格地图与子图进行匹配并构建对应的第一优化模型，解算第一优化模型的最优解；其中，低分辨率的栅格地图对应的第一优化模型的最优解为高分辨率的栅格地图对应的第一优化模型的先验约束值。

示例性的，将栅格地图Q转换为四种分辨率的栅格地图，从低分辨率的栅格地图到高分辨率的栅格地图分别为q4、q3、q2和q1，其中最高分辨率的栅格地图q1即为栅格地图Q本身。将子图P与栅格地图q4进行匹配以构建对应的残差模型，解算该残差模型得到最优解，并将最优解作为栅格地图q3的残差模型的先验约束值。将子图P与栅格地图q3进行匹配以构建对应的残差模型，并根据对应先验约束值解算该残差模型。残差模型的先验约束值相当于给残差模型提供解约束，以在解算残差模型时在先验约束值的有限范围内搜索残差模型的最优解。栅格地图q2和栅格地图q1的残差模型的构建和解算过程同理。

在该实施例中，解算带有先验约束值的残差模型时，根据第一优化模型的先验约束值，基于二分查找规则解算出第一优化模型的最优解。示例性的，假设先验约束值为k，先验约束值的有限范围为(k-d，k+d)，首先分别在(k-d，k)和(k，k+d)中搜索出残差模型的解，并进行比较确定哪个范围的解误差更小。若(k-d，k)中的解k1误差更小，则根据k1将(k-d，k)划分成两个范围(k-d，k1)和(k1，k)，并分别从两个范围中搜索出残差模型的解。重复上述过程直至确定出残差模型的最优解。基于先验约束值和二分查找规则可确定残差模型的优化方向，并基于优化方向快速解算出残差模型的最优解，有效提高残差模型的解算效率。

S12012、将最高分辨率的栅格地图对应的第一优化模型的最优解更新为第一采集位姿。

示例性的，栅格地图q1的残差模型解算得到最优解即为栅格地图Q的残差模型的最优解，将该最优解更新为T1的第一采集位姿。

S1202、根据更新后的第一采集位姿，将对应激光数据生成的子图添加至栅格地图中以更新栅格地图。

示例性的，根据T1新的第一采集位姿和激光雷达外参将对应的激光数据转换到世界坐标系下，并将世界坐标系下的激光数据投影至XY平面内生成新的子图，将新的子图添加到栅格地图中，得到新的栅格地图。新的栅格地图可用于后续优化T1’的第一采集位姿。

S1203、在根据移动机器人返回起点采集的激光数据更新对应的第一采集位姿和栅格地图后，将每一子图分别与最新生成的栅格地图进行匹配以构建对应的第二优化模型，解算第二优化模型并对子图对应的第一采集位姿进行再次更新，再次更新后的第一采集位姿为第二优化模型的最优解。

图8是本申请实施例提供的移动路线的示意图。如图8所示，事先根据待构建栅格地图的目标区域13规划出移动机器人的单程移动路线14，单程移动路线包括起点和终点，移动机器人按照单程移动路线从起点移动到终点并在移动过程中可采集到目标区域中各个位置处的障碍点的激光数据。本实施例为了提高栅格地图的精度，规划了移动机器人的返程移动路线15，当移动机器人按照返程移动路线15返回起点并在返回起点过程中采集激光数据以更新栅格地图。当返回起点时对应采集激光数据，并基于该激光数据和最新生成的栅格地图进行匹配优化对应的第一采集位姿，根据第一采集位姿更新栅格地图。本实施例控制移动机器人返回起点时采集激光数据以形成回环检测，提高了栅格地图的精度。示例性的，移动机器人返回到起点采集对应的激光数据，并根据该激光数据生成对应的子图。将最新生成的栅格地图转换成多种分辨率的栅格地图，并依次将低分辨率到高分辨率的栅格地图与该子图进行匹配以构建对应的残差模型，并从低分辨率到高分辨率解算出残差模型的最优解，将最高分辨率的最优解更新为对应第一采集位姿，并基于该第一采集位姿和对应的激光数据生成对应的子图，并该子图添加至栅格地图中，得到前端检测构建出的栅格地图。由于前端检测构建出的栅格地图精度还不够高，因此需通过后端优化进一步优化第一采集位姿以提高栅格地图的精度。

在后端优化过程中，将各个第一采集位姿对应的子图分别与前端检测构建出的栅格地图进行匹配以构建对应的残差模型，并解算该残差模型的最优解，将最优解更新为对应的第一采集位姿。例如，将T1的子图与前端检测构建出的栅格地图进行匹配以构建对应的残差模型，解算该残差模型的最优解，并将最优解更新为T1的第一采集位姿。在该实施例中，由于多个第一采集位姿的优化过程互不相关，因此可并发执行多个第一采集位姿的优化过程。示例性的，通过多线程并发将多个子图与最新生成的栅格地图进行匹配以构建多个第二优化模型，并对多个第二优化模型进行解算，得到多个子图更新后的第一采集位姿。例如，通过多线程中的一个线程将T1的子图与前端检测构建出的栅格地图进行匹配以构建对应的残差模型，解算该残差模型的最优解，并将最优解更新为T1的第一采集位姿。剩余的第一采集位姿由其他的线程或T1对应的线程更新完T1的第一采集位姿后进行更新。其中，第二优化模型即为各个子图与前端检测构建出的栅格地图进行匹配以构建出的残差模型。

同样的，为提高第二优化模型的解算速度，将前端检测构建出的栅格地图转换成多种分辨率，并从低分辨率到高分辨率的栅格地图与子图进行匹配以构建对应的残差模型，解算该残差模型的最优解，并将低分辨率的残差模型的最优解作为高分辨率的残差模型的先验约束值。示例性的，图9是本申请实施例提供的解算第二优化模型的流程图。如图9所示，该解算第二优化模型的步骤具体包括：

S12031、将最新生成的栅格地图转换为多种分辨率的栅格地图，从低分辨率到高分辨率依次将各个分辨率的栅格地图与子图进行匹配并构建对应的第二优化模型，解算第二优化模型的最优解；其中，低分辨率的栅格地图对应的第二优化模型的最优解为高分辨率的栅格地图对应的第二优化模型的先验约束值。

示例性的，根据前端检测后T1的第一采集位姿和对应的激光数据构建对应的子图H。将前端检测构建出的栅格地图G转换为四种分辨率的栅格地图，从低分辨率的栅格地图到高分辨率的栅格地图分别为g4、g3、g2和g1，其中最高分辨率的栅格地图g1即为栅格地图G本身。将子图H与栅格地图g4进行匹配以构建对应的残差模型，解算该残差模型得到最优解，并将最优解作为栅格地图g3的残差模型的先验约束值。将子图H与栅格地图g3进行匹配以构建对应的残差模型，并根据对应先验约束值解算该残差模型。残差模型的先验约束值相当于给残差模型提供解约束，以在解算残差模型时在先验约束值的有限范围内搜索残差模型的最优解。栅格地图g2和栅格地图g1的残差模型的构建和解算过程同理。

在该实施例中，根据先验约束值解算第二优化模型时，同样也可以通过二分查找规则解算出第二优化模型的最优解。具体实现过程可参考第一优化模型的解算过程。

S12032、将最高分辨率的栅格地图对应的第二优化模型的最优解更新为第一采集位姿。

示例性的，栅格地图g1的残差模型解算得到最优解即为栅格地图G的残差模型的最优解，将该最优解更新为T1的第一采集位姿。对于其他第一时间节点的第一采集位姿的优化过程同理。

S1204、根据再次最新后的第一采集位姿，对栅格地图中对应的子图进行调整以更新栅格地图。

示例性的，根据后端优化后的每个第一采集位姿和激光雷达外参，生成对应的子图，并将子图添加至前端检测构建出的栅格地图中，得到后端优化构建出的栅格地图。

S130、基于更新后的第一采集位姿对第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至栅格地图中。

由于第二采集位姿是基于第一采集位姿确定的，因此构建栅格地图中对第一采集位姿进行更新优化后，也要相应更新优化第二采集位姿，并基于更新优化后的第二采集位姿将对应的传感数据添加至后端优化构建出的栅格地图中。

在一实施例中，图10是本申请实施例提供的将传感数据添加至栅格地图的流程图。如图10所示，该将传感数据添加至栅格地图的步骤具体包括S1301-S1304：

S1301、根据第二位姿信息中的第一位姿信息，确定对应更新后的第一位姿信息。

示例性的，从T2的第二位姿信息中获取T1的第一采集位姿，根据T1对应获取更新优化的第一采集位姿。

S1302、根据第二位姿信息中的位姿变化量对更新后的第一位姿信息进行转换，得到更新后的第二位姿信息。

示例性的，从T2的第二位姿信息中获取T1和T2之间的位姿偏差，将T1的更新优化后的第一采集位姿乘以该位姿偏差，得到T2的更新优化后的第二采集位姿。

同理可得其余第二时间节点的更新优化后的第二采集位姿。

S1303、根据异构传感器与激光雷达的外参，将传感数据对应转换成激光数据，并基于更新后的第二采集位姿，确定对应激光数据在栅格地图中的位置。

示例性的，通过异构传感器坐标系与激光雷达坐标系之间的齐次转换矩阵，将传感数据转换到激光雷达坐标系下，通过激光雷达坐标系与机器人坐标系之间的齐次变换矩阵将传感数据转换到机器人坐标系下，通过第二采集位姿将传感数据转换到世界坐标系下，并投影到世界坐标系的XY平面内，得到传感数据在栅格地图中的横轴坐标和纵轴坐标。其中，异构传感器坐标系与激光雷达坐标系之间的齐次转换矩阵即为异构传感器与激光雷达的外参，转换到激光雷达坐标系下的传感数据可看作激光数据。第二采集位姿可理解为采集对应传感数据时机器人坐标系与世界坐标系之间的齐次变换矩阵，激光数据在栅格地图中的位置可理解为对应传感数据在栅格地图中的横轴坐标和纵轴坐标。

在一实施例中，为确定异构传感器坐标系与激光雷达坐标系之间的齐次变换矩阵，事先对异构传感器的外参进行标定。针对异构传感器中深度相机的标定，将其获取的深度图像转换为2D点云数据，并通过ICP(最近点迭代匹配)和激光雷达采集的2D激光数据进行匹配，通过匹配的2D点云数据和2D激光数据确定深度相机坐标系与激光雷达坐标系之间的旋转矩阵和平移矩阵，进而得到深度相机坐标系与激光雷达坐标系之间的齐次变换矩阵。针对异构传感器中超声波传感器和红外传感器的标定，将齐次变换矩阵和构图效果相结合，通过微调齐次变换矩阵的方式实现最优构图效果，并将最优构图效果对应的齐次变换矩阵确定为对应传感器坐标系与雷达坐标坐标系之间的齐次变换矩阵。

S1304、通过贝叶斯滤波将传感数据对应的激光数据与栅格地图在对应位置处的激光数据进行融合，以更新栅格地图。

示例性的，通过贝叶斯滤波方法根据栅格地图中同一位置处的传感数据和激光数据，估计该位置处的代价值。通过任一位置的代价值和预设的代价阈值，确定对应位置处是否存在避障点云。其中，代价值可理解为对应位置处存在避障点云的概率值，代价阈值可理解为栅格地图中存在避障点云时的最小概率值。示例性的，当该位置处的代价值大于预设的代价阈值时，则表明该位置处存在避障点云，进而在栅格地图的对应位置处添加避障点云。当该位置处的代价值小于预设的代价阈值时，则表明该位置处不存在避障点云，进而在栅格地图的对应位置处删除避障点云。在栅格地图的所有位置上都对应添加或删除避障点云后，得到了传感数据和激光数据融合构建的高精度栅格地图，移动机器人可基于该高精度栅格地图进行移动导航，提高了移动导航效率。

综上，本申请实施例提供的栅格地图构建方法，通过在根据激光数据构建栅格地图时根据惯性测量单元和/或里程计采集的运动数据确定激光数据的采集位姿，根据激光数据和对应的采集位姿构生成栅格地图，并基于栅格地图和激光数据优化采集位姿，减小惯性测量单元和/或里程计带来误差，根据优化的采集位姿更新栅格地图，以构建精度更高的栅格地图。根据优化后的激光数据的采集位姿对传感数据的采集位姿进行更新，以精准确定传感数据位于栅格地图中的位置。将传感数据添加至栅格地图中以生成融合有激光数据和传感数据的栅格地图，异构传感器采集的传感数据可以反馈移动机器人周围的低矮障碍物和负向障碍物的分布情况，使得栅格地图不仅记录有激光雷达采集到的障碍物，还记录有异构传感器采集到的低矮障碍物和负向障碍物，填补了激光雷达的采集盲区，提高了栅格地图的精度和准确度。当移动机器人基于高精度的栅格地图进行导航规划时，可根据栅格地图记录的障碍物区域精准快速规划处避障路线，提高导航路径的规格效率，保证移动机器人的工作效率。

在上述实施例的基础上，图11为本申请实施例提供的一种栅格地图构建装置的结构示意图。参考图11，本实施例提供的栅格地图构建装置具体包括：位姿获取模块21、第一构建模块22和第二构建模块23。

其中，位姿获取模块，被配置为获取激光雷达采集的激光数据和激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和传感数据对应的第二采集位姿；

第一构建模块，被配置为根据激光数据和对应的第一采集位姿生成栅格地图，并在生成栅格地图的过程中更新第一采集位姿；

第二构建模块，被配置为基于更新后的第一采集位姿对第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至栅格地图中。

在上述实施例的基础上，位姿获取模块包括：第一位姿信息获取子模块，被配置为通过激光雷达在第一时间节点采集激光数据，并根据惯性测量单元和/或里程计采集的运动数据确定移动机器人在第一时间节点的第一位姿信息；位姿变化量确定子模块，被配置为通过异构传感器在第二时间节点采集传感数据，根据第二时间节点与最接近的第一时间节点之间的运动数据，确定对应两个时间节点之间的位姿变化量；第二位姿信息获取子模块，被配置为将最接近的第一时间节点的第一位姿信息和对应的位姿变化量设置为第二时间节点的第二位姿信息。

在上述实施例的基础上，第一构建模块包括：第一优化模型生成子模块，被配置为将第一采集位姿对应的激光数据生成对应的子图，将子图与最新生成的栅格地图进行匹配以构建第一优化模型，解算第一优化模型并对第一采集位姿进行更新，更新后的第一采集位姿为第一优化模型的最优解；第一地图更新子模块，被配置为根据更新后的第一采集位姿，将对应激光数据生成的子图添加至栅格地图中以更新栅格地图；第二优化模型生成子模块，被配置为在根据移动机器人返回起点采集的激光数据更新对应的第一采集位姿和栅格地图后，将每一子图分别与最新生成的栅格地图进行匹配以构建对应的第二优化模型，解算第二优化模型并对子图对应的第一采集位姿进行再次更新，再次更新后的第一采集位姿为第二优化模型的最优解；第二地图更新子模块，被配置为根据再次最新后的第一采集位姿，对栅格地图中对应的子图进行调整以更新栅格地图。

在上述实施例的基础上，第一优化模型生成子模块包括：第一优化模型生成单元，被配置为将最新生成的栅格地图转换为多种分辨率的栅格地图，从低分辨率到高分辨率依次将各个分辨率的栅格地图与子图进行匹配并构建对应的第一优化模型，解算第一优化模型的最优解；其中，低分辨率的栅格地图对应的第一优化模型的最优解为高分辨率的栅格地图对应的第一优化模型的先验约束值；第一采集位姿更新单元，被配置为将最高分辨率的栅格地图对应的第一优化模型的最优解更新为第一采集位姿。

在上述实施例的基础上，第一优化模型生成单元包括：第一解算子单元，被配置为根据第一优化模型的先验约束值，基于二分查找规则解算出第一优化模型的最优解。

在上述实施例的基础上，第二优化模型生成子模块包括：多线程生成模型单元，被配置为通过多线程并发将多个子图与最新生成的栅格地图进行匹配以构建多个第二优化模型，并对多个第二优化模型进行解算，得到多个子图更新后的第一采集位姿。

在上述实施例的基础上，多线程生成模型单元包括：第二优化模型生成子单元，被配置为将最新生成的栅格地图转换为多种分辨率的栅格地图，从低分辨率到高分辨率依次将各个分辨率的栅格地图与子图进行匹配并构建对应的第二优化模型，解算第二优化模型的最优解；其中，低分辨率的栅格地图对应的第二优化模型的最优解为高分辨率的栅格地图对应的第二优化模型的先验约束值；第一采集位姿更新子单元，被配置为将最高分辨率的栅格地图对应的第二优化模型的最优解更新为第一采集位姿。

在上述实施例的基础上，第二构建模块包括：最新第一位姿信息获取子模块，被配置为根据第二位姿信息中的第一位姿信息，确定对应更新后的第一位姿信息；第二位姿信息更新子模块，被配置为根据第二位姿信息中的位姿变化量对更新后的第一位姿信息进行转换，得到更新后的第二位姿信息。

在上述实施例的基础上，第二构建模块包括：传感数据转换子模块，被配置为根据异构传感器与激光雷达的外参，将传感数据对应转换成激光数据，并基于更新后的第二采集位姿，确定对应激光数据在栅格地图中的位置；传感数据融合子模块，被配置为通过贝叶斯滤波将传感数据对应的激光数据与栅格地图在对应位置处的激光数据进行融合，以更新栅格地图。

上述，本申请实施例提供的栅格地图构建装置，通过在根据激光数据构建栅格地图时根据惯性测量单元和/或里程计采集的运动数据确定激光数据的采集位姿，根据激光数据和对应的采集位姿构生成栅格地图，并基于栅格地图和激光数据优化采集位姿，减小惯性测量单元和/或里程计带来误差，根据优化的采集位姿更新栅格地图，以构建精度更高的栅格地图。根据优化后的激光数据的采集位姿对传感数据的采集位姿进行更新，以精准确定传感数据位于栅格地图中的位置。将传感数据添加至栅格地图中以生成融合有激光数据和传感数据的栅格地图，异构传感器采集的传感数据可以反馈移动机器人周围的低矮障碍物和负向障碍物的分布情况，使得栅格地图不仅记录有激光雷达采集到的障碍物，还记录有异构传感器采集到的低矮障碍物和负向障碍物，填补了激光雷达的采集盲区，提高了栅格地图的精度和准确度。当移动机器人基于高精度的栅格地图进行导航规划时，可根据栅格地图记录的障碍物区域精准快速规划处避障路线，提高导航路径的规格效率，保证移动机器人的工作效率。

本申请实施例提供的栅格地图构建装置可以用于执行上述实施例提供的栅格地图构建方法，具备相应的功能和有益效果。

图12是本申请实施例提供的一种栅格地图构建设备的结构示意图，参考图12，该栅格地图构建设备包括：处理器31、存储装置32、通信装置33、输入装置34及输出装置35。该栅格地图构建设备中处理器31的数量可以是一个或者多个，该栅格地图构建设备中的存储装置32的数量可以是一个或者多个。该栅格地图构建设备的处理器31、存储装置32、通信装置33、输入装置34及输出装置35可以通过总线或者其他方式连接。

存储装置32作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本申请任意实施例的栅格地图构建方法对应的程序指令/模块(例如，栅格地图构建装置中的位姿获取模块21、第一构建模块22和第二构建模块23)。存储装置32可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据设备的使用所创建的数据等。此外，存储装置32可以包括高速随机存取存储装置，还可以包括非易失性存储装置，例如至少一个磁盘存储装置件、闪存器件、或其他非易失性固态存储装置件。在一些实例中，存储装置可进一步包括相对于处理器远程设置的存储装置，这些远程存储装置可以通过网络连接至设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

通信装置33用于进行数据传输。

处理器31通过运行存储在存储装置32中的软件程序、指令以及模块，从而执行设备的各种功能应用以及数据处理，即实现上述的栅格地图构建方法。

输入装置34可用于接收输入的数字或字符信息，以及产生与设备的用户设置以及功能控制有关的键信号输入。输出装置35可包括显示屏等显示设备。

上述提供的栅格地图构建设备可用于执行上述实施例提供的栅格地图构建方法，具备相应的功能和有益效果。

本申请实施例还提供一种包含计算机可执行指令的存储介质，计算机可执行指令在由计算机处理器执行时用于执行一种栅格地图构建方法，该栅格地图构建方法包括：获取激光雷达采集的激光数据和激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和传感数据对应的第二采集位姿；根据激光数据和对应的第一采集位姿生成栅格地图，并在生成栅格地图的过程中更新第一采集位姿；基于更新后的第一采集位姿对第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至栅格地图中。

存储介质——任何的各种类型的存储装置设备或存储设备。术语“存储介质”旨在包括：安装介质，例如CD-ROM、软盘或磁带装置；计算机系统存储装置或随机存取存储装置，诸如DRAM、DDR RAM、SRAM、EDO RAM，兰巴斯(Rambus)RAM等；非易失性存储装置，诸如闪存、磁介质(例如硬盘或光存储)；寄存器或其它相似类型的存储装置元件等。存储介质可以还包括其它类型的存储装置或其组合。另外，存储介质可以位于程序在其中被执行的第一计算机系统中，或者可以位于不同的第二计算机系统中，第二计算机系统通过网络(诸如因特网)连接到第一计算机系统。第二计算机系统可以提供程序指令给第一计算机用于执行。术语“存储介质”可以包括驻留在不同位置中(例如在通过网络连接的不同计算机系统中)的两个或更多存储介质。存储介质可以存储可由一个或多个处理器执行的程序指令(例如具体实现为计算机程序)。

当然，本申请实施例所提供的一种包含计算机可执行指令的存储介质，其计算机可执行指令不限于如上的栅格地图构建方法，还可以执行本申请任意实施例所提供的栅格地图构建方法中的相关操作。

上述实施例中提供的栅格地图构建装置、存储介质及栅格地图构建设备可执行本申请任意实施例所提供的栅格地图构建方法，未在上述实施例中详尽描述的技术细节，可参见本申请任意实施例所提供的栅格地图构建方法。

上述仅为本申请的较佳实施例及所运用的技术原理。本申请不限于这里的特定实施例，对本领域技术人员来说能够进行的各种明显变化、重新调整及替代均不会脱离本申请的保护范围。因此，虽然通过以上实施例对本申请进行了较为详细的说明，但是本申请不仅仅限于以上实施例，在不脱离本申请构思的情况下，还可以包括更多其他等效实施例，而本申请的范围由权利要求的范围决定。

Claims (12)

1.一种栅格地图构建方法，其特征在于，包括：

获取激光雷达采集的激光数据和所述激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和所述传感数据对应的第二采集位姿；

根据所述激光数据和对应的第一采集位姿生成栅格地图，并在生成所述栅格地图的过程中更新所述第一采集位姿；

基于更新后的第一采集位姿对所述第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至所述栅格地图中。

2.根据权利要求1所述的栅格地图构建方法，其特征在于，所述获取激光雷达采集的激光数据和所述激光数据对应的采集位姿，获取异构传感器采集的传感数据和所述传感数据对应的采集位姿，包括：

通过所述激光雷达在第一时间节点采集所述激光数据，并根据惯性测量单元和/或里程计采集的运动数据确定移动机器人在所述第一时间节点的第一位姿信息；

通过所述异构传感器在第二时间节点采集所述传感数据，根据所述第二时间节点与最接近的第一时间节点之间的运动数据，确定对应两个时间节点之间的位姿变化量；

将最接近的第一时间节点的第一位姿信息和对应的位姿变化量设置为所述第二时间节点的第二位姿信息。

3.根据权利要求1所述的栅格地图构建方法，其特征在于，所述根据所述激光数据和对应的第一采集位姿生成栅格地图，并在生成所述栅格地图的过程中更新所述第一采集位姿，包括：

将所述第一采集位姿对应的激光数据生成对应的子图，将所述子图与最新生成的栅格地图进行匹配以构建第一优化模型，解算所述第一优化模型并对所述第一采集位姿进行更新，更新后的第一采集位姿为所述第一优化模型的最优解；

根据更新后的第一采集位姿，将对应激光数据生成的子图添加至所述栅格地图中以更新所述栅格地图；

在根据移动机器人返回起点采集的激光数据更新对应的第一采集位姿和所述栅格地图后，将每一子图分别与最新生成的栅格地图进行匹配以构建对应的第二优化模型，解算所述第二优化模型并对所述子图对应的第一采集位姿进行再次更新，再次更新后的第一采集位姿为所述第二优化模型的最优解；

根据再次最新后的第一采集位姿，对所述栅格地图中对应的子图进行调整以更新所述栅格地图。

4.根据权利要求3所述的栅格地图构建方法，其特征在于，所述将所述子图与最新生成的栅格地图进行匹配以构建第一优化模型，解算所述第一优化模型并对所述第一采集位姿进行更新，包括：

将最新生成的栅格地图转换为多种分辨率的栅格地图，从低分辨率到高分辨率依次将各个分辨率的栅格地图与所述子图进行匹配并构建对应的第一优化模型，解算所述第一优化模型的最优解；其中，低分辨率的栅格地图对应的第一优化模型的最优解为高分辨率的栅格地图对应的第一优化模型的先验约束值；

将最高分辨率的栅格地图对应的第一优化模型的最优解更新为所述第一采集位姿。

5.根据权利要求4所述的栅格地图构建方法，其特征在于，所述解算所述第一优化模型的最优解，包括：

根据所述第一优化模型的先验约束值，基于二分查找规则解算出所述第一优化模型的最优解。

6.根据权利要求3所述的栅格地图构建方法，其特征在于，所述将每一子图分别与最新生成的栅格地图进行匹配以构建对应的第二优化模型，解算所述第二优化模型并对所述子图对应的第一采集位姿进行再次更新，包括：

通过多线程并发将多个子图与最新生成的栅格地图进行匹配以构建多个第二优化模型，并对多个所述第二优化模型进行解算，得到多个子图更新后的第一采集位姿。

7.根据权利要求6所述的栅格地图构建方法，其特征在于，所述通过多线程并发将多个子图与最新生成的栅格地图进行匹配以构建多个第二优化模型，并对多个所述第二优化模型进行解算，得到多个子图更新后的第一采集位姿，包括：

将最新生成的栅格地图转换为多种分辨率的栅格地图，从低分辨率到高分辨率依次将各个分辨率的栅格地图与所述子图进行匹配并构建对应的第二优化模型，解算所述第二优化模型的最优解；其中，低分辨率的栅格地图对应的第二优化模型的最优解为高分辨率的栅格地图对应的第二优化模型的先验约束值；

将最高分辨率的栅格地图对应的第二优化模型的最优解更新为所述第一采集位姿。

8.根据权利要求2所述的栅格地图构建方法，其特征在于，所述基于更新后的第一采集位姿对所述第二采集位姿进行更新，包括：

根据所述第二位姿信息中的第一位姿信息，确定对应更新后的第一位姿信息；

根据所述第二位姿信息中的位姿变化量对更新后的第一位姿信息进行转换，得到更新后的第二位姿信息。

9.根据权利要求2所述的栅格地图构建方法，其特征在于，所述根据更新后的第二采集位姿，将对应的传感数据添加至所述栅格地图中，包括：

根据所述异构传感器与所述激光雷达的外参，将所述传感数据对应转换成激光数据，并基于更新后的第二采集位姿，确定对应激光数据在所述栅格地图中的位置；

通过贝叶斯滤波将所述传感数据对应的激光数据与所述栅格地图在对应位置处的激光数据进行融合，以更新所述栅格地图。

10.一种栅格地图构建装置，其特征在于，包括：

位姿获取模块，被配置为获取激光雷达采集的激光数据和所述激光数据对应的第一采集位姿，获取异构传感器采集的传感数据和所述传感数据对应的第二采集位姿；

第一构建模块，被配置为根据所述激光数据和对应的第一采集位姿生成栅格地图，并在生成所述栅格地图的过程中更新所述第一采集位姿；

第二构建模块，被配置为基于更新后的第一采集位姿对所述第二采集位姿进行更新，根据更新后的第二采集位姿，将对应的传感数据添加至所述栅格地图中。

11.一种栅格地图构建设备，其特征在于，包括：一个或多个处理器；存储装置，存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-9任一所述的栅格地图构建方法。

12.一种包含计算机可执行指令的存储介质，其特征在于，所述计算机可执行指令在由计算机处理器执行时用于执行如权利要求1-9任一所述的栅格地图构建方法。