CN112068154A - 一种激光建图定位方法、装置、存储介质及电子设备 - Google Patents

Abstract

本发明公开了一种激光建图定位方法、装置、存储介质及电子设备，包括采用占据栅格地图来进行环境表示；运动跟踪线程中相邻两帧激光雷达进行扫描匹配，计算优化的帧间位姿运动矩阵；当前帧激光雷达数据与占据栅格地图匹配，调整帧间位姿运动矩阵。建图线程中由新插入的关键帧和关键帧队列构建共视帧约束，检测到闭环时，构建闭环帧约束，采用增量式优化位姿因子图更新关键帧队列位姿，实现全局地图的更新。本发明通过两次匹配，确保机器人运动过程中局部建图准确可靠；对于原始位姿图非线性最小二乘优化问题的求解，将图式结构转换为树状结构，并将非线性优化问题的求解转换为最新帧相关的共视帧的增量式求解，大大提高计算效率，减小资源的耗费。

Description

一种激光建图定位方法、装置、存储介质及电子设备

技术领域

本发明涉及定位和建图技术领域，具体涉及一种激光建图定位方法、装置、存储介质及电子设备。

背景技术

激光SLAM(simultaneous localization and mapping，即时定位与地图构建)一般是指装备平面单线激光雷达的轮式移动机器人，通过运动来使用激光雷达扫描周围室内环境进行建图，同时在此地图中估算机器人位姿的技术。所使用的传感器除了激光雷达，还可以使用轮速里程计，惯导陀螺仪等。现有方案一般通过激光扫描匹配来计算帧间位姿变换，再通过粒子滤波进行状态更新迭代，或通过位姿图优化来消除大范围建图过程中的积累误差。

粒子滤波方法采用一系列的粒子来表示机器人的位姿和地图信息，通过里程计信息作为运动先验和扫描匹配激光到地图上的匹配度来推算概率分布函数(proposaldistribution)。同时对粒子根据激光匹配度来进行权重系数的评估，再根据这些评估权重系统的大小，来对于粒子进行筛选，确保雷达数据与地图匹配好的粒子能够在多次迭代筛选中大概率的存活下来，从而完成建图。该方法不足之处在于无法消除建图过程中的累计误差，且通过激光扫描匹配强依赖于里程计信息。一旦里程计信息失效，如出现轮子打滑，空转，胎压不稳，或者机器人被意外推动，挪移，就会出现建图花掉的情形。

位姿图优化方法在局部通过扫描匹配来计算帧间位姿，使用的是相关度匹配和基于高斯牛顿的最小化地图灰度差的方法；全局通过优化整个位姿图来优化整体轨迹，闭环检测使用分支定界加速的相关度匹配和基于高斯牛顿的最小化地图灰度差的方法来构造位姿图的闭环运动约束。该方案可以单独直接使用雷达数据(主要对于雷达数据源质量较高或机器人运动速度较低情形)，也可以使用轮速里程计及/或惯导来提供运动先验，同时使用雷达数据。不足之处在于一方面帧间匹配依然存在错误估算的情况，对于边建图边导航的探索模式中容易导致运动规划控制无法正常进行；另一方面全局位姿图优化过程中所使用的加速分支定界来根据相关度匹配寻找相似子图并计算帧间运动而构建运动约束的位姿图优化方法，对于计算资源的耗费是非常显著的。就算使用多线程计算的方式，计算负载也很难降下来。

发明内容

有鉴于此，本发明实施例提供了一种激光建图定位方法、装置、存储介质及电子设备，以解决现有技术在激光建图定位过程中机器人的运动跟踪过程抗干扰能力不够强问题，以及位姿图优化过程存在非线性优化问题不够高效耗时过多的问题。

本发明提出的技术方案如下：

本发明实施例第一方面提供一种激光建图定位方法，该方法包括：根据获取的第一帧激光雷达数据构建局部的占据栅格地图；根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；根据当前帧激光雷达数据与占据栅格地图进行匹配，对所述优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿；根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图；根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

