CN113168717B

CN113168717B - 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达

Info

Publication number: CN113168717B
Application number: CN202180000478.2A
Authority: CN
Inventors: 赵小文
Original assignee: Huawei Technologies Co Ltd
Current assignee: Huawei Technologies Co Ltd
Priority date: 2021-03-11
Filing date: 2021-03-11
Publication date: 2023-03-03
Anticipated expiration: 2041-03-11
Also published as: WO2022188094A1; CN113168717A

Abstract

本申请提供了一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达，该点云匹配方法包括：获取激光雷达采集到的两帧点云数据，所述两帧点云数据包括前后的第一帧点云和第二帧点云；对所述第一帧点云进行聚类，得到所述第一帧点云中各物体的点云簇；对所述第二帧点云进行聚类，得到所述第二帧点云中各物体的点云簇；将所述第二帧点云中各物体的点云簇与所述第一帧点云中各物体的点云簇进行关联；将所述第二帧点云与所述第一帧点云进行点云匹配，得到所述第二帧点云与所述第一帧点云之间的位姿变换参数。本申请具有更快的点云匹配速度和更高的点云匹配精度，提高同步定位与建图SLAM的速度和精度。

Description

一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达

技术领域

本申请涉及定位技术领域，特别涉及一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达。

背景技术

随着移动机器人、智能驾驶技术的发展，在生活服务，工业运输，抗险救灾等领域，同步定位与建图(Simultaneous Localization and Mapping，SLAM)技术逐渐成为最基础、最核心的技术，其原理是利用激光雷达或视觉传感器观测信息进行位置估计，同时将点云或者图像拼接到一起构成完整的地图。当机器人探索未知环境时，利用SLAM技术实时定位并且构建环境地图，是完成后续路径规划与导航任务的重要前提。可以说，SLAM技术对于移动机器人以及智能驾驶技术的发展是至关重要的。虽然在结构化的室内场景中SLAM技术已经得到成熟的应用，但是在室外场景中，由于场景特征相比室内较少，SLAM技术仍然存在较大的困难与挑战，在执行定位与建图的过程中，位姿估计的精度和地图的质量都有待进一步提高。

传统的SLAM方法主要分为激光SLAM与视觉SLAM，激光SLAM使用激光雷达获取点云进行匹配，估计激光雷达的位姿并构建地图，视觉SLAM使用视觉传感器获取图像，并在图像中进行特征点提取与匹配，从而估计视觉传感器的位姿并构建地图。由于视觉SLAM容易受到环境光线变化的影响，尤其在室外环境中，光线变化会直接影响特征点的提取精度，导致建图效果不佳，因此，视觉SLAM在室外环境中的整体表现不如激光SLAM。而激光SLAM在特征稀疏的场景中，容易丢失跟踪目标，导致点云匹配出现失误，从而产生误差，这些误差会逐渐累积，导致最终地图出现重叠的问题，并且，激光SLAM在动态环境中，动态物体也容易对点云匹配产生影响，无法估计正确的位姿。

发明内容

有鉴于此，本申请提供了一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达，具有更快的点云匹配速度和更高的点云匹配精度，提高同步定位与建图SLAM的速度和精度。

为达到上述目的，本申请的第一方面提供一种点云匹配方法，包括：

获取激光雷达采集到的两帧点云数据，所述两帧点云数据包括前后的第一帧点云和第二帧点云；

对所述第一帧点云进行聚类，得到所述第一帧点云中各物体的点云簇；

对所述第二帧点云进行聚类，得到所述第二帧点云中各物体的点云簇；

基于点云簇的相似度，将所述第二帧点云中各物体的点云簇与所述第一帧点云中各物体的点云簇进行关联；

基于所述关联的结果及所述第二帧点云与所述第一帧点云中的各个点的距离，将所述第二帧点云与所述第一帧点云进行点云匹配，得到所述第二帧点云与所述第一帧点云之间的位姿变换参数。

由上，通过本方法，对激光雷达采集的前后两帧点云进行聚类、分割，以得到该两帧点云中各物体的点云簇，通过该两帧点云中各物体的点云簇进行关联计算，得到该两帧点云中各物体的点云簇的相似度，将该两帧点云中相似度最高的物体的点云簇进行关联，基于该关联的结果，对该两帧点云进行相互关联的各物体的点云簇的匹配和该两帧点云中各个点的匹配，即可得到该两帧点云之间的位姿变换参数。利用本方法，可实现速度更快、精度更高的点云匹配，得到精确度更高的位姿变换参数，从而提高同步定位与建图的精度。

在第一方面的一种可能的实现方式中，还包括：

基于所述第二帧点云与所述第一帧点云的位姿变换参数，将所述第二帧点云进行位姿变换；

将位姿变换后的第二帧点云与所述第一帧点云进行融合，得到所述第二帧点云与第一帧点云组成的点云地图。

由上，基于得到的第二帧点云与第一帧点云的位姿变换参数，可将第二帧点云进行位姿变换，并将位姿变换后的第二帧点云与第一帧点云进行融合，同时还可以结合地图坐标系，将融合后的第二帧点云与第一帧点云构建到地图坐标系中，得到点云地图。

在第一方面的一种可能的实现方式中，所述基于点云簇的相似度，将所述第二帧点云中各物体的点云簇与所述第一帧点云中各物体的点云簇进行关联，包括：

获取所述第一帧点云中各物体的点云簇的特征向量；

获取所述第二帧点云中各物体的点云簇的特征向量；

基于所述第二帧点云中各物体的点云簇的特征向量与所述第一帧点云中各物体的点云簇的特征向量计算相似度，将所述第二帧点云中各物体的点云簇与所述第一帧点云中各物体的点云簇进行基于所述相似度的关联。

由上，基于聚类得到的两帧点云中各物体的点云簇，获取每个点云簇的特征向量，通过计算第二帧点云中各物体的点云簇的特征向量与第一帧点云中各物体的点云簇的特征向量的关联距离，可得到第二帧点云中各物体的点云簇与第一帧点云中各物体的点云簇的相似度，基于相似度，可将第二帧点云中各物体的点云簇与第一帧点云中各物体的点云簇进行关联。

在第一方面的一种可能的实现方式中，所述点云簇的特征向量包括至少以下之一：

所述点云簇的位置参数、朝向参数或尺寸参数。

