CN111707262B - 基于最近点向量投影的点云匹配方法、介质、终端和装置 - Google Patents

Abstract

本发明公开一种基于最近点向量投影的点云匹配方法、介质、终端和装置，首先获取每个有效光束端点与对应最近点组成的最近点向量，然后将该向量按照与光束平行或垂直的方向进行投影，分别得到平移与旋转的两个分向量，再将每个点云的平移向量求和得到点云向量的合力，即点云总体的平移方向与大小，并对于每个点云旋转向量乘上光束的长度，然后求和得到点云旋转向量的合向量，即点云总体旋转方向与大小，从而确定三个变量的搜索方向，并在匹配的综合评价得分比前一次迭代低时减半步长，从而减少无效匹配的次数，在保证点云与地图匹配精度的前提下成倍提升激光点云与地图匹配的速度，在机器人定位与导航的应用中具有重要的作用。

Description

基于最近点向量投影的点云匹配方法、介质、终端和装置

【技术领域】

本发明涉及智能机器人领域，尤其涉及一种基于最近点向量投影的点云匹配方法、介质、终端和装置。

【背景技术】

激光slam(simultaneous localization and mapping)是移动机器人定位导航的核心技术，全称为同时定位与建图。当机器人处于未知环境中的未知位置时，基于自身携带的传感器通过激光slam技术可以实现机器人边移动边估计出机器人实时位姿，然后不断扩展与更新地图，逐渐构建环境的完整地图。目前激光slam的两大主流的方向是粒子滤波与图优化的方法，两种方法都涉及点云与栅格地图的匹配，是激光slam的核心组成部分。匹配的精度对定位建图精度有较大的影响，同时匹配的速度影响了定位建图的实时性，进一步影响机器人的导航控制的响应速度与平滑性。因此提高点云与地图的匹配的精度与速度是机器人高效运行的关键技术。

点云与地图的匹配时，已知构建的地图，根据点云原点在地图上的先验位姿将点云栅格化，计算得到点云的每个点所在地图的栅格，将栅格化的点云与地图进行匹配得到点云原点在地图的后验位姿，从而可以确定所携带该激光的机器人在地图中的精确后验位姿。点云端点表示障碍物，理想情况下点云所在的栅格都为占用状态。点云与地图的匹配评价标准采用残差或者得分的方式。根据一些搜索方法确定点云在栅格地图中最近点，计算点云与最近点的距离称为残差，或者根据该距离计算正相关的得分。计算点云的所有点的残差或者正相关的匹配得分用于评价每次迭代的点云与地图匹配效果，残差越小或得分越高，那么点云与地图匹配效果越好。

激光点云与地图匹配比较常用的方法是梯度下降法，确定机器人在地图位姿需要三个变量平移变量x、平移变量y以及旋转变量θ，每个变量有2个方向，那么一次完整的梯度搜索，需要6次迭代，正常情况下点云与地图匹配需要5-10次完整的梯度搜索，残差值才逐渐收敛到比较低的水平。因此点云与地图的匹配需要较长的计算时间，难以满足定位与建图的实时性需求。另外梯度下降法在两个方向同时搜索容易陷入局部极小值，这是由于搜索范围较小导致的，如果搜索范围加大，则需要更多的计算时间。

【发明内容】

本发明提供了一种基于最近点向量投影的点云匹配方法、介质、终端和装置，解决了以上所述的技术问题。

本发明解决上述技术问题的技术方案如下：一种基于最近点向量投影的点云匹配方法，包括以下步骤：

S1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

S2，获取所述激光点云中每个激光点在所述环境地图中的最近点，并将所述激光点与对应最近点组成最近点向量；

S3，根据所有激光点的最近点向量计算用于表示激光点云整体平移方向和大小的第一合向量以及用于表示激光点云整体旋转方向和大小的第二合向量；其中，步骤S3具体包括以下步骤：

S301，将每个最近点向量按照平行于激光点云中对应光束的方向投影生成平移向量，按照垂直于所述对应光束的方向投影生成第一旋转向量，并将所述第一旋转向量乘以所述对应光束的长度生成第二旋转向量；

S302，对所有最近点向量的平移向量求和生成用于表示激光点云整体平移方向和大小的第一合向量，对所有最近点向量的第二旋转向量求和生成用于表示激光点云整体旋转方向和大小的第二合向量；