进一步地，根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵，包括：根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据确定第一帧间位姿运动矩阵；根据基于直线特征的匹配算子构建第一目标函数；将所述第一帧间位姿运动矩阵输入至所述第一目标函数得到第二帧间位姿运动矩阵；根据基于正态分布变换的匹配算子构建第二目标函数；将所述第二帧间位姿运动矩阵输入至所述第二目标函数，计算得到优化的帧间位姿运动矩阵。

进一步地，当前帧激光雷达数据与占据栅格地图进行匹配对所述优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿，包括：根据基于最小灰度差的匹配算子构建第三目标函数；根据优化的帧间位姿运动矩阵、占据栅格地图以及第三目标函数计算得到精确的当前帧位姿。

进一步地，根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图，包括：判断精确的当前帧位姿和当前关键帧的位姿变换与预设阈值的大小；当小于预设阈值时，根据当前帧激光雷达数据更新当前局部地图；当大于预设阈值时，根据当前帧激光雷达数据更新关键帧队列，并重新创建局部地图。

进一步地，根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图，包括：根据新插入的关键帧和关键帧队列构建共视帧约束；当根据新插入的关键帧和关键帧队列检测到闭环时，构建闭环帧约束；根据共视帧约束和闭环帧约束构建的位姿图，进行增量式优化，更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

本发明实施例第二方面提供一种激光建图定位装置，该装置包括：栅格地图构建模块，用于根据获取的第一帧激光雷达数据构建局部的占据栅格地图；初步位姿估算模块，用于根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；精确位姿估算模块，用于根据当前帧激光雷达数据与占据栅格地图进行匹配，对所述优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿；局部地图构建模块，用于根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图；全局地图更新模块，用于根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

进一步地，初步位姿估算模块包括：第一矩阵构建模块，用于根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据确定第一帧间位姿运动矩阵；第一函数构建模块，用于根据基于直线特征的匹配算子构建第一目标函数；第二矩阵构建模块，用于将所述第一帧间位姿运动矩阵输入至所述第一目标函数得到第二帧间位姿运动矩阵；第二函数构建模块，用于根据基于正态分布变换的匹配算子构建第二目标函数；优化模块，用于将所述第二帧间位姿运动矩阵输入至所述第二目标函数，计算得到优化的帧间位姿运动矩阵。

进一步地，全局地图更新模块包括：共视帧构建模块，用于根据新插入的关键帧和关键帧队列构建共视帧约束；闭环帧构建模块，用于当根据新插入的关键帧和关键帧队列检测到闭环时，构建闭环帧约束。全局增量式优化模块，用于根据共视帧约束和闭环帧约束构建的位姿图，进行增量式优化，更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

本发明实施例第三方面提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行如本发明实施例第一方面及第一方面任一项所述的激光建图定位方法。

本发明实施例第四方面提供一种电子设备，包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行如本发明实施例第一方面及第一方面任一项所述的激光建图定位方法。

本发明提供的技术方案，具有如下效果：

本发明实施例提供的激光建图定位方法、装置、存储介质及电子设备，在运动跟踪线程中，将当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；并根据当前帧激光雷达数据与占据栅格地图进行匹配，对优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿。通过两次匹配，可以得到精确的当前帧位姿，确保机器人运动过程中局部建图准确可靠。

本发明实施例提供的激光建图定位方法、装置、存储介质及电子设备，在两次匹配时，采用抗干扰性强的基于直线特征的匹配算子、基于正态分布变换的匹配算子以及基于最小灰度差的匹配算子依次进行计算，能够有效提高帧间激光匹配的准确度，实现了准确的帧间位姿估算，局部建图可靠，抗干扰性能更强，可以为导航的运动规划提供更为健壮的前提保障。

本发明实施例提供的激光建图定位方法、装置、存储介质及电子设备，对于原始位姿图非线性最小二乘优化问题的求解，充分利用了位姿图结构的稀疏性，将图式结构转换为树状结构，并将非线性优化问题的求解转换为最新帧相关的共视帧的增量式求解，从而大大提高计算效率，减小对于计算资源的耗费。