由上，通过获取点云簇的中心坐标、水平航向角、长宽高等参数，可确定该点云簇的位置参数、朝向参数及尺寸参数，以形成每个点云簇的特征点，便于快速完成第二帧点云中各物体的点云簇与第一帧点云中各物体的点云簇的两两关联计算，并基于两两关联计算的结果进行点云簇的关联。

在第一方面的一种可能的实现方式中，所述基于所述关联的结果及所述第二帧点云与所述第一帧点云中的各个点的距离，将所述第二帧点云与所述第一帧点云进行点云匹配，包括：

基于所述关联的结果，计算经过位姿变换后的所述第二帧点云中的各物体的点云簇与所述第一帧点云中各物体的点云簇的物体类别误差；

计算经过所述位姿变换后的所述第二帧点云中各个点与所述第一帧点云中距离最近的点的距离误差；

对所述物体类别误差和所述距离误差进行加权求和，将所述加权求和的结果最小作为约束条件，计算所述第二帧点云与所述第一帧点云之间的位姿变换参数。

由上，基于第二帧点云中的每个点到该第二帧点云中的每个物体的点云簇的距离，可得到该第二帧点云中的每个点聚类属于该第二帧点云中的每个物体的点云簇的概率，基于关联的结果，可进一步计算得到第二帧点云中的每个点经过位姿变换后聚类属于第一帧点云中的每个物体的点云簇的概率，从而可以得到第二帧点云中的所有点经过位姿变换后聚类属于第一帧点云中的每个物体的点云簇的整体概率，该整体概率可以作为经过位姿变换后的第二帧点云中的各物体的点云簇与第一帧点云中各物体的点云簇的物体类别误差。同时，基于迭代最近点(Iterative Closest Point，ICP)算法，可计算经过位姿变换后的第二帧点云中的各个点与第一帧点云中各个点的距离误差，通过对该物体类别误差和距离误差分别设置一权重系数，并进行求和计算，将加权求和的结果最小作为约束条件，即可计算第二帧点云与第一帧点云之间的位姿变换参数。本方法基于物体类别误差和距离误差进行两帧点云之间的点云匹配，与现有的只基于距离误差进行点云匹配相比，具有更快的点云匹配速度和更高的点云匹配精度，可得到更高精度的位姿变换参数。

在第一方面的一种可能的实现方式中，所述进行点云匹配之前，还包括：

滤除所述第一帧点云中对应动态物体的点云簇；和/或

滤除所述第二帧点云中对应动态物体的点云簇。

由上，在复杂的动态环境中，动态的物体会造成两帧点云之间存在较大的差异，导致点云匹配失败，无法获取精确的位姿变换参数，因此在进行点云匹配之前，可对同一物体的点云簇在连续的多帧点云中的速度进行观测和估计，从而判断该物体是否是动态物体，并将对应动态物体的点云簇从每帧点云中滤除，以消除动态物体对点云匹配造成的影响。

在第一方面的一种可能的实现方式中，还包括：

对所述第一帧点云进行闭环检测；和/或

对所述第二帧点云进行闭环检测；

基于所述闭环检测的结果，对所述第二帧点云与所述第一帧点云之间的位姿变换参数进行优化。

由上，基于闭环检测算法，在帧数据库中分别查找与第一帧点云、第二帧点云最接近的帧，分别构成闭环，然后将构成闭环的最接近的帧的局部特征点分别投影到第一帧点云、第二帧点云并最小化重投影误差，以对第一帧点云、第二帧点云的位姿进行优化，从而对第二帧点云与第一帧点云之间的位姿变换参数进行优化。

在第一方面的一种可能的实现方式中，所述进行聚类之前，还包括：

滤除所述第一帧点云中的噪声点；和/或

滤除所述第二帧点云中的噪声点。

由上，通过对采集的第一帧点云、第二帧点云进行滤波，减少第一帧点云、第二帧点云中的噪声点，同时保留点云的形状特征，减少后续处理的计算量。

为达到上述目的，本申请的第二方面提供一种点云匹配装置，包括：

获取模块，用于获取激光雷达采集到的两帧点云数据，所述两帧点云数据包括前后的第一帧点云和第二帧点云；

聚类模块，用于对所述第一帧点云进行聚类，得到所述第一帧点云中各物体的点云簇；对所述第二帧点云进行聚类，得到所述第二帧点云中各物体的点云簇；

关联模块，用于基于点云簇的相似度，将所述第二帧点云中各物体的点云簇与所述第一帧点云中各物体的点云簇进行关联；

点云匹配模块，用于基于所述关联的结果及所述第二帧点云与所述第一帧点云中的各个点的距离，将所述第二帧点云与所述第一帧点云进行点云匹配，得到所述第二帧点云与所述第一帧点云之间的位姿变换参数。

在第二方面的一种可能的实现方式中，还包括：

地图构建模块，用于基于所述第二帧点云与所述第一帧点云的位姿变换参数，将所述第二帧点云进行位姿变换；将位姿变换后的第二帧点云与所述第一帧点云进行融合，得到所述第二帧点云与第一帧点云组成的点云地图。

在第二方面的一种可能的实现方式中，所述关联模块具体用于：

获取所述第一帧点云中各物体的点云簇的特征向量；

获取所述第二帧点云中各物体的点云簇的特征向量；

在第二方面的一种可能的实现方式中，所述点云簇的特征向量包括至少以下之一：

所述点云簇的位置参数、朝向参数或尺寸参数。

在第二方面的一种可能的实现方式中，所述点云匹配模块具体用于：

在第二方面的一种可能的实现方式中，所述进行点云匹配之前，还包括动态滤除模块：

用于滤除所述第一帧点云中对应动态物体的点云簇；和/或

用于滤除所述第二帧点云中对应动态物体的点云簇。

在第二方面的一种可能的实现方式中，还包括闭环优化模块：

用于对所述第一帧点云进行闭环检测；和/或

用于对所述第二帧点云进行闭环检测；

在第二方面的一种可能的实现方式中，所述获取模块还用于：

滤除所述第一帧点云中的噪声点；和/或

滤除所述第二帧点云中的噪声点。

为达到上述目的，本申请的第三方面提供一种导航方法，包括：

基于所述点云匹配方法得到获取的每帧点云与前一帧点云的位姿变换参数；

