CN111427060B - 一种基于激光雷达的二维栅格地图构建方法和系统 - Google Patents

Abstract

本发明实施例公开了一种基于激光雷达的二维栅格地图构建方法和系统。该基于激光雷达的二维栅格地图构建方法包括：获取目标自主体上的激光雷达获取在第一时刻下扫描得到的目标区域的第一二维点云数据；根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；根据所述述第一二维点云数据和所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；根据所述第二位姿信息构建所述目标区域的二维栅格地图。本发明实施例实现了高效低占用的构建高精度二维栅格地图。

Description

一种基于激光雷达的二维栅格地图构建方法和系统

技术领域

本发明实施例涉及定位和建图技术，尤其涉及一种基于激光雷达的二维栅格地图构建方法和系统。

背景技术

随着人类进入信息产业革命时代，人工智能技术迅速发展，智能机器人技术正以前所未有的速度发展革新，并逐渐渗透各个行业。

定位和建图技术是智能机器人、无人驾驶等领域不可或缺的一个核心模块，该技术能够告诉机器人本体所处的位置，并指导其进行运动和避障。

目前，在工程上比较常用的定位建图技术有Gmapping、Cartographer等，它们分别利用粒子滤波和图优化的技术来实现2D的定位和建图。Gmapping采用自适应蒙特卡洛定位，利用许多的粒子来表示机器人可能的位姿，每一个粒子都是机器人在真实空间中的位姿的一个可能假设，并且在运动过程中，需要更新所有粒子的状态并维持每一个粒子的位姿以及地图，该算法虽然能够实现较好的定位和建图，但是它十分耗时且对计算机的资源占用也比较大。

发明内容

本发明实施例提供一种基于激光雷达的二维栅格地图构建方法和系统，以实现高效低占用的构建高精度二维栅格地图。

为达此目的，本发明实施例提供了一种基于激光雷达的二维栅格地图构建方法，该基于激光雷达的二维栅格地图构建方法包括：

获取目标自主体上的激光雷达获取在第一时刻下扫描得到的目标区域的第一二维点云数据；

根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；

预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；

根据所述第一二维点云数据和所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；

根据所述第二位姿信息构建所述目标区域的二维栅格地图。

进一步的，所述获取目标自主体上的激光雷达获取在第一时刻下扫描得到的目标区域的第一二维点云数据包括：

目标自主体上的激光雷达为单线激光雷达时，直接通过所述激光雷达获取第一时刻下目标区域的第一二维点云数据；

目标自主体上的激光雷达为多线激光雷达，确认所述多线激光雷达中预设发射角度的第一激光，并将所述第一激光获取的点云数据根据所述预设发射角度投影至水平面上以获取第一时刻下目标区域的第一二维点云数据。

进一步的，所述根据所述第一二维点云数据得到所述目标自主体的第一位姿信息包括：

获取所述激光雷达获取在第二时刻下扫描得到的目标区域的第三二维点云数据，所述第二时刻为所述第一时刻的前一时刻；

获取所述目标自主体第二时刻下的第三位姿信息；

根据NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系；

根据所述第一相对位姿关系和第三位姿信息确定所述目标自主体的第一位姿信息。

进一步的，所述根据NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系包括：

获取所述目标自主体第二时刻下的移动速度；

根据所述移动速度和第三位姿信息预测所述目标自主体第一时刻下的第四位姿信息；

根据所述第四位姿信息利用NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系。

进一步的，所述预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据包括：

获取所述激光雷达的性能参数和预先建立的二维环境地图；

根据所述性能参数和所述二维环境地图预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据。

进一步的，所述根据所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息包括：

确认所述第一位姿信息与所述第二二维点云数据之间的映射关系；

根据所述映射关系对所述第一位姿信息进行调整以获取所述第一二维点云数据和第二二维点云数据之间误差最小时的第二位姿信息。

进一步的，所述确认所述第一位姿信息与所述第二二维点云数据之间的映射关系包括：