S4，将所述第一合向量和所述第二合向量作为搜索方向，并采用基于梯度下降法的地图匹配算法将所述激光点云和所述环境地图进行匹配，生成所述激光点云在所述环境地图中的后验位姿。

在一个优选实施方式中，步骤S4采用梯度下降法进行迭代时计算所述激光点云和所述环境地图的匹配得分，当所述匹配得分低于前一次的计算结果时，将搜索步长减半并继续迭代，直至满足预设迭代终止条件，并输出对应的后验位姿，所述预设迭代终止条件为：相邻两次迭代的匹配得分变化量低于预设阈值。

在一个优选实施方式中，采用以下预设公式计算所述激光点云和环境地图的匹配得分：

其中，S表示所述匹配得分，i表示激光点云中的激光点，d_i为每个激光点与最近点的距离，σ表示激光点云的标准差。

本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的基于最近点向量投影的点云匹配方法。

本发明实施例的第三方面提供了一种基于最近点向量投影的点云匹配终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述基于最近点向量投影的点云匹配方法的步骤。

本发明实施例的第四方面提供了一种基于最近点向量投影的点云匹配装置，包括栅格化模块、搜索模块、计算模块和迭代模块，

所述栅格化模块用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

所述搜索模块用于获取所述激光点云中每个激光点在所述环境地图中的最近点，并将所述激光点与对应最近点组成最近点向量；

所述计算模块用于根据所有激光点的最近点向量获取用于表示激光点云整体平移方向和大小的第一合向量以及用于表示激光点云整体旋转方向和大小的第二合向量；

所述迭代模块用于将所述第一合向量和所述第二合向量作为搜索方向，并采用基于梯度下降法的地图匹配算法将所述激光点云和所述环境地图进行匹配，生成所述激光点云在所述环境地图中的后验位姿；

其中，所述计算模块具体包括：

变换单元，用于将每个最近点向量按照平行于激光点云中对应光束的方向投影生成平移向量，按照垂直于所述对应光束的方向投影生成第一旋转向量，并将所述第一旋转向量乘以所述对应光束的长度生成第二旋转向量；

求和单元，用于对所有最近点向量的平移向量求和生成用于表示激光点云整体平移方向和大小的第一合向量，对所有最近点向量的第二旋转向量求和生成用于表示激光点云整体旋转方向和大小的第二合向量。

在一个优选实施方式中，所述迭代模块具体用于采用梯度下降法进行迭代时计算所述激光点云和所述环境地图的匹配得分，当所述匹配得分低于前一次的计算结果时，将搜索步长减半并继续迭代，直至满足预设迭代终止条件，并输出对应的后验位姿，所述预设迭代终止条件为：相邻两次迭代的匹配得分变化量低于预设阈值。

在一个优选实施方式中，所述迭代模块采用以下预设公式计算所述激光点云和环境地图的匹配得分：

其中，S表示所述匹配得分，i表示激光点云中的激光点，d_i为每个激光点与最近点的距离，σ表示激光点云的标准差。

本发明提供了一种激光点云与环境栅格地图匹配的方法、介质、终端和装置，首先获取每个有效光束端点与其对应最近点组成的最近点向量，然后将该向量按照与光束平行或垂直的方向进行投影，分别得到平移与旋转的两个分向量，再将每个点云的平移向量求和，得到点云向量的合力，即点云总体的平移方向与大小，并对于每个点云旋转向量乘上光束的长度，即光束端点到激光原点的距离，然后求点云旋转向量的合向量，即点云总体旋转方向与大小，从而确定三个变量的搜索方向，并在匹配的综合评价得分比前一次迭代低时减半步长，从而减少无效匹配的次数，在保证点云与地图匹配精度的前提下成倍提升激光点云与地图匹配的速度，在机器人定位与导航的应用中具有重要的作用。

为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并配合所附附图，作详细说明如下。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1是实施例1提供的基于最近点向量投影的点云匹配方法的流程示意图；

图2是实施例2提供的基于最近点向量投影的点云匹配装置的结构示意图；

图3是实施例3提供的基于最近点向量投影的点云匹配终端的结构示意图。

【具体实施方式】

为了使本发明的目的、技术方案和有益技术效果更加清晰明白，以下结合附图和具体实施方式，对本发明进行进一步详细说明。应当理解的是，本说明书中描述的具体实施方式仅仅是为了解释本发明，并不是为了限定本发明。