基于所述位姿变换参数，对所述每帧点云进行变换，并与前一帧点云进行融合，得到包含所述每帧点云的点云地图；

基于所述点云地图进行路径规划和动态障碍物避障。

为达到上述目的，本申请的第四方面提供一种导航设备，包括：

所述点云匹配装置，用于得到获取的每帧点云与前一帧点云的位姿变换参数；基于所述位姿变换参数，对所述每帧点云进行变换，并与前一帧点云进行融合，得到包含所述每帧点云的点云地图；

导航模块，用于基于所述点云地图进行路径规划和动态障碍物避障。

为达到上述目的，本申请的第五方面提供一种定位方法，包括：

基于所述点云匹配方法，得到激光雷达的当前时刻与前一时刻的位姿变换参数；

基于所述位姿变换参数、所述激光雷达的前一时刻的位姿，确定所述激光雷达的当前时刻的位姿。

为达到上述目的，本申请的第六方面提供一种激光雷达，包括：

所述点云匹配装置，用于得到激光雷达的当前时刻与前一时刻的位姿变换参数；

定位模块，用于基于所述位姿变换参数、所述激光雷达的前一时刻的位姿，确定所述激光雷达的当前时刻的位姿。

为达到上述目的，本申请的第七方面提供一种计算设备，包括：

总线；

通信接口，其与所述总线连接；

至少一个处理器，其与所述总线连接；以及

至少一个存储器，其与所述总线连接并存储有程序指令，所述程序指令当被所述至少一个处理器执行时使得所述至少一个处理器执行所述点云匹配方法、或所述导航方法、或所述定位方法。

为达到上述目的，本申请的第八方面提供一种计算机可读存储介质，其上存储有程序指令，所述程序指令当被计算机执行时使得所述计算机执行点云匹配方法、或所述导航方法、或所述定位方法。

为达到上述目的，本申请的第九方面提供一种计算机程序产品，其包括有程序指令，所述程序指令当被计算机执行时使得所述计算机执行点云匹配方法、或所述导航方法、或所述定位方法。

本申请的这些和其它方面在以下(多个)实施例的描述中会更加简明易懂。

附图说明

以下参照附图来进一步说明本申请的各个特征和各个特征之间的联系。附图均为示例性的，一些特征并不以实际比例示出，并且一些附图中可能省略了本申请所涉及领域的惯常的且对于本申请非必要的特征，或是额外示出了对于本申请非必要的特征，附图所示的各个特征的组合并不用以限制本申请。另外，在本说明书全文中，相同的附图标记所指代的内容也是相同的。具体的附图说明如下：

图1为现有的一种视觉SLAM方法的流程图；

图2为现有的一种激光SLAM方法的流程图；

图3为两帧激光点云之间的距离约束的示意图；

图4为本申请实施例提供的一种SLAM方法的流程图；

图5为本申请实施例的两帧点云中的多个点云簇之间建立关联的示意图；

图6为本申请实施例提供的一种动态环境中的SLAM方法的流程图；

图7为本申请实施例的两帧点云中的多个点云簇之间建立关联的示意图；

图8为本申请实施例提供的一种静态环境中的SLAM方法的流程图；

图9为本申请实施例提供的一种SLAM装置的模块图；

图10为本申请实施例提供的一种计算设备的架构图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

说明书和权利要求书中的词语“第一、第二、第三等”或模块A、模块B、模块C等类似用语，仅用于区别类似的对象，不代表针对对象的特定排序，可以理解地，在允许的情况下可以互换特定的顺序或先后次序，以使这里描述的本申请实施例能够以除了在这里图示或描述的以外的顺序实施。

在以下的描述中，所涉及的表示步骤的标号，如S110、S120……等，并不表示一定会按此步骤执行，在允许的情况下可以互换前后步骤的顺序，或同时执行。

说明书和权利要求书中使用的术语“包括”不应解释为限制于其后列出的内容；它不排除其它的元件或步骤。因此，其应当诠释为指定所提到的所述特征、整体、步骤或部件的存在，但并不排除存在或添加一个或更多其它特征、整体、步骤或部件及其组群。因此，表述“包括装置A和B的设备”不应局限为仅由部件A和B组成的设备。

本说明书中提到的“一个实施例”或“实施例”意味着与该实施例结合描述的特定特征、结构或特性包括在本申请的至少一个实施例中。因此，在本说明书各处出现的用语“在一个实施例中”或“在实施例中”并不一定都指同一实施例，但可以指同一实施例。此外，在一个或多个实施例中，能够以任何适当的方式组合各特定特征、结构或特性，如从本公开对本领域的普通技术人员显而易见的那样。

对本申请具体实施方式进行进一步详细说明之前，对本申请实施例中涉及的技术用语进行说明。除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。如有不一致，以本说明书中所说明的含义或者根据本说明书中记载的内容得出的含义为准。另外，本文中所使用的术语只是为了描述本申请实施例的目的，不是旨在限制本申请。

为了准确地对本申请中的技术内容进行叙述，以及为了准确地理解本申请，在对具体实施方式进行说明之前先对本说明书中所使用的术语给出如下的解释说明或定义。

同步定位与建图，Simultaneous Localization and Mapping，SLAM，或称为即时定位与地图构建，或并发建图与定位。本申请中，同步定位与建图技术的原理是利用激光雷达或视觉传感器观测信息进行位置估计，同时将点云或者图像拼接到一起构成完整的地图。

迭代最近点，Iterative Closest Point，ICP。一种迭代计算方法，分别在待匹配的目标点云P和源点云Q中，按照一定的约束条件，找到最邻近点(pi，qi)，然后计算出最优匹配参数(旋转参数Rotate，平移参数Translate)，使得误差函数最小的一种方法。本申请中，最优匹配参数就是两个点云的位姿变换参数。

激光雷达里程计和建图，Lidar Odometry and Mapping，LOAM。一种利用激光雷达实现SLAM的算法，通过激光雷达的点云匹配，计算机器人相对于初始时刻的位置，称为激光雷达里程计，根据每个位置把点云结合到一起构成地图，称为建图。

点云库，Point Cloud Library，PCL。是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。支持多种操作系统平台，可在Windows、Linux、Android、Mac OS X、部分嵌入式实时系统上运行。

位姿，Position and Orientation，Pose。表示位置和姿态，位置是机器人所处的相对于参考坐标系的坐标，姿态是机器人相对于参考坐标系的朝向，包括俯仰角，偏航角和翻滚角。