本发明实施例提供的激光建图定位方法、装置、存储介质及电子设备，能够提高全局位姿图优化过程的效率和准确度，对于大范围建图过程的误差累积得到很好消除。这样机器人在大面积室内场景(如办公室，停车场等)建图的时候，可以通过访问曾经到过地方来触发全局闭环位姿图优化功能，从而均匀减小在整个运动轨迹所累积误差，进而对于整体的建图进行更新。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是根据本发明实施例的激光建图定位方法的流程图；

图2是根据本发明另一实施例的激光建图定位方法的流程图；

图3是根据本发明另一实施例的激光建图定位方法的流程图；

图4是根据本发明另一实施例的激光建图定位方法的流程图；

图5是根据本发明实施例的激光建图定位装置的结构框图；

图6是根据本发明另一实施例的激光建图定位装置的结构框图；

图7是根据本发明另一实施例的激光建图定位装置的结构框图；

图8是根据本发明实施例提供的计算机可读存储介质的结构示意图；

图9是根据本发明实施例提供的电子设备的结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供一种激光建图定位方法，如图1所示，该方法包括如下步骤：

步骤S101：根据获取的第一帧激光雷达数据构建局部的占据栅格地图；具体地，在获取第一帧激光雷达数据之前，可以先进行初始化，将帧间位姿运动矩阵初始化为单位阵。其中，帧间位姿运动矩阵描述了相邻两帧激光雷达数据之间相对的旋转和平移的信息，初始化时将帧间位姿运动矩阵设置为单位阵，即帧间位姿运动先验矩阵，意思是没有旋转，没有平移。

在获取第一帧激光雷达数据之后，可以根据该数据构建占据栅格地图，在构建时，可以使用激光雷达数据ray tracking的方法进行实现，即根据该占据栅格中激光束穿透的次数与打到的次数的比率，再和一个阈值进行比较而构建地图。也可以采用其他方式构建，本发明对占据栅格地图构建方式不做限定。

在构建占据栅格地图之后，可以在其中构造地图坐标系，并将第一帧激光雷达数据记录成关键帧激光和上一帧激光，以备后续建图和激光匹配之用。

步骤S102：根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵。

在一实施例中，在采用两帧激光雷达数据进行匹配时，如图2所示，可以采用步骤S201至步骤S205所示的激光建图定位方法实现。

步骤S201：根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据确定第一帧间位姿运动矩阵；具体地，在步骤S101中已经将帧间位姿运动矩阵初始化为单位阵。在运动跟踪线程中，可以不断获取到激光雷达数据，对于获取的当前帧激光雷达数据以及上一帧激光雷达数据，可以分别确定它们相对世界坐标系的位姿，可以由T_w,i+1和T_wi表示，之后可以根据两个位姿确定当前帧和上一帧之间的帧间位姿运动矩阵，具体可以由公式(1)表示。

步骤S202：根据基于直线特征的匹配算子构建第一目标函数；具体地，在确定帧间位姿运动矩阵之后，可以先采用基于直线特征的激光－激光扫描匹配的方法对其进行优化，该匹配方法是一种以点到直线的度量来构造目标函数的迭代最近点(IterativeClosest Point)的激光匹配方法。该方法能够充分使用室内类办公室环境墙壁的直线几何特征，匹配准确。在采用该匹配方法时，可以调用基于直线特征的匹配算子(Point－LineScan Matcher)，该匹配算子会构造公式(2)所示的目标函数进行优化求解。其中{t,θ}为帧间相对变换的平移和旋转，Π(·)为直线上的对应点，{n}为直线的法向量。

{t，θ}＝argmin||n^T·[∏(R(θ)·P+t)-(R(θ)·P+t)]||² 公式(2)

步骤S203：将帧间位姿运动矩阵输入至第一目标函数得到第二帧间位姿运动矩阵；具体地，在调用基于直线特征的匹配算子构造出第一目标函数之后，可以将步骤S201计算得到的第一帧间位姿运动矩阵输入至公式(2)所示的目标函数中进行优化，得到第二帧间位姿运动矩阵。