图1是实施例1提供的基于最近点向量投影的点云匹配方法的流程示意图，如图1所示，包括以下步骤：

S1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化，得到激光点云每个点所在的栅格。

S2，获取所述激光点云中每个激光点在所述环境地图中的最近点，并将所述激光点与对应最近点组成最近点向量。已知环境栅格地图M与一帧激光点云S，且激光点云的原点在地图M下先验估计位姿为P₀，需要确定激光点云S的原点在地图M的精确位置，从而确定所携带该激光的机器人在地图中的精确后验位姿。首先确定激光点云S的每束激光端点在栅格地图M中的位置，在地图M上确定距离每个激光端点e_i最近的占用栅格点q_i，具体可以依靠金字塔原理提出来的分级收缩算法、反向定标技术的投影搜索算法、近似多维二元搜索树(AK-D Tree)算法等等搜索激光点在环境栅格地图中的最近点q_i，计算得到每个激光束端点与最近点的距离为d_i。基于先验位姿P₀确定的后验位姿在P₀附近，因此距离较远的最近点q_i应予以剔除。具体实施例中，d_i大于栅格地图2个栅格长度的最近点q_i视为无效，因此与该最近点对应的激光点云的激光点为无效点，不参与地图匹配评价。

S3，根据所有激光点的最近点向量计算用于表示激光点云整体平移方向和大小的第一合向量以及用于表示激光点云整体旋转方向和大小的第二合向量。具体包括以下步骤：

S301，将每个最近点向量V_i＝eq按照平行于激光点云中对应光束的方向投影生成平移向量V_it，按照垂直于所述对应光束的方向投影生成第一旋转向量V_in，并将所述第一旋转向量V_in乘以所述对应光束的长度l生成第二旋转向量。所述光束长度l为光束端点到激光原点的距离。

S302，对所有最近点向量的平移向量求和生成用于表示激光点云整体平移方向和大小的第一合向量

对所有最近点向量的第二旋转向量求和生成用于表示激光点云整体旋转方向和大小的第二合向量

即点云绕点云原点旋转的方向。

然后执行S4，将所述第一合向量和所述第二合向量作为搜索方向，并采用基于梯度下降法的地图匹配算法，比如暴力匹配方法等将所述激光点云和所述环境地图进行匹配，即将所述第一合向量和所述第二合向量确定的三个方向作为搜索方向进行占用栅格搜索并计算点云匹配得分，并根据该点云匹配得分对后验位姿进行优化更新，从而得到该激光点云在所述环境地图中的最优后验位姿。具体来说，平移合向量投影到地图坐标系X与Y轴上，得到x与y的平移方向，旋转合向量确定了θ的方向，从而确定了点云与地图匹配搜索的三个方向，而平移与旋转的步长需要单独设定。然后采用梯度下降法进行迭代时计算所述激光点云和所述环境地图的匹配得分，当所述匹配得分低于前一次的计算结果时，将搜索步长减半并继续迭代，直至满足预设迭代终止条件，并输出对应的后验位姿，所述预设迭代终止条件为：相邻两次迭代的匹配得分变化量低于预设阈值。

在一个优选实施方式中，采用以下预设公式计算所述激光点云和环境地图的匹配得分：

其中，S表示所述匹配得分，i表示激光点云中的激光点，d_i为每个激光点与最近点的距离，σ表示激光点云的标准差。

以上实施例提供了一种激光点云与环境栅格地图匹配的方法，首先获取每个有效光束端点与其对应最近点组成的最近点向量，然后将该向量按照与光束平行或垂直的方向进行投影，分别得到平移与旋转的两个分向量，再将每个点云的平移向量求和，得到点云向量的合力，即点云总体的平移方向与大小，并对于每个点云旋转向量乘上光束的长度，即光束端点到激光原点的距离，然后求点云旋转向量的合向量，即点云总体旋转方向与大小，从而确定三个变量的搜索方向，并在匹配的综合评价得分比前一次迭代低时减半步长，从而减少无效匹配的次数，在保证点云与地图匹配精度的前提下成倍提升激光点云与地图匹配的速度，在机器人定位与导航的应用中具有重要的作用。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

本发明实施例还提供了一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，实现以上所述的基于最近点向量投影的点云匹配方法。