定义一个映射函数f_i(x)，其中f_i(x)的值表示在位姿信息为x时所述激光雷达理论上扫描得到的第二二维点云数据，x表示所述位姿信息，i表示确定所述目标自主体位姿信息的次数。

进一步的，所述根据所述映射关系对所述第一位姿信息进行调整以获取所述第一二维点云数据和第二二维点云数据之间误差最小时的第二位姿信息包括：

定义一个误差函数e_i(x)，其中e_i(x)＝f_i(x)-z_i，e_i(x)的值表示所述第一二维点云数据和第二二维点云数据之间误差，z_i表示所述第一二维点云数据；

定义一个目标函数F(x)，其中

目标函数F(x)由所述误差函数e_i(x)平方后取其协方差矩阵得到；

对x进行调整以获取所述目标函数F(x)的值最小时的x_min，并将所述x_min作为第二位姿信息。

进一步的，所述对x进行调整以获取所述目标函数F(x)最小时的x_min包括：

将所述第一位姿信息作为初始值x₀；

从所述初始值x₀开始对所述目标函数F(x)进行多次迭代直至F(x_k+1)的值达到极小值，其中x_k+1＝x_k+Δx_k，k表示迭代次数，当Δx_k小于第一阈值时，停止迭代并将此时的x_k作为x_min。

一方面，本发明实施例还提供了一种基于激光雷达的二维栅格地图构建系统，该基于激光雷达的二维栅格地图构建系统包括：

数据获取模块，用于获取目标自主体上的激光雷达获取在第一时刻下扫描得到的目标区域的第一二维点云数据；

位姿获取模块，用于根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；

数据预测模块，用于预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；

位姿优化模块，用于根据所述第一二维点云数据和所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；

地图构建模块，用于根据所述第二位姿信息构建所述目标区域的二维栅格地图。

另一方面，本发明实施例还提供了一种基于激光雷达的二维栅格地图构建设备，该设备包括：一个或处理器；存储装置，用于存储一个或程序，当所述一个或程序被所述一个或处理器执行，使得所述一个或处理器实现如本发明任一实施例提供的方法。

又一方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如本发明任一实施例提供的方法。

本发明实施例获取目标自主体上的激光雷达在第一时刻下扫描得到的目标区域的第一二维点云数据；根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；根据所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；根据所述第二位姿信息构建所述目标区域的二维栅格地图，解决了现有的构建二维栅格地图精度不够的问题。

附图说明

图1是本发明实施例一提供的一种基于激光雷达的二维栅格地图构建方法的流程示意图；

图2是本发明实施例二提供的一种基于激光雷达的二维栅格地图构建方法的流程示意图；

图3是图2所示实施例中步骤S230的一具体流程示意图；

图4是图2所示实施例中步骤S260的一具体流程示意图；

图5是本发明实施例三提供的一种基于激光雷达的二维栅格地图构建系统的结构示意图；

图6为本发明实施例四提供的一种基于激光雷达的二维栅格地图构建设备的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各步骤描述成顺序的处理，但是其中的许多步骤可以被并行地、并发地或者同时实施。此外，各步骤的顺序可以被重新安排。当其操作完成时处理可以被终止，但是还可以具有未包括在附图中的附加步骤。处理可以对应于方法、函数、规程、子例程、子程序等等。

此外，术语“第一”、“第二”等可在本文中用于描述各种方向、动作、步骤或元件等，但这些方向、动作、步骤或元件不受这些术语限制。这些术语仅用于将第一个方向、动作、步骤或元件与另一个方向、动作、步骤或元件区分。举例来说，在不脱离本申请的范围的情况下，可以将第一二维点云数据称为第二二维点云数据，且类似地，可将第二二维点云数据称为第一二维点云数据。第一二维点云数据和第二二维点云数据两者都是二维点云数据，但其不是同一二维点云数据。术语“第一”、“第二”等不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更所述特征。在本发明实施例的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

实施例一

如图1所示，本发明实施例一提供了一种基于激光雷达的二维栅格地图构建方法，该方法包括：

S110、获取目标自主体上的激光雷达在第一时刻下扫描得到的目标区域的第一二维点云数据。