下面，首先对现有技术进行分析：

技术一：如图1所示为现有的一种视觉SLAM方法的流程图，该方法通过视觉传感器(摄像头)采集视觉图像，并提取图像特征点，利用惯性传感器的预积分作为初值进行图像特征点匹配，估计出相机的位姿，得到视觉里程计。并在闭环检测之后将所有帧的误差通过非线性优化的技术来进行优化，得到优化后的位姿，利用采集的视觉图像以及优化后的位姿构建全局地图。

技术一缺陷：由于视觉SLAM是提取图像特征点，图像的亮度或灰度容易对特征点的提取效果产生影响，当视觉传感器朝向地面或单独的墙壁时，容易造成图像特征点丢失，导致无法提取到足够多的图像特征点；或者当视觉传感器处于强光线的室外环境中，光线的剧烈变化也容易对图像特征点的提取产生较大影响，从而导致最终的建图效果不理想。基于此，现有的视觉SLAM方法容易受到多种因素的影响，存在适用环境范围较小，鲁棒性较差的缺陷。

技术二：如图2所示为现有的一激光SLAM方法的流程图，该方法通过激光雷达传感器获取点云，将前后两帧的点云进行点云匹配，在点云匹配过程中，采用迭代最近点ICP算法，根据源点云中的每个点查找其变换到目标点云后在目标点云中的最近点，利用每个点与最近点之间的误差求和，并最小化误差求和的方式，来估计两帧点云之间的位姿变换参数。同时还通过在帧数据库中查找帧点云执行闭环检测，在检测到闭环后利用非线性优化技术来优化位姿，并将所有帧的点云根据对应的位姿变换参数拼接到一起构建点云地图。

技术二缺陷：如图3所示，由于该激光SLAM方法在点云匹配过程中，只考虑了源点云和目标点云中点的距离最近，将其距离误差作为距离约束来求解位姿变换参数，源点云中的部分点云簇的边缘的点，其在目标点云中距离最近的点属于其他的点云簇，导致前后帧的匹配结果容易产生较大误差，同时，点云匹配的过程耗时较长，位姿变换参数的精度较低，导致构建的地图容易产生重叠的问题。并且在室外动态环境中使用时，由于动态物体的影响，源点云中的点难以在目标点云中找到准确的对应点，导致点云匹配失败，无法收敛到正确的位姿，从而导致最终的建图效果不理想，同样存在鲁棒性较差的问题。

本申请实施例提供了一种SLAM方法，在两帧点云之间的距离约束的基础上，增加两帧点云之间的物体类别的关联约束，根据该距离约束和关联约束，进行点云匹配，提高了点云匹配的速度和精度，以得到高精度的位姿变换参数，从而提高最终构建的点云地图的精度。本申请可以应用于机器人、车辆自动驾驶领域。下面对本申请进行详细介绍。

实施例一

如图4所示，本申请实施例提供的一种SLAM方法中，通过激光雷达进行激光点云的采集，并通过对采集的多帧点云进行聚类分割、关联计算、动态滤除、点云匹配和闭环优化等过程的处理，构建点云地图。其中激光雷达可以安装在车辆、机器人等可移动的设备上，后文描述激光雷达的移动时，可以理解为所述设备的移动而导致激光雷达的空间位置发生了变化。具体的，该方法包括：

S401：获取激光雷达采集到的两帧点云数据；

本实施例中，通过激光雷达进行点云采集时，以间隔一设定的行走距离d(例如d＝0.3m)的方式，采集多帧点云，本实施例中，以其中的两帧点云数据为例进行详细介绍，该两帧点云数据为激光雷达前后采集的T1帧点云和T2帧点云，在一些实施例中，该T1帧点云和T2帧点云可以为前后相邻关系，也可以为不相邻关系。

然而通过激光雷达采集的点云，往往会较为密集，过多的点云数量会对后续分割工作带来困难。本实施例通过点云库(PCL)中的体素栅格滤波器对采集的每帧点云进行降采样，体素栅格滤波器可以达到降采样的同时不破坏点云本身几何结构的功能，经过降采样，减少每帧点云中的点的数量，并同时保持点云的形状特征。

S402：对所述T1帧点云、T2帧点云分别进行聚类，得到所述T1帧点云中各物体的点云簇、所述T2帧点云中各物体的点云簇；

本实施例中，所述聚类具体包括下述子步骤：

基于欧氏距离的聚类算法对所述采集的T1帧点云、T2帧点云分别进行聚类；在其他一些实施例中，也可以采用K-means算法，马氏距离的聚类算法等。

将所述聚类得到的T1帧点云中各物体的点云簇从T1帧点云中分割出来，构建各物体的点云簇的包围框，并获取各物体的点云簇的包围框的参数；

将所述聚类得到的T2帧点云中各物体的点云簇从T2帧点云中分割出来，构建各物体的点云簇的包围框，并获取各物体的点云簇的包围框的参数。

其中，点云簇的包围框的参数具体可以包括包围框的位置坐标，包围框的水平航向角朝向，包围框的长，宽，高等参数。

S403：基于点云簇的相似度，将所述T2帧点云中各物体的点云簇与所述T1帧点云中各物体的点云簇进行关联；

本实施例中，该关联过程具体包括：

基于所述T1帧点云中各物体的点云簇的包围框的参数、所述T2帧点云中各物体的点云簇的包围框的参数，计算T2帧点云中各物体的点云簇与T1帧点云中各物体的点云簇的关联距离，该关联距离用于表示T2帧点云中各物体的点云簇与T1帧点云中各物体的点云簇的相似度，两个点云簇的关联距离越小，表示两个点云簇的相似度越高，基于该点云簇的相似度，将T2帧点云中各物体的点云簇与T1帧点云中各物体的点云簇进行两两关联。

如图5所示，通过该关联过程，可得到T2帧点云中各物体的点云簇与所述T1帧点云中各物体的点云簇的两两关联关系。需要说明的是，本实施例以及下述实施例中所述建立两两关联关系的点云簇，具体指的是该T2帧点云中和T1帧点云中包括的对应相同物体的点云簇。由于T1帧点云和T2帧点云是通过移动的激光雷达前后采集的两帧点云，由于激光雷达的移动，T1帧点云中的部分物体的点云簇，并未在T2帧点云中采集到，同理，T2帧点云中的部分物体的点云簇，也未在T1帧点云中采集到，因此在进行关联计算时，仅需将T1帧点云中和T2帧点云中同时包括的对应相同物体的点云簇进行关联。

