CN117095061B - 基于点云强度显著点的机器人位姿优化方法及系统 - Google Patents

Abstract

本发明提供了一种基于点云强度显著点的机器人位姿优化方法及系统，属于智能机器人建图与定位领域。当某有效扫描点与前一个有效扫描点之间的强度差大于设定强度阈值时，判定此有效扫描点为强度显著候选点，对各有效扫描点依次判断，得到所有的强度显著候选点；当某强度显著候选点之前的N个点之间的强度差均小于设定平坦阈值，且此强度显著候选点之后的N个点之间的强度差均小于设定平坦阈值时，判定此强度显著候选点为强度显著点，对各强度显著候选点依次判断，得到所有的强度显著点；根据相邻两帧图像之间的强度显著点，进行ICP匹配，得到两帧间的位姿变换估计结果；本发明提高了SLAM系统的机器人建图精度。

Description

基于点云强度显著点的机器人位姿优化方法及系统

技术领域

本发明涉及智能机器人建图与定位领域，特别涉及一种基于点云强度显著点的机器人位姿优化方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术，并不必然构成现有技术。

智能配送机器人、消毒机器人等一系列自动化产品的发展，加快了对于智能化技术的快速发展，对于室内自动化机器人的需求越来越多，基于激光的SLAM（SimultaneousLocalization And Mapping）领域的大部分研究工作都集中在利用环境几何模型上，激光雷达除了通过扫描得到物体的距离外，还可以获取从激光环境表面反射后得到的强度值，可作为各种机器人应用的附加信息。

与SLAM领域相比，激光强度已被用于人体探测、定位、视觉测程、地形分类和目标跟踪，强度值的信息依赖于反射率以及外部参数，如到表面的距离和相对于表面法线的入射角。

发明人发现，当前一些研究着重于建模外部参数对强度的影响，以实现表面反射率的位态不变测量，构建强度值地图，但是这种方法需要对环境进行强度建模，使得在不同的位置和角度发出入射光，环境中的同一物体能够返回相同的强度值，这种方法建模过程复杂，不利于使用；还有一些研究通过设定强度阈值提取一帧数据中的所有超过强度阈值的点，利用强度值信息构建新的表达式估计机器人姿态之间的转换来提高建图精度，这种方法提取得到的强度显著点较多，需要构建与强度信息相关的约束式，对于SLAM系统，如果只使用几何度量信息，则机器人建图精度较差。

发明内容

为了解决现有技术的不足，本发明提供了一种基于点云强度显著点的机器人位姿优化方法及系统，能够通过提取点云数据中强度显著变化的扫描点进行ICP匹配以获得位姿变换估计，提高了SLAM系统的机器人建图精度，实现了机器人位姿的更高精度优化。

为了实现上述目的，本发明采用如下技术方案：

本发明第一方面提供了一种基于点云强度显著点的机器人位姿优化方法。

一种基于点云强度显著点的机器人位姿优化方法，包括以下过程：

当某有效扫描点与前一个有效扫描点之间的强度差大于设定强度阈值时，判定此有效扫描点为强度显著候选点，对各有效扫描点依次判断，得到所有的强度显著候选点；

根据强度显著候选点得到强度显著点，根据相邻两帧扫描数据之间的强度显著点，进行点云匹配，得到两帧扫描数据间的位姿变换估计结果，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿。

作为本发明第一方面进一步的限定，根据强度显著候选点得到强度显著点，包括：

当某强度显著候选点之前的N个点之间的强度差均小于设定平坦阈值，且此强度显著候选点之后的N个点之间的强度差均小于设定平坦阈值时，判定此强度显著候选点为强度显著点，对各强度显著候选点依次判断，得到所有的强度显著点，其中N为大于1的正整数；

对得到的各强度显著点，将任一强度显著点与前一点的中间位置设为此强度显著点对应的最终强度显著点的位置。

作为本发明第一方面进一步的限定，根据相邻两帧图像之间的强度显著点，采用I-Kd树数据结构进行点云匹配。

作为本发明第一方面进一步的限定，进行点云匹配，包括：对于每个强度显著点对应的匹配误差，将其乘以对应强度显著点的权重，以加权后的匹配误差最小为目标更新匹配变换参数。

作为本发明第一方面更进一步的限定，对任一强度显著点，以不确定性值为权重，包括：

计算当前强度显著点的位置与前一个强度显著点的距离，以所述距离与数值1的加和为第一变量，以某一正常数为第一常量，以第一常量与第一变量的比值的归一化值为当前强度显著点的不确定性值。