步骤S204：根据基于正态分布变换的匹配算子构建第二目标函数；具体地，在确定第二帧间位姿运动矩阵之后，可以采用基于正态分布变换(NormalDistributionTransformation)的激光－激光扫描的方法，该方法主要将参考帧激光构造出正态分布的梯度连续的势场，再通过将当前帧按照优化变量帧间相对位移变换到参考帧的坐标系中，就可以按照该势场计算出相应的目标函数值。由于势场梯度连续可导，方便使用非线性优化中通用方法来加以求解出帧间位姿变换。在采用该匹配方法时，可以调用基于正态分布变换的匹配算子(Normal－Distribution－Transformation Scan Matcher)，该算子会将参考雷达数据帧进行网格化划分，每个网格中的点会计算出均值{μ}和方差{Σ}，然后构造出公式(3)所示的目标函数进行求解，其中{t,θ}为帧间相对变换的平移和旋转，exp(·)为指数函数，

步骤S205：将第二帧间位姿运动矩阵输入至所述第二目标函数，计算得到优化的帧间位姿运动矩阵。具体地，在调用基于正态分布变换的匹配算子构造出第二目标函数之后，可以将步骤S203计算得到的第二帧间位姿运动矩阵输入至公式(3)所示的目标函数中进行优化，得到优化的帧间位姿运动矩阵。

步骤S103：根据当前帧激光雷达数据与占据栅格地图进行匹配，对优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿；具体地，在确定优化的帧间位姿运动矩阵后，可以采用金字塔式的激光－地图扫描匹配方法对其进行精细调整。该方法会将参考帧激光数据来构造出多层多分辨率的局部跟踪地图(Multi－Layered Multi－ResolutionMap)。激光地图匹配时可以从低分辨率的地图匹配开始，因为粗略的地图层可以更快速地收敛到帧间相对位姿变换的大概数值，然后使用该计算结果到更为精细的地图层来进行匹配，进而得到精细调整帧间位姿估算值。

其中，在采用该匹配方法时，可以调用基于最小灰度差的匹配算子，该算子的特点在于将参考帧雷达构成分辨率成倍数增长的参考地图，然后按照由粗略到精细的过程，先在分辨率较大的地图上，大范围来搜索靠近准确值的帧间相对变换，然后在此基础之上，在分辨率较小的精细地图层上来继续匹配，得到精细调整过的更加准确解。在每一层地图上面进行匹配的时候，都会构造公式(4)所示的目标函数进行优化求解，其中{t,θ}为帧间相对变换的平移和旋转，M(·)为地图灰度值函数，它可以将输入的空间坐标，先投影到地图平面得到像素点，在按照这个像素点的位置来获取地图灰度数值。

{t，θ}＝argmin||1-M(R(θ)·P^k+t)||² 公式(4)

在构造出公式(4)所示的目标函数后，可以将优化的帧间位姿运动矩阵输入至该目标函数中，通过将激光数据分别与由粗到精多分辨率地图的匹配(Coarse－to－FineMulti－Resolution Scan Map Matching)，来进一步细化调整当前帧雷达的位姿。由此可以计算出精确的当前帧位姿。

步骤S104：根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图。具体地，当确定精确的当前帧位姿后，可以计算出相对于上一帧激光的帧间相对位姿，以此作为后续位姿跟踪的帧间相对位姿变换的运动先验(假设常速运动模型，在没有新激光数据输入时，速度会逐渐降为0)。此外，还可以根据精确的当前帧位姿和当前关键帧的位姿变换与预设阈值的大小确定局部地图。当小于预设阈值时，根据当前帧激光雷达数据更新当前局部地图；当大于预设阈值时，根据当前帧激光雷达数据更新关键帧队列，并重新创建局部地图。