S404：滤除所述T1帧点云、T2帧点云中的对应动态物体的点云簇；

由于动态环境中的动态物体会对点云匹配产生一定影响，因此在进行点云匹配之前，可采用卡尔曼滤波算法对每帧点云中的每个点云簇在连续的多帧点云中的速度进行观测和估计，通过观测每个点云簇的质心位置，结合匀速运动的模型，从而判断该点云簇的运动速度，基于运动速度将每帧点云中的多个点云簇划分为动态物体的点云簇和静态物体的点云簇，并将动态物体的点云簇从每帧点云中滤除，以消除动态物体对点云匹配的影响。

S405：基于所述关联的结果及所述T2帧点云与所述T1帧点云中的各个点的距离，将所述T2帧点云与所述T1帧点云进行点云匹配，得到所述T2帧点云与所述T1帧点云之间的位姿变换参数；

本实施例中，基于步骤S403获取的T2帧点云中各物体的点云簇与所述T1帧点云中各物体的点云簇的两两关联关系，建立包括关联约束和距离约束的点云匹配函数，利用该点云匹配函数计算得到T2帧点云与T1帧点云之间的位姿变换参数。具体的，

该关联约束的建立过程包括：考虑到每帧点云中可能会存在多个点云簇交叉重合的可能，以T2帧点云中的某个点的在该T2帧点云的聚类结果为例，通过计算该某个点到该T2帧点云中各物体的点云簇的距离，根据一设定阈值，筛选出该T2帧点云中小于该设定阈值的一个或多个物体的点云簇，作为该某个点可能聚类属于的一个或多个物体的点云簇，然后计算该某个点与该一个或多个点云簇的误差；假设该某个点经过位姿变换后，在T1帧点云中具有一个投影点，基于步骤S403的关联结果，同理可计算该投影点与具有关联关系的T1帧点云中的一个或多个点云簇的误差。通过计算T1帧点云中的所有投影点与可能属于的各物体的点云簇的误差，可得到T2帧点云中各物体的点云簇与T1帧点云中各物体的点云簇的物体类别误差，本实施例中，可以将所述物体类别误差最小作为所述点云匹配过程中的所述T2帧点云中各物体的点云簇与所述T1帧点云中各物体的点云簇之间的关联约束；

该距离约束的建立过程包括：基于迭代最近点ICP算法，计算T2帧点云中的每个点经过位姿变换后，到T1帧点云中的最近的点的几何距离，对T2帧点云中的各个点的所述几何距离进行求和，得到T2帧点云与T1帧点云的距离误差，本实施例中，可以将所述距离误差最小作为所述点云匹配过程中的所述T2帧点云中各个点与所述T1帧点云中各个点之间的距离约束；

通过对所述物体类别误差和距离误差分别设置一权重系数，并进行求和计算，基于所述关联约束和距离约束，将加权求和的结果最小作为最终的约束条件，即可计算T2帧点云与T1帧点云之间的位姿变换参数，该位姿变换参数即为T2帧点云与T1帧点云在点云匹配过程中的最优的位姿变换参数。

S406：对所述T1帧点云、T2帧点云进行闭环检测优化；

本实施例中，基于一查找条件，在帧数据库中分别查找每帧点云最接近的帧，构成闭环后，可利用非线性优化技术对T1帧点云、T2帧点云的位姿分别进行优化，以对T2帧点云与T1帧点云之间的位姿变换参数进行优化。

S407：基于所述T2帧点云与T1帧点云之间的位姿变换参数，构建点云地图；

本实施例中，可以将T1帧点云作为参考帧，基于该T1帧点云的位姿、所述T2帧点云与T1帧点云之间的位姿变换参数，将所述T2帧点云进行位姿变换；

将位姿变换后的T2帧点云与T1帧点云进行融合，得到T2帧点云与T1帧点云组成的点云地图，基于本方法得到的位姿变换参数精度的提高，构建的点云地图的精度也相应的提高。

实施例二

如图6所示，本申请实施例提供了一种动态环境中的SLAM方法，该方法中，以激光雷达在动态环境中，采集的前后相邻的两帧点云的处理过程为例，对本方法进行详细描述，设该相邻的两帧点云分别为T1帧点云和T2帧点云，其中T1帧点云为T2帧点云的前一帧。该方法包括：

S601：点云预处理与分割，对输入的T1帧点云和T2帧点云进行预处理与分割；

其中，首先采用点云库中的体素栅格滤波器对该T1帧点云和T2帧点云进行滤波，通过将点云划分到三维体素栅格中，栅格的分辨率可以调整，本实施例中可调整分辨率为0.2m，用每个栅格中心的点代替该栅格内的所有的点，实现降采样，减少点云中的点的数量，并同时保持点云的形状特征；

然后对降采样后的点云进行聚类分割，其中可采用基于欧氏距离的聚类算法对降采样后的T1帧点云和T2帧点云进行聚类，形成一个个不同的点云簇，通过将该不同的点云簇从原始点云中分割出来，并给每个点云簇建立包围框，以表示不同的物体，基于该包围框的参数即可进行后续的关联处理，而无需识别出物体的具体类别，其中该包围框的参数具体可以包括该包围框的中心的位置坐标(x，y，z)，包围框的水平航向角朝向角θ，长L，宽W，高H等参数。

S602：物体关联，对T2帧点云和T1帧点云进行关联计算，将T2帧点云和T1帧点云中的对应同一物体的点云簇进行关联；

其中，对于T2帧点云和T1帧点云分割出的多个点云簇，基于其各自的包围框的参数，分别计算T2帧点云中的每个点云簇与T1帧点云中的每个点云簇的关联距离，该关联距离用于表示两个点云簇之间的相似度，关联距离越小，相似度越高，则表示该两个点云簇属于同一物体的可能性越大。具体的，设参与该关联计算的T2帧点云中的点云簇为物体i，物体i的位置坐标为

水平航向角为θ₂，物体i的尺寸为

参与该关联计算的T1帧点云中的点云簇为物体j，物体j的位置坐标为

水平航向角为θ₁，物体j的尺寸为