作为本发明第一方面更进一步的限定，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿，包括：

对初始帧扫描数据进行体素滤波，基于体素滤波后的扫描数据得到机器人的初始预测位姿，根据初始预测位姿以及位姿变换估计结果，得到更新后的预测位姿；

结合更新后的预测位姿构建子地图，以机器人追踪位姿与子地图原点位姿在世界坐标系下的相对位姿与约束的差值作为第一残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在世界坐标系下的相对坐标变换的差值作为第二残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在强度显著点匹配后得到的相对坐标变换的差值作为第三残差项；

以第一残差项、第二残差项和第三残差项的加和最小为目标，求解后得到一个位姿增量，将位姿增量与更新后的预测位姿相加后，得到优化后的位姿。

本发明第二方面提供了一种基于点云强度显著点的机器人位姿优化系统。

一种基于点云强度显著点的机器人位姿优化系统，包括：

强度显著候选点判断模块，被配置为：当某有效扫描点与前一个有效扫描点之间的强度差大于设定强度阈值时，判定此有效扫描点为强度显著候选点，对各有效扫描点依次判断，得到所有的强度显著候选点；

位姿优化模块，被配置为：根据强度显著候选点得到强度显著点，根据相邻两帧扫描数据之间的强度显著点，进行点云匹配，得到两帧扫描数据间的位姿变换估计结果，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿。

作为本发明第二方面更进一步的限定，位姿优化模块中，根据强度显著候选点得到强度显著点，包括：

当某强度显著候选点之前的N个点之间的强度差均小于设定平坦阈值，且此强度显著候选点之后的N个点之间的强度差均小于设定平坦阈值时，判定此强度显著候选点为强度显著点，对各强度显著候选点依次判断，得到所有的强度显著点，其中N为大于1的正整数；

对得到的各强度显著点，将任一强度显著点与前一点的中间位置设为此强度显著点对应的最终强度显著点的位置。

作为本发明第二方面进一步的限定，位姿优化模块中，根据相邻两帧图像之间的强度显著点，采用I-Kd树进行点云匹配。

作为本发明第二方面进一步的限定，位姿优化模块中，进行点云匹配，包括：对于每个强度显著点对应的匹配误差，将其乘以对应强度显著点的权重，以加权后的匹配误差最小为目标更新匹配变换参数。

作为本发明第二方面更进一步的限定，对任一强度显著点，以不确定性值为权重，包括：

计算当前强度显著点的位置与前一个强度显著点的距离，以所述距离与数值1的加和为第一变量，以某一正常数为第一常量，以第一常量与第一变量的比值的归一化值为当前强度显著点的不确定性值。

作为本发明第一方面更进一步的限定，位姿优化模块中，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿，包括：

对初始帧扫描数据进行体素滤波，基于体素滤波后的扫描数据得到机器人的初始预测位姿，根据初始预测位姿以及位姿变换估计结果，得到更新后的预测位姿；

结合更新后的预测位姿构建子地图，以机器人追踪位姿与子地图原点位姿在世界坐标系下的相对位姿与约束的差值作为第一残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在世界坐标系下的相对坐标变换的差值作为第二残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在强度显著点匹配后得到的相对坐标变换的差值作为第三残差项；

以第一残差项、第二残差项和第三残差项的加和最小为目标，求解后得到一个位姿增量，将位姿增量与更新后的预测位姿相加后，得到优化后的位姿。

与现有技术相比，本发明的有益效果是：

1、本发明利用强度显著点，可以筛选出点云中具有显著强度变化的点，将它们用于点云匹配可以提高匹配的精度，此外，相较于之前的研究，本发明仅利用强度信息来提取强度显著点，获得强度显著点的位姿信息进行匹配，不直接利用强度值信息来进行匹配，可以减少工作的复杂性。

2、本发明采用I-Kd树数据结构进行点云匹配，可以提高最近邻搜索的效率，I-Kd树的增量式构建方式适用于动态点云数据，能够快速地插入新的点并处理点云数据的变化，从而提高匹配的效率。

3、本发明通过根据距离值分析为各强度显著点赋予不同权重，可以更精确地处理匹配点之间的差异，可以更好地区分匹配点的可靠性，从而细化匹配结果并提高匹配的质量，实现了机器人位姿的更高精度优化。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明实施例1提供的连续两帧扫描数据强度示意图；