其中，关键帧队列中的各个关键帧是彼此之间空间距离超过事先设定阈值的雷达位姿帧。这些关键帧可以反映出雷达坐标系在空间运动的里程碑式历史轨迹。当精确的当前帧位姿和当前关键帧的位姿变换小于预设阈值时，说明当前帧激光与当前关键帧激光之间的空间距离较小，此时可以对当前关键帧周围的地图进行更新。

当精确的当前帧位姿和当前关键帧的位姿变换大于预设阈值时，说明当前帧激光与当前关键帧激光之间的空间距离较大，可以将此时的当前帧激光作为新的关键帧插入到关键帧队列中。同时，局部地图中通常是由一个关键帧和它周围小于预设阈值的激光雷达数据构成，当关键帧队列更新后，可以以新插入的关键帧重新创建局部地图。前一个局部地图可以保存在后台。

步骤S105：根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

在一实施例中，在构建运动约束并进行增量式优化时，如图3所示，可以采用步骤S301至步骤S303所示的方法实现。

步骤S301：根据新插入的关键帧和关键帧队列构建共视帧约束；具体地，对于形成的关键帧队列，可以完成建图线程。建图是构建包含所有关键帧信息的整张地图。建图线程会将关键帧队列中的每一帧关键帧位姿构造成因子图(Factor Graph)中一个节点(Node)，两个关键帧之间的运动约束来构造成因子图中一个因子(Factor)。如果两个关键帧在时序上直接相连，则可以称为连续帧约束(Consecutive Constraints)；如果两个关键帧在时序上没有直接相连但是时序上相差不大(如没有超过10帧)，则可以称为共视帧约束(Co－visibility Constraints)；如果两个关键帧在时序上没有直接相连且相差较大(超过10帧)但是空间上比较靠近，则可以称为闭环帧约束(Loop－Closure Constraints)。

当在关键帧队列中插入新的关键帧后，可以根据空间几何约束，来构建与新插入关键帧有共视关系的运动约束。具体可以通过激光－激光扫描匹配计算出相应的帧间相对位姿，从而在新插入的关键帧附近一定阈值半径的圆内筛选出共视帧。该激光－激光扫描匹配计算可以调用上述的匹配算子进行计算。

步骤S302：当根据新插入的关键帧和关键帧队列检测到闭环时，构建闭环帧约束；具体地，在通过激光－激光扫描匹配计算相应的帧间相对位姿的过程中，在检测到闭环时，可以构建闭环帧约束。该闭环检测方式可以采用现有的检测闭环方法实现。

步骤S303：根据共视帧约束和闭环帧约束构建的位姿图，进行增量式优化，更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。具体地，在构建新的运动约束后，可以扩展当前的位姿因子图，因子图更新后可以调用增量式优化的功能来更新关键帧队列中各个关键帧的位姿，从而使得整张地图得到更新。这张全局地图，同时也是导航的路径规划来构建成本地图的静态地图层数据源，从而为路径规划提供可靠保障。

其中，增量式优化是指只优化与最新帧临近的部分帧位姿，没有对所有位姿进行优化。而且优化的过程会将图结构简化成树状结构，会提高计算效率。对于增量式因子图和传统的位姿图优化方法一样，使用机器人的位姿来构造图节点(Node)，使用位姿间的相对变换来构造运动约束(Constraint)，从而构造出位姿图优化的最小二乘问题。但是不同之处在于其一增量式因子图方法使用直接法来求解非线性优化问题，并且通过矩阵的QR分解来对方程求解进行提速，其二将图状结构变换为树状结构，并且针对最新帧附近新添加的约束，有选择性的进行优化和位姿更新，从而减少了计算量。

本发明实施例提供的激光建图定位方法，在运动跟踪线程中，将当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；并根据当前帧激光雷达数据与占据栅格地图进行匹配对优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿。通过两次匹配，可以得到精确的当前帧位姿，确保机器人运动过程中局部建图准确可靠。