该物体i和物体j的关联距离为d_i,j，则该关联距离d_i,j具体包括该两个点云簇的包围框之间的位置距离d₁、朝向距离d₂和尺寸距离d₃；

d₂＝θ₂-θ₁

基于上述三个不同参数之间的关联距离d₁、d₂和d₃，即可计算出T2帧点云中的物体i与T1帧点云中的物体j的关联距离为d_i,j，

其中，w_k用于表示不同参数的关联距离的权重，基于不同参数对物体特征的表述能力及工程经验，可分别选取位置距离d₁、朝向距离d₂和尺寸距离d₃在关联距离d_i,j中所占的权重分别为w₁＝0.5，w₂＝0.1，w₃＝0.4。

如图7所示，通过分别计算T2帧点云中的每个点云簇与T1帧点云中的每个点云簇的关联距离，通过设定一关联距离的阈值，可将该两帧点云中的处于该阈值范围内的多个点云簇进行一一对应，建立对应相同物体的两个点云簇之间的关联关系，该关联关系具体可以为点云簇的ID的对应关系，例如T2帧点云中的某个点云簇的ID为C，则其在T1帧点云中具有关联关系的点云簇的ID也可以设置为C。

需要说明的是，由于激光雷达的移动，对于T1帧点云中的部分点云簇，并未在T2帧中采集到，同理，T2帧点云中新采集的部分点云簇也未包含在T1帧点云中，在进行关联计算的过程中，其关联距离不处于筛选的阈值范围内，因此不会建立其关联关系。

S603：滤除动态物体，基于关联的结果，滤除T1帧点云和T2帧点云中的动态物体；

其中，基于关联的结果，可采用卡尔曼滤波算法，通过测量连续多帧点云中的同一物体的点云簇的质心位置，并基于匀速运动的模型，可估计同一物体在连续的多帧点云中运动速度，根据运动速度，可将点云簇划分为动态物体的点云簇和静态物体的点云簇，由此可滤除动态提的点云簇，减少动态物体对后续点云匹配造成的影响。

S604：点云匹配，基于关联计算的结果及T2帧点云与T1帧点云中的各个点的距离，构建多约束的点云匹配，得到T2帧点云与T1帧点云之间的位姿变换参数；

其中，基于关联计算的结果，设T2帧点云中的某个点p_i到该T2帧点云中的某个点云簇C的质心的距离为d_i，定义该点p_i在该T2帧点云中分类属于点云簇C的误差概率

为：

假设经过位姿变换后，该点p_i在T1帧点云中的投影点为q_i，基于关联计算的结果，T2帧点云中的点云簇C在T1帧点云中对应的ID同样为C，则投影点q_i到T1帧点云中的点云簇C中最近的点的距离为dis(q_i，C)，则投影点q_i在T1帧点云中属于点云簇C的误差函数为：

当dis(q_i，C)为0时，该误差函数的值也为0，表示投影点q_i到T1帧点云中的点云簇C的误差为0。

由于每帧点云中的多个点云簇之间可能会存在交叉重叠的情况，因此每个点在聚类时可能会聚类属于不止一个点云簇，以点p_i在T2帧点云的聚类结果为例，通过计算该点p_i到该T2帧点云中每个点云簇的距离，根据一设定阈值，筛选出计算出的距离小于设定阈值的一个或多个点云簇(例如点云簇C₁、C₂、C₃)，然后计算该点p_i可能属于该T2帧点云中的点云簇C₁、C₂、C₃的误差；同理可计算该点p_i对应的投影点q_i在T1帧点云中属于点云簇C₁、C₂、C₃的误差，据此，可对投影点q_i可能属于T1帧点云中的点云簇C₁、C₂、C₃的误差进行求和，得到该投影点q_i与T1帧点云中可能属于的各物体的点云簇的总的误差：

由此，可对T2帧点云在T1帧点云中的各个投影点的所述总的误差进行求和，得到T2帧点云中各物体的点云簇与T1帧点云中各物体的点云簇的物体类别误差，将物体类别误差最小作为T2帧点云中各物体的点云簇与T1帧点云中各物体的点云簇进行点云匹配的关联约束：

其中，该E(R,T)_Label表示物体类别误差与位姿变换参数之间的关联关系。

然后，基于迭代最近点算法，计算T2帧点云中的每个点p_i经过位姿变换后，与T1帧点云中的距离最近的点q_i的几何距离，对T2帧点云中的各个点的所述几何距离进行求和，得到T2帧点云与T1帧点云的距离误差，将所述距离误差最小作为所述点云匹配过程中的所述T2帧点云中各个点与所述T1帧点云中各个点之间的距离约束：

构建关联约束和距离约束的点云匹配函数，通过对所述物体类别误差和距离误差分别设置一权重系数，并进行求和计算，将加权求和的结果最小作为最终的约束条件，即可计算T2帧点云与T1帧点云之间的位姿变换参数：

E(R,T)_icp＝λE(R,T)_Label+(1-λ)E(R,T)

其中，λ为权重系数，根据工程经验，可设置物体类别误差在点云匹配中所占的权重为λ＝0.42，则距离误差所占的权重为1-λ＝0.58，利用该点云匹配函数，求解使加权求和的结果最小的位姿变换参数(R，T)即为T2帧点云与T1帧点云的位姿变换参数，其中R为T2帧点云相对于T1帧点云的旋转向量，T为T2帧点云相对于T1帧点云的位移向量。

S605：闭环检测，对T2帧点云和T1帧点云分别在帧数据库中查找最接近的帧，构成闭环，分别对其位姿进行优化，以对T2帧点云与T1帧点云之间的位姿变换参数进行优化；

其中，通过一预设的距离阈值和时间戳间隔阈值，在帧数据库中查找最接近的帧，构成闭环后，进行非线性优化，以对T2帧点云和T1帧点云的位姿分别优化，从而对T2帧点云与T1帧点云之间的位姿变换参数进行优化。

S606，构建地图，基于优化后的位姿变换参数，构建包含T2帧点云和T1帧点云的点云地图。

具体的，可以将T1帧点云作为参考帧，基于该T1帧点云的位姿、所述T2帧点云与T1帧点云之间的位姿变换参数，将所述T2帧点云进行位姿变换；

将位姿变换后的T2帧点云与T1帧点云进行融合，得到T2帧点云与T1帧点云组成的点云地图。