图2为本发明实施例1提供的提取强度显著候选点的流程示意图；

图3为本发明实施例1提供的强度显著点提取的流程示意图；

图4为本发明实施例1提供的强度显著点匹配流程示意图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

实施例1：

本实施例中，以二维激光SLAM为基础，采用纯激光雷达建图的方式，获取激光雷达扫描数据，对其进行运动补偿，提取强度显著点，将提取出来的强度显著点构建成I-Kd树作为扫描匹配的目标帧，对这些强度显著点进行ICP匹配，为后续的Ceres匹配提供初始变换位姿，进而完成子地图构建；

本实施例中，通过对相邻两帧激光雷达扫描数据提取得到的强度显著点进行匹配，得到位姿变换估计，经过体素滤波以后的点云数据已经完成了时间同步和点云去畸变，在进行第一次扫描匹配（采用第一帧的激光雷达扫描数据）时，通过Cartographer系统中的位姿估计器可以得到传入基于Ceres优化的前端匹配中的机器人的预测位姿pose_prediction_1d；Ceres匹配采用了Ceres库进行优化求解，将匹配转换成最小二乘的问题，建立最小二乘等式，包含3个代价函数，可配置其相关权重，使用基于Ceres库的扫描匹配器，完成激光扫描数据与地图之间的匹配工作，输出最可能的机器人位姿。

在经过扫描匹配后返回的是机器人最可能的位姿pose_estimate_2d；因此，在进行第二次扫描匹配（针对相邻的第二帧激光雷达扫描数据）时，本发明将经过强度显著点进行匹配的位姿变换估计作用到预测位姿pose_estimate_2d上，得到更新后的机器人的预测位姿pose_prediction_1d’来参与下一次的Ceres匹配；

采用连续的几帧激光雷达扫描数据构成子地图（submap），子地图采用概率栅格地图表达，首先将连续空间按照设定的精度栅格化，每一个栅格中都含有一个值代表该栅格被占据的概率，每个扫描点集在插入子图中时，都会先寻找在该子图中的最优位姿，进而插入，这样一帧一帧的激光数据处理，不断更新得到子图；

本实施例中，通过前端匹配构建地图后，由于前端里程计会有累计误差，采用SPA（Sparse Pose Adjustment）优化将这种累计误差减小，寻找一个节点间的配置关系，使得节点间约束的测量概率最大，通过位姿图结构，根据约束与所有的节点，将所有的误差减小，并且平均分散到每个节点上去，以达到减小整体误差的作用。

具体的过程，包括：第一步：确定两个节点在世界坐标系下的相对位姿变换；第二步：通过其他方式再次获取这两个节点的相对位姿变换；第三步：对这两个相对位姿变换的差的最小二乘问题进行求解；第四步：进行求解之后会得到一个增量，将当前位姿加上这个增量后就得到了优化后的位姿。

在未使用强度显著点信息的SLAM系统中，残差项主要有两种，第一残差项：将节点（跟踪位姿）与节点（子图原点位姿）在世界坐标系下的相对位姿与约束的差值；第二残差项：节点（跟踪位姿）与节点（子图原点位姿）间在世界坐标系下的相对坐标变换与相邻两个节点（即相邻两个跟踪位姿）在本地坐标系下的相对坐标变换的差值；

在加入强度显著点匹配之后，构建新的第三残差项：节点（跟踪位姿）与节点（子图原点位姿）间在世界坐标系下的相对坐标变换与相邻2个节点（跟踪位姿）在强度显著点匹配后得到的相对坐标变换的差值，构建残差项最小二乘问题的求解，通过采用Levenberg-Marquadt（L-M）优化求解之后会得到一个增量，将当前的更新的机器人的预测位姿加上这个增量后得到优化后的位姿。

具体的，包括以下过程：

S1：提取强度显著点，通过对每一帧点云中的每个点进行逐个比较，得到强度显著变化的数据点。

分析激光雷达扫描数据的XYZI信息，即每个点的坐标值（X，Y，Z）和强度值I，绘制相邻两帧数据之间的强度图，图1中scan_intensity_1为前一帧点云数据每个点的强度图，scan_intensity_2为当前帧点云数据的强度图，通过比较图像和数据，相邻两帧得到的点云数据的强度信息分布是一致的，这是由于获得前后两帧数据时间间隔非常短，激光雷达扫描得到的信息也就比较一致。