本发明实施例提供的激光建图定位方法，在两次匹配时，采用抗干扰性强的基于直线特征的匹配算子、基于正态分布变换的匹配算子以及基于最小灰度差的匹配算子依次进行计算，能够有效提高帧间激光匹配的准确度，实现了准确的帧间位姿估算，局部建图可靠，抗干扰性能更强，可以为导航的运动规划提供更为健壮的前提保障。

本发明实施例提供的激光建图定位方法，对于原始位姿图非线性最小二乘优化问题的求解，充分利用了位姿图结构的稀疏性，将图式结构转换为树状结构，并将非线性优化问题的求解转换为最新帧相关的共视帧的增量式求解，从而大大提高计算效率，减小对于计算资源的耗费。

本发明实施例提供的激光建图定位方法，能够提高全局位姿图优化过程的效率和准确度，对于大范围建图过程的误差累积得到很好消除。这样机器人在大面积室内场景(如办公室，停车场等)建图的时候，可以通过访问曾经到过地方来触发全局闭环位姿图优化功能，从而均匀减小在整个运动轨迹所累积误差，进而对于整体的建图进行更新。

在一实施例中，如图4所示，本发明实施例提供的激光建图定位方法可以按照以下流程实现。该激光建图定位方法分为运动跟踪和地图构建两个线程：运动跟踪线程是系统的主线程，建图线程辅助运动跟踪线程的正常工作，同时依赖于运动跟踪线程的计算结果。

在运动跟踪线程中，对于当前帧激光雷达数据可以与上一帧激光雷达数据采用基于直线特征的帧间激光匹配和基于正态分布变换的帧间激光匹配得到优化的帧间位姿运动矩阵，之后采用基于最小二乘灰度差的激光地图匹配确定精确的当前帧位姿。根据精确的当前帧位姿和当前关键帧可以确定局部地图。具体通过判断精确的当前帧位姿和当前关键帧的位姿变换与预设阈值的大小，当小于预设阈值时，根据当前帧激光雷达数据更新当前局部地图；当大于预设阈值时，根据当前帧激光雷达数据更新关键帧队列，并重新创建局部地图。

在建图线程中，可以根据新插入的关键帧和关键帧队列构建共视帧约束，之后采用增量式优化位姿因子图更新关键帧队列位姿。在此过程中，当检测到闭环时，可以构建闭环帧约束，并通过增量式优化更新关键帧队列中位姿，从而实现全局地图的更新，完成全局地图的创建。

本发明实施例还提供一种激光建图定位装置，如图5所示，该装置包括：

栅格地图构建模块10，用于根据获取的第一帧激光雷达数据构建局部的占据栅格地图；详细内容参见上述方法实施例中步骤S101的相关描述。

初步位姿估算模块20，用于根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；详细内容参见上述方法实施例中步骤S102的相关描述。

精确位姿估算模块30，用于根据当前帧激光雷达数据与占据栅格地图进行匹配，对优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿；详细内容参见上述方法实施例中步骤S103的相关描述。

局部地图构建模块40，用于根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图。详细内容参见上述方法实施例中步骤S104的相关描述。

全局地图更新模块50，用于根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。详细内容参见上述方法实施例中步骤S105的相关描述。

本发明实施例提供的激光建图定位装置，在运动跟踪线程中，将当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；并根据当前帧激光雷达数据与占据栅格地图进行匹配对优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿。通过两次匹配，可以得到精确的当前帧位姿，确保机器人运动过程中局部建图准确可靠。

本发明实施例提供的激光建图定位装置，在两次匹配时，采用抗干扰性强的基于直线特征的匹配算子、基于正态分布变换的匹配算子以及基于最小灰度差的匹配算子依次进行计算，能够有效提高帧间激光匹配的准确度，实现了准确的帧间位姿估算，局部建图可靠，抗干扰性能更强，可以为导航的运动规划提供更为健壮的前提保障。

本发明实施例提供的激光建图定位装置，对于原始位姿图非线性最小二乘优化问题的求解，充分利用了位姿图结构的稀疏性，将图式结构转换为树状结构，并将非线性优化问题的求解转换为最新帧相关的共视帧的增量式求解，从而大大提高计算效率，减小对于计算资源的耗费。