在一些实施例中，当具有多帧点云时，也采用该方法，将每帧点云按照其与前一帧点云的位姿变换参数，进行位姿变换，与前一帧融合，构建包含多帧点云的点云地图。

综上，本实施例通过采用两帧点云的关联约束和距离约束作为约束项，进行点云匹配，可得到精度更高的位姿变换参数，从而使得构建的点云地图的误差更小，精度更高。同时，本方法还考虑了动态环境中动态物体的影响，通过对动态物体进行滤除，消除了动态物体对点云匹配造成的影响，提高了本方法在动态环境中的鲁棒性。

以室外不同场景(A、B、C三种场景，B为人流相对较多的动态场景，A、C为静态场景)为例，比较了本实施例SLAM与前述技术二中的现有激光SLAM得到的轨迹精度，分别与GPS真实值进行了对比，轨迹的效果和误差如下表所示，可以看出本实施例SLAM得到的轨迹与GPS真实值更接近，在动态场景下，本实施例SLAM具有更明显的精度提升。

实施例三

如图8所示，本申请实施例提供了一种静态环境中的SLAM方法，本实施例相对于图6所示的实施例二，缺少了动态物体的消除过程，其他处理过程与图6所示的实施例二一致，在此不做赘述。

本实施例同样采用两帧点云的关联约束和距离约束作为点云匹配的约束项，进行点云匹配，具有更快的匹配速度，可得到精度更高的位姿变换参数，从而使得构建的点云地图的误差更小，精度更高。

同理，以室外不同场景(A、B、C三种场景，B为人流相对较多的动态场景，A、C为静态场景)为例，比较了本实施例SLAM与前述技术二中的现有激光SLAM得到的轨迹精度，分别与GPS真实值进行了对比，可以看出本实施例SLAM得到的轨迹与GPS真实值更接近，在动态场景下，虽然本实施例没有滤除动态物体，相比于实施例二，误差有所增大，但仍然比现有激光SLAM的精度高。

综上，本申请实施例所提供的SLAM方法，具有如下有益效果：

相比于现有的激光SLAM方法中，仅仅使用两帧点云的点之间的距离约束来求解位姿变换参数，具有更高的精度，并且点云匹配的时间明显降低，收敛更块。本方法采用的基于物体包围框参数来关联两帧点云中各物体的点云簇的方法，不需要识别出物体的种类，即可找到两帧点云的物体对应关系，相比于语义SLAM(通过识别物体的语义来关联物体)，不需要训练模型，具有更小的计算量和操作难度；

通过将动态物体移除，避免了在点云匹配过程中，由于动态物体影响导致找到错误的匹配点，进而使点云匹配收敛到错误的位姿变换参数的情况，本方法提高了位姿变换参数的精度，同时避免了将动态物体的点云添加到最终的点云地图中，有效地提高了SLAM技术在室外环境中的鲁棒性。

除此之外，本方法还具有广阔的应用范围，例如自动驾驶中的定位技术、自动导航技术，以及根据预测的动态物体的运动轨迹，进行自动驾驶过程中的自动避障技术。

作为本实施例的变形实施例，本申请实施例提供的一种导航方法中，可基于上述实施例中的SLAM方法得到获取的每帧点云与前一帧点云的位姿变换参数；基于所述位姿变换参数，对所述每帧点云进行变换，并与前一帧点云进行融合，得到包含所述每帧点云的点云地图；基于所述点云地图进行路径规划和动态障碍物避障。

作为本实施例的变形实施例，本申请实施例提供的一种定位方法中，可用于对激光雷达的当前位姿进行确定，具体的，基于上述实施例中的SLAM方法，可得到激光雷达的当前时刻与前一时刻的位姿变换参数；基于所述位姿变换参数、所述激光雷达的前一时刻的位姿，确定所述激光雷达的当前时刻的位姿。

实施例四

如图9所示，本申请实施例提供了一种SLAM装置，该装置包括：

获取模块901，用于获取激光雷达采集到的两帧点云数据，所述两帧点云数据包括前后的第一帧点云和第二帧点云；

聚类模块902，用于对所述第一帧点云进行聚类，得到所述第一帧点云中各物体的点云簇；对所述第二帧点云进行聚类，得到所述第二帧点云中各物体的点云簇；

关联模块903，用于将所述第二帧点云中各物体的点云簇与所述第一帧点云中各物体的点云簇进行关联；

动态滤除模块904，用于滤除所述第一帧点云、第二帧点云中对应动态物体的点云簇；

点云匹配模块905，用于基于所述关联的结果，将所述第二帧点云与所述第一帧点云进行点云匹配，得到所述第二帧点云与所述第一帧点云之间的位姿变换参数；

闭环优化模块906，用于对所述第一帧点云、所述第二帧点云进行闭环检测；基于所述闭环检测的结果，对所述第二帧点云与所述第一帧点云之间的位姿变换参数进行优化；

地图构建模块907，用于基于所述第二帧点云与所述第一帧点云的位姿变换参数，将所述第二帧点云进行位姿变换；将位姿变换后的第二帧点云与所述第一帧点云进行融合，得到所述第二帧点云与第一帧点云组成的点云地图。

基于本实施例提供的SLAM装置，可实现上述实施例中的SLAM方法，对激光雷达采集的点云进行点云匹配、构建点云地图等，其实现的技术原理与上述实施例中的SLAM方法一致，此处不再赘述。

实施例五

图10是本申请实施例提供的一种计算设备1000的结构性示意性图。该计算设备1000包括：处理器1010、存储器1020、通信接口1030、总线1040。

应理解，图10所示的计算设备1000中的通信接口1030可以用于与其他设备之间进行通信。

其中，该处理器1010可以与存储器1020连接。该存储器1020可以用于存储该程序代码和数据。因此，该存储器1020可以是处理器1010内部的存储模块，也可以是与处理器1010独立的外部存储模块，还可以是包括处理器1010内部的存储模块和与处理器1010独立的外部存储模块的部件。

其中，计算设备1000还可以包括总线1040。其中，存储器1020、通信接口1030可以通过总线1040与处理器1010连接。总线1040可以是外设部件互连标准(PeripheralComponent Interconnect，PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，EISA)总线等。所述总线1040可以分为地址总线、数据总线、控制总线等。为便于表示，图10中仅用一条线表示，但并不表示仅有一根总线或一种类型的总线。