强度值通常与物体的反射特性和表面属性有关，激光雷达扫描到不同材料时会返回不同的强度值。强度值显著的点通常具有更明显的特征，相比于普通点云中的所有点，它们更容易被正确匹配，提高匹配的鲁棒性，强度值显著的点通常对匹配算法提供更强的约束，可以增加位姿估计的准确性，这对于SLAM算法尤其重要，因为准确的位姿估计直接影响地图的建立和路径规划的质量。在此理论基础上，本发明提出提取强度显著变化的点作为特征区域，使用强度显著变化的点作为特征点，通过对这些点的坐标值进行帧间匹配。

本发明与之前的研究不同点就在于：本发明利用激光从一种物体到另外一种不同材料的物体时强度值发生显著变化的特点，并且这种显著变化的点在连续两帧之间是不变的；例如，从墙壁到金属门再到墙壁过度区域，强度值信息会在两种材料的物体的交界处发生明显的梯度变化，反映到数据中就是如图2中箭头所指的上升沿或者下降沿的这种拐点处，因此可以利用这种显著变化的强度点进行两帧数据之间的匹配。

首先介绍强度显著候选点，得到此类点的流程图如图2所示，获取激光扫描点云数据，判断其是否为无效点；对于有效点，比较其与前一个有效点之间的强度差是否超过设定的强度阈值，当其与非NAN（Not a Number）点的前一点的强度差超过阈值时被判定为强度显著候选点，未超过阈值时判定为非强度显著点并将该点的强度值设为零。

其中，NAN点表示一个无效的数字或值，通常是由于数据输入错误、计算错误或其他问题导致的，NAN点可能表示一个不存在的点或者一个超出点云范围的点或者一个无效的坐标值。

确定强度显著点的流程如图3所示，在得到强度显著候选点以后，设定“平坦阈值”，通过对该点前面的五个点进行逐次比较，当这些点两两之间的强度差均不超过平坦阈值时，则进入下一步；对该强度显著侯选点后面的五个点进行逐次比较，当这些点两两之间的强度差也均不超过平坦阈值时，则判定该候选点为强度显著点，也就是说本发明提取得到的强度显著点前后的数据点的强度值必须满足平稳且连续的要求，这种方式提取得到的点才具有可信度，因为在实际环境中两个物体之间的强度值在连续的两帧间一定都会发生同样的强度梯度变化的情况。

本实施例中，可选的，在得到强度显著点以后，将该点与前一点的中间位置设为最终的强度显著点的位置，因为强度显著变化发生在我们找到的这一点与前一点之间，取其中值作为真正的强度变化点是合理的。

S2：利用强度图进行预匹配以提供系统中所用到的Ceres匹配的初始位姿变换估计，使用I-Kd树数据结构进行ICP匹配。

在帧间匹配中，利用两帧数据中的点云强度显著变化点可以提高匹配的精度。这是因为点云强度信息可以提供额外的特征约束，帮助识别物体表面的显著特征，并更准确地匹配对应点。I-Kd树（Incremental K-d Tree）是一种增量式构建的K-d树，通过使用I-Kd树，可以实现高效的最近邻搜索，I-Kd树的增量构建过程允许在保持树结构稳定的同时，灵活地处理动态点云数据的插入和删除操作。

如图4所示，利用点云强度显著变化点进行帧间匹配的一般步骤为：

S2.1：数据预处理，从两个帧的激光雷达扫描数据中提取点云的位置和强度信息；

S2.2：提取显著变化点，比较两个帧的点云强度信息，筛选出在强度上有显著变化的点；

S2.3：构建I-Kd树，将目标帧中的显著变化点的位置信息构建成I-Kd树，I-Kd树的构建过程是增量式的，可以高效地处理动态点云数据；

S2.4：初始化变换矩阵，选择一个初始的变换矩阵作为帧间匹配的初始估计，一般为单位矩阵；

S2.5：迭代ICP匹配，重复以下步骤直到收敛：

a.对应点对选择：对于源帧中的每个显著变化点，使用I-Kd树搜索目标帧中的最近邻点；

b.计算配准误差：对于每个点对，计算其残差（例如欧氏距离）作为配准误差；

c.更新变换矩阵：使用ICP的优化方法（如最小二乘法）来更新变换矩阵，以最小化配准误差；

d.判断收敛：检查更新后的变换矩阵的变化是否小于某个阈值，如果是，则认为ICP收敛，停止迭代；输出结果：返回最终收敛时得到的变换矩阵，该矩阵表示源帧与目标帧之间的最佳刚体变换。