本实施例中，目标自主体可以为机器人、自移动车辆或其他具有自移动功能的自主体。示例性的，目标自主体为扫地机器人，目标区域则为需要打扫的房间区域，二维栅格地图的构建可以是实时构建或离线的非实时构建，在本实施例中，二维栅格地图是一个实时建图的过程，第一时刻是指当前的实时时刻，第一二维点云数据也即为实时获取到的点云数据。在其他的实施例中，二维栅格地图也可以是离线建图过程，此时第一时刻指的是扫地机器人行走过程中的任一需要进行位姿确定的时刻，此时第一二维点云数据则为对应于该第一时刻下的激光雷达扫描得到的点云数据。激光雷达可以安装在自主体的侧面或者顶部，只需要确保其能够给实现对目标区域的扫描即可。激光雷达可以为多线激光雷达也可以为单线激光雷达，激光雷达可以采用机械式激光雷达也可以采用混合固态或者固态激光雷达。

S120、根据所述第一二维点云数据得到所述目标自主体的第一位姿信息。

在得到第一二维点云数据后可以根据本领域常用的配准算法或者深度学习算法确定目标自主体在第一时刻下的第一位姿信息。例如可以根据NDT(Normal DistributionsTransform，正态分布变换)配准算法匹配第一二维点云数据以得到目标自主体的第一位姿信息。具体的，使用基于NDT配准算法的二维NDT配准模块第一二维点云数据与前一时刻获取到的二维点云数据进行匹配进而确定第一时刻下的第一位姿信息。

S130、预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据。

通常自主体上安装的激光雷达的性能参数是固定已知的，且目标自主体所处的环境也是可以提前通过激光雷达进行扫描得到并预先构建对应的二维栅格地图的。因此在理论上是可以预测出来目标自主体位于任意位姿下时，激光雷达所能够扫描得到理论上的二维点云数据，也即可以预测目标自主体处于第一位姿信息时，激光雷达理论上能够扫描得到的第二二维点云数据。

S140、根据所述所述第一二维点云数据和第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息。

本实施例中，由于NDT配准算法在三维空间能实现高效实时的点云数据配准，且配准精度较好，而在二维空间中少了一个维度，使得算法在不损失精度的情况下，进一步提高了匹配效率，这使得有足够多时间和资源用于优化。在本实施例中，在得到第一位姿信息后，还会对第一位姿信息进行优化，从而利用优化后的具有更高精准度的位姿信息进行建图。

第二二维点云数据是理论上激光雷达在第一时刻位于第一位姿信息时所能够获取到的二维点云数据，而第一二维点云数据是激光雷达在第一时刻实际上所获取到的二维点云数据，因此可以根据实际和理论上的偏差对第一位姿信息进行优化，从而得到一个相对于第一位姿信息更加精准的第二位姿信息。

具体的，使用预设算法结合第一二维点云数据和第二二维点云数据对第一位姿信息进行优化来得到第二位姿信息，其中预设算法可以为基于非线性优化技术的算法，具体为先预测目标自主体处于第一位姿信息时所述激光雷达得到的第二二维点云数据，然后获取第二二维点云数据和第一二维点云数据的误差，并将该误差最小时的第一位姿信息作为第二位姿信息。

S150、根据所述第二位姿信息构建所述目标区域的二维栅格地图。

根据第二位姿信息构建目标区域的二维栅格地图，完成高效低占用的构建高精度二维栅格地图。二维栅格地图的建立是利用激光雷达当前已完成扫描得到的二维点云数据来构建。

本发明实施例获取目标自主体上的激光雷达在第一时刻下扫描得到的目标区域的第一二维点云数据；根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；根据所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；根据所述第二位姿信息构建所述目标区域的二维栅格地图，解决了现有的构建二维栅格地图方法占用计算机资源过高且精度不够的问题，实现了高效低占用的构建高精度二维栅格地图的效果。

实施例二