本发明实施例提供的激光建图定位装置，能够提高全局位姿图优化过程的效率和准确度，对于大范围建图过程的误差累积得到很好消除。这样机器人在大面积室内场景(如办公室，停车场等)建图的时候，可以通过访问曾经到过地方来触发全局闭环位姿图优化功能，从而均匀减小在整个运动轨迹所累积误差，进而对于整体的建图进行更新。

在一实施例中，如图6所示，初步位姿估算模块20包括：

第一矩阵构建模块21，用于根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据确定第一帧间位姿运动矩阵；详细内容参见上述方法实施例中步骤S201的相关描述。

第一函数构建模块22，用于根据基于直线特征的匹配算子构建第一目标函数；详细内容参见上述方法实施例中步骤S202的相关描述。

第二矩阵构建模块23，用于将所述第一帧间位姿运动矩阵输入至所述第一目标函数得到第二帧间位姿运动矩阵；详细内容参见上述方法实施例中步骤S203的相关描述。

第二函数构建模块24，用于根据基于正态分布变换的匹配算子构建第二目标函数；详细内容参见上述方法实施例中步骤S204的相关描述。

优化模块25，用于将所述第二帧间位姿运动矩阵输入至所述第二目标函数，计算得到优化的帧间位姿运动矩阵。详细内容参见上述方法实施例中步骤S205的相关描述。

在一实施例中，如图7所示，全局地图更新模块50包括：

共视帧构建模块31，用于根据新插入的关键帧和关键帧队列构建共视帧约束；详细内容参见上述方法实施例中步骤S301的相关描述。

闭环帧构建模块32，用于当根据新插入的关键帧和关键帧队列检测到闭环时，构建闭环帧约束。详细内容参见上述方法实施例中步骤S302的相关描述。

全局增量式优化模块33，用于根据共视帧约束和闭环帧约束构建的位姿图，进行增量式优化，更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。详细内容参见上述方法实施例中步骤S303的相关描述。

本发明实施例提供的激光建图定位修复装置的功能描述详细参见上述实施例中激光建图定位方法描述。