本发明通过利用点云强度显著变化点，可以增加匹配过程中的特征约束，从而提高帧间匹配的精度，这对于场景中存在明显特征变化的情况下特别有效。

S3：对强度显著点的置信度分析，比较强度显著变化时前后两点之间的距离，距离值的大小反应。

当寻找到显著的强度点时，可以计算它与前一个点之间的距离，并根据距离的大小来评估强度变化点的位置不确定性。如果距离较大，说明强度变化点很可能位于这两个点之间的某个位置，对具体位置的确定性较低，因为存在更多的可能性，而如果距离较小，说明强度变化点更有可能是这两个点的中点位置，则对匹配的准确性更有信心，因为位置的概率更大。

因此，距离越大表示位置的不确定性越高，而距离越小则表示强度变化点是这两个点中点位置的概率更大，因此匹配起来更准确，要通过数学式子来评估这种影响，使用一种度量距离和不确定性的方法，使用了线性函数来表示距离和不确定性之间的关系：

设强度点的位置为x，前一个点的位置为x_previous，距离为d=|x-x_previous|，定义不确定性（uncertainty）为U，使用一个递减函数来表示距离越大，不确定性越高的关系如式（1）所示：

U=k/(d+1)（1）

其中，k是一个正常数，用于调节不确定性的衰减速度。接下来，定义匹配准确性（matching accuracy）为A，用一个递增函数来表示距离越小，准确性越高的关系如式（2）所示：

A=1-U

这里假设匹配准确性与不确定性之和为1，这样当不确定性增加时，准确性会减少。

通过上述定义，可以使用这些数学式子来计算和评估距离和不确定性之间的关系，以及匹配准确性的变化情况，根据不确定性赋予不同的权重以提高ICP匹配精度，计算每个点的不确定性值，用前述的不确定性度量方法来计算每个点的不确定性，将不确定性值归一化到[0， 1]的范围，以确保权重处于合适的范围内，归一化可以使用线性归一化或者其他合适的方法，将不确定性值映射到[0，1]区间。

对于每个点，将归一化后的不确定性值作为权重，在ICP算法的优化过程中，考虑每个点的权重来调整匹配误差的影响。可以通过修改ICP算法的目标函数来引入权重，例如，将每个点的权重与匹配误差相乘，以减小具有较大不确定性的点的影响。在ICP算法的迭代过程中，使用带权重的匹配误差来计算最小化目标函数。

具体而言，对于每个点对应的匹配误差，将其乘以对应点的权重，并用加权误差来更新匹配变换参数。通过这种方式，ICP算法将更关注那些具有较低不确定性（权重较高）的点，从而提高匹配的准确性，这样可以避免较大不确定性的点对匹配结果的干扰，使匹配结果更加精确。

实施例2：

本发明实施例2提供了一种基于点云强度显著点的机器人位姿优化系统，包括：

强度显著候选点判断模块，被配置为：当某有效扫描点与前一个有效扫描点之间的强度差大于设定强度阈值时，判定此有效扫描点为强度显著候选点，对各有效扫描点依次判断，得到所有的强度显著候选点；

位姿优化模块，被配置为：根据强度显著候选点得到强度显著点，根据相邻两帧扫描数据之间的强度显著点，进行ICP匹配，得到两帧扫描数据间的位姿变换估计结果，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿。

所述系统的各模块的工作方法与实施例1提供的基于点云强度显著点的机器人位姿优化方法相同，这里不再赘述。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于点云强度显著点的机器人位姿优化方法，其特征在于，包括以下过程：

当某有效扫描点与前一个有效扫描点之间的强度差大于设定强度阈值时，判定此有效扫描点为强度显著候选点，对各有效扫描点依次判断，得到所有的强度显著候选点；

根据强度显著候选点得到强度显著点，包括：

当某强度显著候选点之前的N个点之间的强度差均小于设定平坦阈值，且此强度显著候选点之后的N个点之间的强度差均小于设定平坦阈值时，判定此强度显著候选点为强度显著点，对各强度显著候选点依次判断，得到所有的强度显著点，其中N为大于1的正整数；

对得到的各强度显著点，将任一强度显著点与前一点的中间位置设为此强度显著点对应的最终强度显著点的位置；

根据强度显著候选点得到强度显著点，根据相邻两帧扫描数据之间的强度显著点，进行点云匹配，得到两帧扫描数据间的位姿变换估计结果，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿；