如图2-图4所示，本发明实施例二提供了一种基于激光雷达的二维栅格地图构建方法，本发明实施例二是在本发明实施例一的基础上进一步的解释说明，如图2所示，该方法包括：

S210、目标自主体上的激光雷达为单线激光雷达时，直接通过所述激光雷达获取第一时刻下目标区域的第一二维点云数据。

S220、目标自主体上的激光雷达为多线激光雷达，确认所述多线激光雷达中预设发射角度的第一激光，并将所述第一激光获取的点云数据根据所述预设发射角度投影至水平面上以获取第一时刻下目标区域的第一二维点云数据。

本实施例中，若目标自主体上的激光雷达为单线激光雷达，则可以直接通过激光雷达获取第一时刻下目标区域的第一二维点云数据，若目标自主体上的激光雷达为多线激光雷达，则需要确认多线激光雷达中预设发射角度的第一激光，并将该第一激光获取的点云数据根据预设发射角度投影至水平面上以获取第一时刻下目标区域的第一二维点云数据，其中预设发射角度由多线激光雷达的自身参数确定，由此实现了无论是单线激光雷达还是多线激光雷达都可以用来构建二维栅格地图。

也即在本实施例中，步骤S210和步骤S220是一个或的关系，也即需要根据激光雷达的类型来选择执行其中一个步骤，在执行完步骤S210或者步骤S220之后，执行步骤S230。

S230、根据所述第一二维点云数据得到所述目标自主体的第一位姿信息。

S240、获取所述激光雷达的性能参数和预先建立的二维环境地图。

激光雷达的性能参数可以包括发射频率、激光束的发射角度以及扫描角度等信息。预先建立的二维环境地图，是指执行本方法中的步骤之前，会先通过激光雷达对整个环境进行扫描，从而预先构建二维环境地图，预先构建的二维环境地图是一个静态地图。二维环境地图可以在建立后直接存储，从而供后续操作过程直接调用。可以理解，当环境中的静态物体发生变化时，会更新二维环境地图。

S250、根据所述性能参数和所述二维环境地图预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据。

本实施例中，首先获取到激光雷达的性能参数和预先建立的二维环境地图，然后就可以根据这些性能参数和二维环境地图预测目标自主体处于第一位姿信息时激光雷达可能得到的第二二维点云数据。也即根据激光雷达的性能参数，可以知晓激光雷达位于第一位姿信息时，其扫描区域，而根据预先建立的二维环境地图则可以知晓对应于该扫描区域的环境分布情况，从而能够确定出来相应环境物体对激光光束进行反射后所形成的点云数据的分布情况，也即得到理论上的第二二维点云数据。

S260、根据所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息。

S270、根据所述第二位姿信息构建所述目标区域的二维栅格地图。

进一步的，如图3所示，本发明实施例二所提供的一种基于激光雷达的二维栅格地图构建方法中，步骤S230可以具体包括：

S231、获取所述激光雷达在第二时刻下扫描得到的目标区域的第三二维点云数据，所述第二时刻为所述第一时刻的前一时刻。

第二时刻为第一时刻的前一时刻，因此激光雷达在第二时刻下扫描得到的目标区域的第三二维点云数据可以直接从激光雷达已获取到的点云数据中进行读取时间戳为第二时刻的二维点云数据作为第三二维点云数据。

S232、获取所述目标自主体第二时刻下的第三位姿信息。

第二时刻下的第三位姿信息可以根据第二时刻下的第三二维点云数据进行确定。通常在第二时刻下的建图过程中，已经会对该第三位姿信息进行求取确定，此时直接读取即可。

S233、获取所述目标自主体第二时刻下的移动速度。

自主体在第二时刻下的移动速度，可以根据第二时刻下获得的第三二维点云数据或者结合第二时刻及其前面至少一帧的点云数据进行确定。移动速度是一个矢量，包括方向和大小。

S234、根据所述移动速度和第三位姿信息预测所述目标自主体第一时刻下的第四位姿信息。