本发明实施例还提供一种存储介质，如图8所示，其上存储有计算机程序601，该指令被处理器执行时实现上述实施例中激光建图定位方法的步骤。该存储介质上还存储有音视频流数据，特征帧数据、交互请求信令、加密数据以及预设数据大小等。其中，存储介质可为磁碟、光盘、只读存储记忆体(Read－Only Memory，ROM)、随机存储记忆体(RandomAccess Memory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固态硬盘(Solid－State Drive，SSD)等；所述存储介质还可以包括上述种类的存储器的组合。

本领域技术人员可以理解，实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述存储介质可为磁碟、光盘、只读存储记忆体(Read－Only Memory，ROM)、随机存储记忆体(Random AccessMemory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固态硬盘(Solid－State Drive，SSD)等；所述存储介质还可以包括上述种类的存储器的组合。

本发明实施例还提供了一种电子设备，如图9所示，该电子设备可以包括处理器51和存储器52，其中处理器51和存储器52可以通过总线或者其他方式连接，图9中以通过总线连接为例。

处理器51可以为中央处理器(Central Processing Unit，CPU)。处理器51还可以为其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field－Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片，或者上述各类芯片的组合。

存储器52作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块，如本发明实施例中的对应的程序指令/模块。处理器51通过运行存储在存储器52中的非暂态软件程序、指令以及模块，从而执行处理器的各种功能应用以及数据处理，即实现上述方法实施例中的激光建图定位方法。

存储器52可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储处理器51所创建的数据等。此外，存储器52可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器52可选包括相对于处理器51远程设置的存储器，这些远程存储器可以通过网络连接至处理器51。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

所述一个或者多个模块存储在所述存储器52中，当被所述处理器51执行时，执行如图1－4所示实施例中的激光建图定位方法。

上述电子设备具体细节可以对应参阅图1至图4所示的实施例中对应的相关描述和效果进行理解，此处不再赘述。

虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims (10)

1.一种激光建图定位方法，其特征在于，包括：

根据获取的第一帧激光雷达数据构建局部的占据栅格地图；

根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；

根据当前帧激光雷达数据与占据栅格地图进行匹配，对所述优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿；

根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图；

根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

2.根据权利要求1所述的激光建图定位方法，其特征在于，根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵，包括：

根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据确定第一帧间位姿运动矩阵；

根据基于直线特征的匹配算子构建第一目标函数；

将所述第一帧间位姿运动矩阵输入至所述第一目标函数得到第二帧间位姿运动矩阵；

根据基于正态分布变换的匹配算子构建第二目标函数；

将所述第二帧间位姿运动矩阵输入至所述第二目标函数，计算得到优化的帧间位姿运动矩阵。

3.根据权利要求1所述的激光建图定位方法，其特征在于，当前帧激光雷达数据与占据栅格地图进行匹配，对所述优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿，包括：

根据基于最小灰度差的匹配算子构建第三目标函数；

根据优化的帧间位姿运动矩阵、占据栅格地图以及第三目标函数计算得到精确的当前帧位姿。

4.根据权利要求1所述的激光建图定位方法，其特征在于，根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图，包括：

判断精确的当前帧位姿和当前关键帧的位姿变换与预设阈值的大小；

当小于预设阈值时，根据当前帧激光雷达数据更新当前局部地图；

当大于预设阈值时，根据当前帧激光雷达数据更新关键帧队列，并重新创建局部地图。

5.根据权利要求1所述的激光建图定位方法，其特征在于，根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图，包括：

根据新插入的关键帧和关键帧队列构建共视帧约束；

当根据新插入的关键帧和关键帧队列检测到闭环时，构建闭环帧约束；

根据共视帧约束和闭环帧约束构建的位姿图，进行增量式优化，更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

6.一种激光建图定位装置，其特征在于，包括：

栅格地图构建模块，用于根据获取的第一帧激光雷达数据构建局部的占据栅格地图；

初步位姿估算模块，用于根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据进行匹配，计算得到优化的帧间位姿运动矩阵；

精确位姿估算模块，用于根据当前帧激光雷达数据与占据栅格地图进行匹配，对所述优化的帧间位姿运动矩阵进行调整，得到精确的当前帧位姿；

局部地图构建模块，用于根据当前帧和当前关键帧之间的距离更新局部地图或重新构建局部地图；

全局地图更新模块，用于根据新插入的关键帧构建的运动约束，增量式优化更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

7.根据权利要求6所述的激光建图定位装置，其特征在于，初步位姿估算模块包括：

第一矩阵构建模块，用于根据运动跟踪线程中当前帧激光雷达数据与上一帧激光雷达数据确定第一帧间位姿运动矩阵；

第一函数构建模块，用于根据基于直线特征的匹配算子构建第一目标函数；

第二矩阵构建模块，用于将所述第一帧间位姿运动矩阵输入至所述第一目标函数得到第二帧间位姿运动矩阵；

第二函数构建模块，用于根据基于正态分布变换的匹配算子构建第二目标函数；

优化模块，用于将所述第二帧间位姿运动矩阵输入至所述第二目标函数，计算得到优化的帧间位姿运动矩阵。

8.根据权利要求6所述的激光建图定位装置，其特征在于，全局地图更新模块包括：

共视帧构建模块，用于根据新插入的关键帧和关键帧队列构建共视帧约束；

闭环帧构建模块，用于当根据新插入的关键帧和关键帧队列检测到闭环时，构建闭环帧约束。

全局增量式优化模块，用于根据共视帧约束和闭环帧约束构建的位姿图，进行增量式优化，更新关键帧队列中各个关键帧的位姿，得到更新后的全局地图。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行如权利要求1－5任一项所述的激光建图定位方法。

10.一种电子设备，其特征在于，包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行如权利要求1－5任一项所述的激光建图定位方法。

Images