基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿，包括：

对初始帧扫描数据进行体素滤波，基于体素滤波后的扫描数据得到机器人的初始预测位姿，根据初始预测位姿以及位姿变换估计结果，得到更新后的预测位姿；

结合更新后的预测位姿构建子地图，以机器人追踪位姿与子地图原点位姿在世界坐标系下的相对位姿与约束的差值作为第一残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在世界坐标系下的相对坐标变换的差值作为第二残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在强度显著点匹配后得到的相对坐标变换的差值作为第三残差项；

以第一残差项、第二残差项和第三残差项的加和最小为目标，求解后得到一个位姿增量，将位姿增量与更新后的预测位姿相加后，得到优化后的位姿。

2.如权利要求1所述的基于点云强度显著点的机器人位姿优化方法，其特征在于，

根据相邻两帧图像之间的强度显著点，采用I-Kd树数据结构进行点云匹配。

3.如权利要求1所述的基于点云强度显著点的机器人位姿优化方法，其特征在于，

进行点云匹配，包括：对于每个强度显著点对应的匹配误差，将其乘以对应强度显著点的权重，以加权后的匹配误差最小为目标更新匹配变换参数。

4.如权利要求3所述的基于点云强度显著点的机器人位姿优化方法，其特征在于，

对任一强度显著点，以不确定性值为权重，包括：

计算当前强度显著点的位置与前一个强度显著点的距离，以所述距离与数值1的加和为第一变量，以某一正常数为第一常量，以第一常量与第一变量的比值的归一化值为当前强度显著点的不确定性值。

5.一种基于权利要求1-4任一项所述的基于点云强度显著点的机器人位姿优化方法的系统，其特征在于，包括：

强度显著候选点判断模块，被配置为：当某有效扫描点与前一个有效扫描点之间的强度差大于设定强度阈值时，判定此有效扫描点为强度显著候选点，对各有效扫描点依次判断，得到所有的强度显著候选点；

位姿优化模块，被配置为：根据强度显著候选点得到强度显著点，根据相邻两帧扫描数据之间的强度显著点，进行点云匹配，得到两帧扫描数据间的位姿变换估计结果，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿。

6.如权利要求5所述的基于点云强度显著点的机器人位姿优化系统，其特征在于，

根据强度显著候选点得到强度显著点，包括：

当某强度显著候选点之前的N个点之间的强度差均小于设定平坦阈值，且此强度显著候选点之后的N个点之间的强度差均小于设定平坦阈值时，判定此强度显著候选点为强度显著点，对各强度显著候选点依次判断，得到所有的强度显著点，其中N为大于1的正整数；

对得到的各强度显著点，将任一强度显著点与前一点的中间位置设为此强度显著点对应的最终强度显著点的位置。

7.如权利要求5所述的基于点云强度显著点的机器人位姿优化系统，其特征在于，

位姿优化模块中，进行点云匹配，包括：对于每个强度显著点对应的匹配误差，将其乘以对应强度显著点的权重，以加权后的匹配误差最小为目标更新匹配变换参数；

对任一强度显著点，以不确定性值为权重，包括：

计算当前强度显著点的位置与前一个强度显著点的距离，以所述距离与数值1的加和为第一变量，以某一正常数为第一常量，以第一常量与第一变量的比值的归一化值为当前强度显著点的不确定性值。

8.如权利要求5-7任一项所述的基于点云强度显著点的机器人位姿优化系统，其特征在于，

位姿优化模块中，基于位姿变换估计结果以及初始帧扫描数据，得到优化后的机器人位姿，包括：

对初始帧扫描数据进行体素滤波，基于体素滤波后的扫描数据得到机器人的初始预测位姿，根据初始预测位姿以及位姿变换估计结果，得到更新后的预测位姿；

结合更新后的预测位姿构建子地图，以机器人追踪位姿与子地图原点位姿在世界坐标系下的相对位姿与约束的差值作为第一残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在世界坐标系下的相对坐标变换的差值作为第二残差项；

以机器人追踪位姿与子地图原点位姿间在世界坐标系下的相对坐标变换，与相邻两个机器人追踪位姿在强度显著点匹配后得到的相对坐标变换的差值作为第三残差项；

以第一残差项、第二残差项和第三残差项的加和最小为目标，求解后得到一个位姿增量，将位姿增量与更新后的预测位姿相加后，得到优化后的位姿。