在目标自主体的移动速度已知的情况下，可以根据其速度大致预测出来目标自主体在未来时刻下的位姿信息。因此，根据第二时刻的移动速度和第三位姿信息，则能够大致预测第二时刻下一时刻的第一时刻目标自主体的位姿信息，作为第四位姿信息。

S235、根据所述第四位姿信息利用NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系。

具体地，将第四位姿信息作为NDT算法匹配的初始值来进行第一二维点云数据和第三二维点云数据之间的配准，从而可以极大的减少匹配过程的数据量，提高了匹配效率。

S236、根据所述第一相对位姿关系和第三位姿信息确定所述目标自主体的第一位姿信息。

本实施例中，需要使用基于NDT配准算法的二维NDT配准模块对目标自主体在第一时刻下获取到的第一二维点云数据和在第二时刻下获取到的第三二维点云数据进行配准。示例性的，通过所述激光雷达获取第二时刻下目标区域的第三二维点云数据，第二时刻为第一时刻的前一时刻，然后利用NDT配准算法匹配第一二维点云数据和第三二维点云数据以得到第一时刻和第二时刻的第一相对位姿关系，第二时刻目标自主体的位姿信息已在第二时刻确定，因此可以直接获取目标自主体第二时刻下的第三位姿信息，最后根据第一相对位姿关系和第三位姿信息确定目标自主体的第一位姿信息。其中，如果第一时刻为目标自主体最初运动的时刻，也即不存在前一时刻时，则直接将其作为二维栅格地图的地图数据。

进一步的，如图4所示，本发明实施例二所提供的一种基于激光雷达的二维栅格地图构建方法中，步骤S260可以具体包括：

S261、定义一个映射函数f_i(x)，其中f_i(x)的值表示在位姿信息为x时所述激光雷达理论上扫描得到的第二二维点云数据，x表示位姿信息，i表示确定所述目标自主体位姿信息的次数。

S262、定义一个误差函数e_i(x)，其中e_i(x)＝f_i(x)-z_i，e_i(x)的值表示所述第一二维点云数据和第二二维点云数据之间误差，z_i表示所述第一二维点云数据。

S263、定义一个目标函数F(x)，其中

目标函数F(x)由所述误差函数e_i(x)平方后取其协方差矩阵得到。

本实施例中，利用非线性优化技术结合第一二维点云数据和第一位姿信息来得到第二位姿信息时，首先需要定义i表示确定目标自主体位姿信息的次数，然后定义x为位姿信息，x为一个状态向量，位姿信息包括目标自主体在直角坐标系中x轴和y轴的位置信息和方向信息，因最终目的是建立二维栅格地图，因此不考虑直角坐标系中z轴的位置信息、横滚角信息和俯仰角信息，定义z_i为目标自主体处于第一位姿信息时实际扫描得到的第一二维点云数据，定义f_i(x)为一个非线性映射以表示第三二维点云数据，其中第三二维点云数据为目标自主体处于状态x时，从目标区域预测会扫描得到的点云数据，那么f_i(x)表示目标自主体任一次动作后基于所处于的第一位姿信息从目标区域预测会扫描得到的点云数据，由此定义第一二维点云数据和第三二维点云数据的误差为e_i(x)，其中e_i(x)＝f_i(x)-z_i。

进一步的，通常认为误差服从高斯分布，因此定义该误差e_i(x)的平方为E_i(x)，

再取E_i(x)的协方差矩阵，就可以得到非线性最小二乘的目标函数

求得目标函数F(x)为最小值时对应的x值，就可以确定最优的第一位姿信息了。

S264、将所述第一位姿信息作为初始值x₀。

S265、从所述初始值x₀开始对所述目标函数F(x)进行多次迭代直至F(x_k+1)的值达到极小值，其中x_k+1＝x_k+Δx_k，k表示迭代次数，当Δx_k小于第一阈值时，停止迭代并将此时的x_k作为x_min，并将所述x_min作为第二位姿信息。

本实施例中，为了求得目标函数F(x)为最小值时对应的x值，首先将第一位姿信息作为初始值x₀，由于第一位姿信息是根据激光雷达扫描得到的第一二维点云数据通过配准算法初步得到的位姿信息，将其作为初始值能够减少迭代过程的次数，加速迭代过程的效率。