应理解，在本申请实施例中，该处理器1010可以采用中央处理模块(centralprocessing unit，CPU)。该处理器还可以是其它通用处理器、数字信号处理器(digitalsignal processor，DSP)、专用集成电路(application specific integrated circuit，ASIC)、现成可编程门阵列(field programmable gate Array，FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。或者该处理器1010采用一个或多个集成电路，用于执行相关程序，以实现本申请实施例所提供的技术方案。

该存储器1020可以包括只读存储器和随机存取存储器，并向处理器1010提供指令和数据。处理器1010的一部分还可以包括非易失性随机存取存储器。例如，处理器1010还可以存储设备类型的信息。

在计算设备1000运行时，所述处理器1010执行所述存储器1020中的计算机执行指令执行上述方法的操作步骤。

应理解，根据本申请实施例的计算设备1000可以对应于执行根据本申请各实施例的方法中的相应主体，并且计算设备1000中的各个模块的上述和其它操作和/或功能分别为了实现本实施例各方法的相应流程，为了简洁，在此不再赘述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

本申请实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时用于执行一种点云匹配方法、或一种导航方法、或一种定位方法，该方法包括上述各个实施例所描述的方案中的至少之一。

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

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

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

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

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

Claims

1.一种点云匹配方法，其特征在于，包括：

基于所述关联的结果及所述第二帧点云与所述第一帧点云中的各个点的距离，将所述第二帧点云与所述第一帧点云进行点云匹配，得到所述第二帧点云与所述第一帧点云之间的位姿变换参数，具体包括：

基于所述关联的结果，计算经过位姿变换后的所述第二帧点云中的各物体的点云簇与所述第一帧点云中各物体的点云簇的物体类别误差，其中，所述计算所述物体类别误差包括：对于所述第二帧点云中的每个点在所述第一帧点云中的投影点，分别针对每个所述投影点与所述第一帧点云中可能属于的各物体的点云簇的误差进行求和，分别得到每个所述投影点与所述第一帧点云中可能属于的各物体的点云簇的总误差；对各个所述投影点的所述总误差进行求和，其结果作为所述物体类别误差；

2.根据权利要求1所述的方法，其特征在于，还包括：

3.根据权利要求1所述的方法，其特征在于，所述基于点云簇的相似度，将所述第二帧点云中各物体的点云簇与所述第一帧点云中各物体的点云簇进行关联，包括：

获取所述第一帧点云中各物体的点云簇的特征向量；

获取所述第二帧点云中各物体的点云簇的特征向量；

4.根据权利要求3所述的方法，其特征在于，所述点云簇的特征向量包括至少以下之一：

所述点云簇的位置参数、朝向参数或尺寸参数。

5.根据权利要求1所述的方法，其特征在于，所述进行点云匹配之前，还包括：

滤除所述第一帧点云中对应动态物体的点云簇；和/或

滤除所述第二帧点云中对应动态物体的点云簇。

6.根据权利要求1所述的方法，其特征在于，还包括：

对所述第一帧点云进行闭环检测；和/或

对所述第二帧点云进行闭环检测；

7.根据权利要求1所述的方法，其特征在于，所述进行聚类之前，还包括：

滤除所述第一帧点云中的噪声点；和/或

滤除所述第二帧点云中的噪声点。

8.一种点云匹配装置，其特征在于，包括：

点云匹配模块，用于基于所述关联的结果及所述第二帧点云与所述第一帧点云中的各个点的距离，将所述第二帧点云与所述第一帧点云进行点云匹配，得到所述第二帧点云与所述第一帧点云之间的位姿变换参数，具体用于：

9.根据权利要求8所述的装置，其特征在于，还包括：

10.根据权利要求8所述的装置，其特征在于，所述关联模块具体用于：

获取所述第一帧点云中各物体的点云簇的特征向量；

获取所述第二帧点云中各物体的点云簇的特征向量；

11.根据权利要求10所述的装置，其特征在于，所述点云簇的特征向量包括至少以下之一：

所述点云簇的位置参数、朝向参数或尺寸参数。

12.根据权利要求8所述的装置，其特征在于，所述进行点云匹配之前，还包括动态滤除模块：

用于滤除所述第一帧点云中对应动态物体的点云簇；和/或

用于滤除所述第二帧点云中对应动态物体的点云簇。

13.根据权利要求8所述的装置，其特征在于，还包括闭环优化模块：

用于对所述第一帧点云进行闭环检测；和/或

用于对所述第二帧点云进行闭环检测；

14.根据权利要求8所述的装置，其特征在于，所述获取模块还用于：

滤除所述第一帧点云中的噪声点；和/或

滤除所述第二帧点云中的噪声点。

15.一种导航方法，其特征在于，包括：

基于权利要求1至7任意一项所述的点云匹配方法，得到获取的每帧点云与前一帧点云的位姿变换参数；

基于所述点云地图进行路径规划和动态障碍物避障。

16.一种导航设备，其特征在于，包括：

权利要求8至14任意一项所述的点云匹配装置，用于得到获取的每帧点云与前一帧点云的位姿变换参数；基于所述位姿变换参数，对所述每帧点云进行变换，并与前一帧点云进行融合，得到包含所述每帧点云的点云地图；

17.一种定位方法，其特征在于，包括：

基于权利要求1至7任意一项所述的点云匹配方法，得到激光雷达的当前时刻与前一时刻的位姿变换参数；

18.一种激光雷达，其特征在于，包括：

权利要求8至14任意一项所述的点云匹配装置，用于得到激光雷达的当前时刻与前一时刻的位姿变换参数；

19.一种计算设备，其特征在于，包括：

总线；

通信接口，其与所述总线连接；

至少一个处理器，其与所述总线连接；以及

至少一个存储器，其与所述总线连接并存储有程序指令，所述程序指令当被所述至少一个处理器执行时使得所述至少一个处理器执行权利要求1至7任意一项所述的点云匹配方法、或权利要求15所述的导航方法、或权利要求17所述的定位方法。

20.一种计算机可读存储介质，其特征在于，其上存储有程序指令，所述程序指令当被计算机执行时使得所述计算机执行权利要求1至7任意一项所述的点云匹配方法、或权利要求15所述的导航方法、或权利要求17所述的定位方法。