图2是实施例2提供的基于最近点向量投影的点云匹配装置的结构示意图，如图2所示，包括栅格化模块100、搜索模块200、计算模块300和迭代模块400，

所述栅格化模块100用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

所述搜索模块200用于获取所述激光点云中每个激光点在所述环境地图中的最近点，并将所述激光点与对应最近点组成最近点向量；

所述计算模块300用于根据所有激光点的最近点向量获取用于表示激光点云整体平移方向和大小的第一合向量以及用于表示激光点云整体旋转方向和大小的第二合向量；

所述迭代模块400用于将所述第一合向量和所述第二合向量作为搜索方向，并采用基于梯度下降法的地图匹配算法将所述激光点云和所述环境地图进行匹配，生成所述激光点云在所述环境地图中的后验位姿。

在一个优选实施方式中，所述计算模块300具体包括：

变换单元301，用于将每个最近点向量按照平行于激光点云中对应光束的方向投影生成平移向量，按照垂直于所述对应光束的方向投影生成第一旋转向量，并将所述第一旋转向量乘以所述对应光束的长度生成第二旋转向量；

求和单元302，用于对所有最近点向量的平移向量求和生成用于表示激光点云整体平移方向和大小的第一合向量，对所有最近点向量的第二旋转向量求和生成用于表示激光点云整体旋转方向和大小的第二合向量。

在一个优选实施方式中，所述迭代模块400具体用于采用梯度下降法进行迭代时计算所述激光点云和所述环境地图的匹配得分，当所述匹配得分低于前一次的计算结果时，将搜索步长减半并继续迭代，直至满足预设迭代终止条件，并输出对应的后验位姿，所述预设迭代终止条件为：相邻两次迭代的匹配得分变化量低于预设阈值。

在一个优选实施方式中，所述迭代模块400采用以下预设公式计算所述激光点云和环境地图的匹配得分：

其中，S表示所述匹配得分，i表示激光点云中的激光点，di为每个激光点与最近点的距离，σ表示激光点云的标准差。

本发明实施例还提供了一种基于最近点向量投影的点云匹配终端，包括所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现以上所述基于最近点向量投影的点云匹配方法的步骤。图3是本发明实施例3提供的基于最近点向量投影的点云匹配终端的结构示意图，如图3所示，该实施例的基于最近点向量投影的点云匹配终端8包括：处理器80、可读存储介质81以及存储在所述可读存储介质81中并可在所述处理器80上运行的计算机程序82。所述处理器80执行所述计算机程序82时实现上述各个方法实施例中的步骤，例如图1所示的步骤1至步骤4。或者，所述处理器80执行所述计算机程序82时实现上述各装置实施例中各模块的功能，例如图2所示模块100至400的功能。

示例性的，所述计算机程序82可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述可读存储介质81中，并由所述处理器80执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序82在所述基于最近点向量投影的点云匹配终端8中的执行过程。

所述基于最近点向量投影的点云匹配终端8可包括，但不仅限于，处理器80、可读存储介质81。本领域技术人员可以理解，图3仅仅是基于最近点向量投影的点云匹配终端8的示例，并不构成对基于最近点向量投影的点云匹配终端8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述基于最近点向量投影的点云匹配终端还可以包括电源管理模块、运算处理模块、输入输出设备、网络接入设备、总线等。

所称处理器80可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述可读存储介质81可以是所述基于最近点向量投影的点云匹配终端8的内部存储单元，例如基于最近点向量投影的点云匹配终端8的硬盘或内存。所述可读存储介质81也可以是所述基于最近点向量投影的点云匹配终端8的外部存储设备，例如所述基于最近点向量投影的点云匹配终端8上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述可读存储介质81还可以既包括所述基于最近点向量投影的点云匹配终端8的内部存储单元也包括外部存储设备。所述可读存储介质81用于存储所述计算机程序以及所述基于最近点向量投影的点云匹配终端所需的其他程序和数据。所述可读存储介质81还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

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

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

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

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

本发明并不仅仅限于说明书和实施方式中所描述，因此对于熟悉领域的人员而言可容易地实现另外的优点和修改，故在不背离权利要求及等同范围所限定的一般概念的精神和范围的情况下，本发明并不限于特定的细节、代表性的设备和这里示出与描述的图示示例。

Claims (8)

1.一种基于最近点向量投影的点云匹配方法，其特征在于，包括以下步骤：

S1，根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