具体的，首先将e_i(x)进行一阶泰勒展开得到:

e_i(x+Δx)＝e_i(x)+J_i(x)Δx

其中J为雅克比矩阵，其含义为对x的导数。因此可以将目标函数F(x)转化为：

对公式右侧进一步展开并简化可以得到：

因是求解增量Δx，将与Δx无关的量用系数表示后得到：

此时求F(x+Δx)关于Δx的导数并令结果等于0：

进一步简化得到：

Δx^*＝-H^-1b

在上述基础上从初始值x₀开始对目标函数F(x)进行k次迭代，直至F(x_k+Δx_k)，即F(x_k+1)达到极小值，其中x_k+1＝x_k+Δx_k，在目标函数F(x)进行k次迭代中，若Δx_k小于第一阈值，停止迭代并将此时的x_k作为x_min，并将x_min作为第二位姿信息，同时根据第二位姿信息构建目标区域的二维栅格地图，从而得到具有极高精准的地图。

实施例三

如图5所示，本发明实施例三提供了一种基于激光雷达的二维栅格地图构建系统100，本发明实施例三所提供的基于激光雷达的二维栅格地图构建系统100可执行本发明任意实施例所提供的基于激光雷达的二维栅格地图构建方法，具备执行方法相应的功能模块和有益效果。该基于激光雷达的二维栅格地图构建系统100包括数据获取模块200、位姿获取模块300、数据预测模块400、位姿优化模块500和地图构建模块600。

具体的，数据获取模块200用于获取目标自主体上的激光雷达获取在第一时刻下扫描得到的目标区域的第一二维点云数据；位姿获取模块300用于根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；数据预测模块400用于预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；位姿优化模块500用于根据所述第一二维点云数据和所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；地图构建模块600用于根据所述第二位姿信息构建所述目标区域的二维栅格地图。

本实施例中，数据获取模块200具体用于目标自主体上的激光雷达为单线激光雷达时，直接通过所述激光雷达获取第一时刻下目标区域的第一二维点云数据；目标自主体上的激光雷达为多线激光雷达，确认所述多线激光雷达中预设发射角度的第一激光，并将所述第一激光获取的点云数据根据所述预设发射角度投影至水平面上以获取第一时刻下目标区域的第一二维点云数据。

位姿获取模块300具体用于获取所述激光雷达在第二时刻下扫描得到的目标区域的第三二维点云数据，所述第二时刻为所述第一时刻的前一时刻；获取所述目标自主体第二时刻下的第三位姿信息；根据NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系；根据所述第一相对位姿关系和第三位姿信息确定所述目标自主体的第一位姿信息。位姿获取模块300具体还用于获取所述目标自主体第二时刻下的移动速度；根据所述移动速度和第三位姿信息预测所述目标自主体第一时刻下的第四位姿信息；根据所述第四位姿信息利用NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系。

数据预测模块400具体用于获取所述激光雷达的性能参数和预先建立的二维环境地图；根据所述性能参数和所述二维环境地图预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据。

位姿优化模块500具体用于确认所述第一位姿信息与所述第二二维点云数据之间的映射关系；根据所述映射关系对所述第一位姿信息进行调整以获取所述第一二维点云数据和第二二维点云数据之间误差最小时的第二位姿信息。位姿优化模块500具体还用于定义一个映射函数f_i(x)，其中f_i(x)的值表示在位姿信息为x时所述激光雷达理论上扫描得到的第二二维点云数据，x表示位姿信息，i表示确定所述目标自主体位姿信息的次数。位姿优化模块500具体还用于定义一个误差函数e_i(x)，其中e_i(x)＝f_i(x)-z_i，e_i(x)的值表示所述第一二维点云数据和第二二维点云数据之间误差，z_i表示所述第一二维点云数据；定义一个目标函数F(x)，其中

目标函数F(x)由所述误差函数e_i(x)平方后取其协方差矩阵得到；对x进行调整以获取所述目标函数F(x)的值最小时的x_min，并将所述x_min作为第二位姿信息。位姿优化模块500具体还用于将所述第一位姿信息作为初始值x₀；从所述初始值x₀开始对所述目标函数F(x)进行多次迭代直至F(x_k+1)的值达到极小值，其中x_k+1＝x_k+Δx_k，k表示迭代次数，当Δx_k小于第一阈值时，停止迭代并将此时的x_k作为x_min。

实施例四

图6为本发明实施例四提供的一种基于激光雷达的二维栅格地图构建计算机设备的结构示意图。图6示出了适于用来实现本发明实施方式的示例性计算机设备12的框图。图6显示的计算机设备12仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图6所示，计算机设备12以通用计算设备的形式表现。计算机设备12的组件可以包括但不限于：一个或者处理器或者处理单元16，系统存储器28，连接不同系统组件(包括系统存储器28和处理单元16)的总线18。

总线18表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(ISA)总线，微通道体系结构(MAC)总线，增强型ISA总线、视频电子标准协会(VESA)局域总线以及外围组件互连(PCI)总线。

计算机设备12典型地包括多种计算机系统可读介质。这些介质可以是任何能够被计算机设备12访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。

系统存储器28可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(RAM)30和/或高速缓存存储器32。计算机设备12可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例，存储系统34可以用于读写不可移动的、非易失性磁介质(图6未显示，通常称为“硬盘驱动器”)。尽管图6中未示出，可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器，以及对可移动非易失性光盘(例如CD-ROM,DVD-ROM或者其它光介质)读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者数据介质接口与总线18相连。存储器28可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。

具有一组(至少一个)程序模块42的程序/实用工具40，可以存储在例如存储器28中，这样的程序模块42包括——但不限于——操作系统、一个或者应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块42通常执行本发明所描述的实施例中的功能和/或方法。