S2，获取所述激光点云中每个激光点在所述环境地图中的最近点，并将所述激光点与对应最近点组成最近点向量；

S3，根据所有激光点的最近点向量计算用于表示激光点云整体平移方向和大小的第一合向量以及用于表示激光点云整体旋转方向和大小的第二合向量；其中，步骤S3具体包括以下步骤：

S301，将每个最近点向量按照平行于激光点云中对应光束的方向投影生成平移向量，按照垂直于所述对应光束的方向投影生成第一旋转向量，并将所述第一旋转向量乘以所述对应光束的长度生成第二旋转向量；

S302，对所有最近点向量的平移向量求和生成用于表示激光点云整体平移方向和大小的第一合向量，对所有最近点向量的第二旋转向量求和生成用于表示激光点云整体旋转方向和大小的第二合向量；

S4，将所述第一合向量和所述第二合向量作为搜索方向，并采用基于梯度下降法的地图匹配算法将所述激光点云和所述环境地图进行匹配，生成所述激光点云在所述环境地图中的后验位姿。

2.根据权利要求1所述基于最近点向量投影的点云匹配方法，其特征在于，步骤S4采用梯度下降法进行迭代时计算所述激光点云和所述环境地图的匹配得分，当所述匹配得分低于前一次的计算结果时，将搜索步长减半并继续迭代，直至满足预设迭代终止条件，并输出对应的后验位姿，所述预设迭代终止条件为：相邻两次迭代的匹配得分变化量低于预设阈值。

3.根据权利要求2所述基于最近点向量投影的点云匹配方法，其特征在于，采用以下预设公式计算所述激光点云和环境地图的匹配得分：

其中，S表示所述匹配得分，i表示激光点云中的激光点，d_i为每个激光点与最近点的距离，σ表示激光点云的标准差。

4.一种计算机可读存储介质，其特征在于，存储有计算机程序，所述计算机程序被处理器执行时，实现权利要求1-3任一项所述的基于最近点向量投影的点云匹配方法。

5.一种基于最近点向量投影的点云匹配终端，其特征在于，包括权利要求4所述的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程序时实现如权利要求1-3任一项所述基于最近点向量投影的点云匹配方法的步骤。

6.一种基于最近点向量投影的点云匹配装置，其特征在于，包括栅格化模块、搜索模块、计算模块和迭代模块，

所述栅格化模块用于根据激光点云在当前时刻的先验位姿将所述激光点云在环境地图上栅格化；

所述搜索模块用于获取所述激光点云中每个激光点在所述环境地图中的最近点，并将所述激光点与对应最近点组成最近点向量；

所述计算模块用于根据所有激光点的最近点向量获取用于表示激光点云整体平移方向和大小的第一合向量以及用于表示激光点云整体旋转方向和大小的第二合向量；

所述迭代模块用于将所述第一合向量和所述第二合向量作为搜索方向，并采用基于梯度下降法的地图匹配算法将所述激光点云和所述环境地图进行匹配，生成所述激光点云在所述环境地图中的后验位姿；

其中，所述计算模块具体包括：

变换单元，用于将每个最近点向量按照平行于激光点云中对应光束的方向投影生成平移向量，按照垂直于所述对应光束的方向投影生成第一旋转向量，并将所述第一旋转向量乘以所述对应光束的长度生成第二旋转向量；

求和单元，用于对所有最近点向量的平移向量求和生成用于表示激光点云整体平移方向和大小的第一合向量，对所有最近点向量的第二旋转向量求和生成用于表示激光点云整体旋转方向和大小的第二合向量。

7.根据权利要求6所述基于最近点向量投影的点云匹配装置，其特征在于，所述迭代模块具体用于采用梯度下降法进行迭代时计算所述激光点云和所述环境地图的匹配得分，当所述匹配得分低于前一次的计算结果时，将搜索步长减半并继续迭代，直至满足预设迭代终止条件，并输出对应的后验位姿，所述预设迭代终止条件为：相邻两次迭代的匹配得分变化量低于预设阈值。

8.根据权利要求7所述基于最近点向量投影的点云匹配装置，其特征在于，所述迭代模块采用以下预设公式计算所述激光点云和环境地图的匹配得分：

其中，S表示所述匹配得分，i表示激光点云中的激光点，d_i为每个激光点与最近点的距离，σ表示激光点云的标准差。

Images