计算机设备12也可以与一个或外部设备14(例如键盘、指向设备、显示器24等)通信，还可与一个或者使得用户能与该计算机设备12交互的设备通信，和/或与使得该计算机设备12能与一个或其它计算设备进行通信的任何设备(例如网卡，调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口22进行。并且，计算机设备12还可以通过网络适配器20与一个或者网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图所示，网络适配器20通过总线18与计算机设备12的其它模块通信。应当明白，尽管图中未示出，可以结合计算机设备12使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。

处理单元16通过运行存储在系统存储器28中的程序，从而执行各种功能应用以及数据处理，例如实现本发明实施例所提供的方法：

通过目标自主体上的激光雷达获取第一时刻下目标区域的第一二维点云数据；

根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；

预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；

根据所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；

根据所述第二位姿信息构建所述目标区域的二维栅格地图。

实施例五

本发明实施例五还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如本申请所有发明实施例提供的方法：

通过目标自主体上的激光雷达获取第一时刻下目标区域的第一二维点云数据；

根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；

预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；

根据所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；

根据所述第二位姿信息构建所述目标区域的二维栅格地图。

本发明实施例的计算机存储介质，可以采用一个或计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括——但不限于无线、电线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

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

Claims (10)

1.一种基于激光雷达的二维栅格地图构建方法，其特征在于，包括：

获取目标自主体上的激光雷达在第一时刻下扫描得到的目标区域的第一二维点云数据；

根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；

预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；

根据所述第一二维点云数据和所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；

根据所述第二位姿信息构建所述目标区域的二维栅格地图；

所述根据所述第一二维点云数据得到所述目标自主体的第一位姿信息，包括：

将所述第一二维点云数据与所述第一时刻前一时刻的二维点云数据进行匹配，根据匹配结果确定所述目标自主体的第一位姿信息。

2.根据权利要求1所述的方法，其特征在于，所述获取目标自主体上的激光雷达在第一时刻下扫描得到的目标区域的第一二维点云数据包括：

目标自主体上的激光雷达为单线激光雷达时，直接通过所述激光雷达获取第一时刻下目标区域的第一二维点云数据；

目标自主体上的激光雷达为多线激光雷达，确认所述多线激光雷达中预设发射角度的第一激光，并将所述第一激光获取的点云数据根据所述预设发射角度投影至水平面上以获取第一时刻下目标区域的第一二维点云数据。

3.根据权利要求1所述的方法，其特征在于，所述根据所述第一二维点云数据得到所述目标自主体的第一位姿信息包括：

获取所述激光雷达在第二时刻下扫描得到的目标区域的第三二维点云数据，所述第二时刻为所述第一时刻的前一时刻；

获取所述目标自主体第二时刻下的第三位姿信息；

根据NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系；

根据所述第一相对位姿关系和第三位姿信息确定所述目标自主体的第一位姿信息。

4.根据权利要求3所述的方法，其特征在于，所述根据NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系包括：

获取所述目标自主体第二时刻下的移动速度；

根据所述移动速度和第三位姿信息预测所述目标自主体第一时刻下的第四位姿信息；

根据所述第四位姿信息利用NDT算法匹配所述第一二维点云数据和第三二维点云数据以得到所述第一时刻和第二时刻的第一相对位姿关系。

5.根据权利要求1所述的方法，其特征在于，所述预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据包括：

获取所述激光雷达的性能参数和预先建立的二维环境地图；

根据所述性能参数和所述二维环境地图预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据。

6.根据权利要求1所述的方法，其特征在于，所述根据所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息包括：

确认所述第一位姿信息与所述第二二维点云数据之间的映射关系；

根据所述映射关系对所述第一位姿信息进行调整以获取所述第一二维点云数据和第二二维点云数据之间误差最小时的第二位姿信息。

7.根据权利要求6所述的方法，其特征在于，所述确认所述第一位姿信息与所述第二二维点云数据之间的映射关系包括：

定义一个映射函数f_i(x)，其中f_i(x)的值表示在位姿信息为x时所述激光雷达理论上扫描得到的第二二维点云数据，x表示位姿信息，i表示确定所述目标自主体位姿信息的次数。

8.根据权利要求7所述的方法，其特征在于，所述根据所述映射关系对所述第一位姿信息进行调整以获取所述第一二维点云数据和第二二维点云数据之间误差最小时的第二位姿信息包括：

定义一个误差函数e_i(x)，其中e_i(x)＝f_i(x)-z_i，e_i(x)的值表示所述第一二维点云数据和第二二维点云数据之间误差，z_i表示所述第一二维点云数据；

定义一个目标函数F(x)，其中

目标函数F(x)由所述误差函数e_i(x)平方后取其协方差矩阵得到；

对x进行调整以获取所述目标函数F(x)的值最小时的x_min，并将所述x_min作为第二位姿信息。

9.根据权利要求8所述的方法，其特征在于，所述对x进行调整以获取所述目标函数F(x)最小时的X_min包括：

将所述第一位姿信息作为初始值x₀；

从所述初始值x₀开始对所述目标函数F(x)进行多次迭代直至F(x_k+1)的值达到极小值，其中x_k+1＝x_k+Δx_k，k表示迭代次数，当Δx_k小于第一阈值时，停止迭代并将此时的x_k作为x_min。

10.一种基于激光雷达的二维栅格地图构建系统，其特征在于，包括：

数据获取模块，用于获取目标自主体上的激光雷达在第一时刻下扫描得到的目标区域的第一二维点云数据；

位姿获取模块，用于根据所述第一二维点云数据得到所述目标自主体的第一位姿信息；

数据预测模块，用于预测所述目标自主体处于所述第一位姿信息时所述激光雷达得到的第二二维点云数据；

位姿优化模块，用于根据所述第一二维点云数据和所述第二二维点云数据对所述第一位姿信息进行优化以得到第二位姿信息；

地图构建模块，用于根据所述第二位姿信息构建所述目标区域的二维栅格地图；

所述位姿获取模块，具体用于将所述第一二维点云数据与所述第一时刻前一时刻的二维点云数据进行匹配，根据匹配结果确定所述目标自主体的第一位姿信息。

Images