CN119152140B - 基于全约束的3d激光slam方法、系统、介质及设备 - Google Patents

Abstract

本发明属于机器人同时定位和建图领域，提供了基于全约束的3D激光SLAM方法、系统、介质及设备，其技术方案为通过计算点云几何约束和强度图像强度约束对优化方向约束强度的贡献值，选择其中贡献值更大的一方约束优化方向，得到对系统的全约束观测，实现几何约束和强度约束互补的强约束系统在退化方向上添加额外约束实现全约束的LIO系统；在系统更新步骤，通过推导强度图像帧间匹配的残差及相应的雅可比矩阵，结合点云点面匹配的残差和雅可比矩阵，共同实现对系统的全约束状态更新。在退化方向上额外引入约束，使用拉格朗日乘子法，修正系统的全约束状态更新。通过以上步骤，实现各方向均稳健的SLAM系统，提高了SLAM系统的稳健性。

Description

基于全约束的3D激光SLAM方法、系统、介质及设备

技术领域

本发明属于机器人同时定位和建图技术领域，尤其涉及基于全约束的3D激光SLAM方法、系统、介质及设备，可以实现在特征稀疏的退化环境下高精度、高稳健性的定位和地图构建。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

近年来，自动驾驶汽车、智能巡检机器人等一系列自动化产品的发展，推动了机器人自主移动技术的发展。同时定位和建图（SLAM）作为其中的关键技术，受到广泛关注。而3D激光SLAM以其精度高的特点，成为SLAM发展的重要方向之一。现有的3D激光SLAM通常会融合惯性测量单元（IMU），为系统提供高频运动数据。但是在长走廊、隧道、室外空旷地区等场景下，由于周围环境特征稀疏，激光雷达在某些方向会发生退化，这会降低系统定位和建图精度甚至会导致SLAM系统发生错误。

目前解决3D激光SLAM退化的主要方法是引入额外的传感器，如相机、轮速计、雷达等。许多3D激光SLAM系统会融合IMU，为系统提供预测值。融合轮速计能提供额外的速度观测，为系统提供更精确的预测值，能提高SLAM系统的性能。融合相机能提高SLAM系统对外界环境的信息获取，但引入更多的传感器意味着引入更多的噪声，并且需要对相机标定，增加了工作量，同时，在光线昏暗的场合相机难以正常工作，不利于SLAM系统对周围环境信息的获取。

发明内容

为了解决上述背景技术中存在的至少一项技术问题，本发明提供基于全约束的3D激光SLAM方法、系统、介质及设备，在特征稀疏的场合，设计了一种融合轮速计以及利用激光雷达的反射强度信息的SLAM系统，能显著提高SLAM系统对周围环境信息的获取，提高SLAM系统的稳健性。

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

本发明的第一方面提供基于全约束的3D激光SLAM方法，包括如下步骤：

获取激光雷达点云数据；

基于激光雷达点云数据得到点面匹配的信息矩阵，对点面匹配的信息矩阵中仅与旋转变量和位移变量相关的子块进行特征分解，得到多个特征向量，将每个特征向量作为一个优化方向，计算点面匹配在各优化方向上的约束贡献值；

基于激光雷达点云数据生成强度图像，通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

比较点面匹配和强度图像帧间匹配在各优化方向上的约束贡献值大小，选择贡献值大的一方，构建得到全约束观测；

对全约束观测进行退化检测，若发生退化，在退化方向上添加额外约束后进行全约束状态更新，否则直接进行全约束状态更新，得到全约束观测系统增量并更新系统状态。

进一步地，基于激光雷达点云数据得到点面匹配的信息矩阵之前，利用系统状态的预测值对每帧点云数据进行去畸变处理，其中，系统状态的预测值的计算方法为：当仅有IMU数据时，根据IMU预测模型进行向前传播得到预测值，当接收到轮速计数据时，通过扩展卡尔曼滤波器融合IMU预测数据和轮速计数据，得到预测值。

进一步地，所述通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值，包括：

在强度图像上选择像素块后，根据给予激光雷达位姿的扰动，计算像素块中心像素对应的激光点投影到强度图像上坐标对该扰动的雅可比矩阵，即为投影梯度，投影梯度在各优化方向上投影得到各优化方向上的投影梯度；

计算像素块在其中心像素点处的二阶矩，用二阶矩的最大特征值对应的特征向量表示像素块的像素梯度；

计算各优化方向上的投影梯度在像素块的像素梯度方向上的投影，即为该像素块对优化方向上的约束贡献值；

计算所有像素块的约束贡献值的和，得到强度图像在各优化方向上的约束贡献值。

进一步地，基于激光雷达点云数据生成强度图像时，通过将激光雷达点云数据按照球面投影模型投影生成强度图像。

进一步地，所述在退化方向上添加额外约束后进行全约束状态更新包括：

获取计算得到的系统状态变量增量中与系统姿态和位置有关的部分；

构建在退化方向上的更新约束，结合系统状态变量增量中与系统姿态和位置有关的部分，得到约束方程；

通过拉格朗日乘子法，计算在引入约束方程条件下重新得到的系统状态增量。

进一步地，在构建全约束观测时，包括计算强度图像的光度误差和点云数据的几何残差；

其中，采用直接法得到两帧强度图像之间的光度误差，然后通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

其中，对于该帧激光雷达点云中的点，在局部地图中搜索与之匹配的面，构建点面之间的距离残差，得到点云数据的几何残差。

进一步地，全约束状态更新，包括如下步骤：

计算点面匹配的残差和强度图像的匹配残差；

基于计算得到的强度图像的匹配残差更新雅可比矩阵；

融合点面匹配的残差和更新后的雅可比矩阵，得到全约束观测的残差和雅可比矩阵；

基于全约束观测的残差和雅可比矩阵得到几何约束的系统增量和强度约束的系统增量，求和后得到全约束观测系统增量。

本发明的第二方面提供基于全约束的3D激光SLAM系统，包括：

数据获取模块，其用于获取激光雷达点云数据；

几何约束模块，其用于基于激光雷达点云数据得到点面匹配的信息矩阵，对点面匹配的信息矩阵中仅与旋转变量和位移变量相关的子块进行特征分解，得到多个特征向量，将每个特征向量作为一个优化方向，计算点面匹配在各优化方向上的约束贡献值；

强度约束模块，其用于基于激光雷达点云数据生成强度图像，通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

全约束构建模块，其用于比较点面匹配和强度图像帧间匹配在各优化方向上的约束贡献值大小，选择贡献值大的一方，构建得到全约束观测；

状态更新模块，其用于对全约束观测进行退化检测，若发生退化，在退化方向上添加额外约束后进行全约束状态更新，否则直接进行全约束状态更新，得到全约束观测系统增量并更新系统状态。

本发明的第三方面提供一种计算机可读存储介质。

一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述所述的基于全约束的3D激光SLAM方法中的步骤。

本发明的第四方面提供一种计算机设备。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述所述的基于全约束的3D激光SLAM方法中的步骤。

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

1、本发明通过构建几何约束和强度约束，通过选择点面匹配和强度图像帧间匹配在优化方向上贡献值更大的一方，构建全约束观测，并对全约束观测进行退化检测和退化处理，实现各方向均得到有效约束的SALM系统。

2、当检测到系统某些方向发生退化时，在退化方向上额外引入约束，使用拉格朗日乘子法，修正系统的全约束状态更新，提高了SLAM系统的稳健性。

3、在系统更新时，通过推导强度图像帧间匹配的残差及相应的雅可比矩阵，结合点云点面匹配的残差和雅可比矩阵，共同实现对系统的全约束状态更新，进一步提高了SLAM系统的稳健性。

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

附图说明

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

图1是本发明实施例提供的基于全约束的3D激光SLAM方法流程图；

图2是本发明实施例提供的机械激光雷达测量模型。

具体实施方式

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

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

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

正如本申请背景技术中提及的融合相机能提高SLAM系统对外界环境的信息获取，但引入更多的传感器意味着引入更多的噪声，并且需要对相机标定，增加了工作量，同时，在光线昏暗的场合相机难以正常工作。激光雷达的点云数据不仅包括几何信息，还有反射强度信息，但利用反射强度信息的工作较少，合理利用反射强度信息能近似达到相机的效果，对SLAM系统的稳健性有很大帮助。

因此，在特征稀疏的场合，本发明设计一种融合轮速计以及利用激光雷达的反射强度信息的SLAM系统，能显著提高SLAM系统对周围环境信息的获取，提高SLAM系统的稳健性，这对3D激光SLAM系统是十分有利的。

实施例一

如图1所示，本实施例提供基于全约束的3D激光SLAM方法，包括如下步骤：

步骤1：根据IMU数据和轮速计数据融合得到系统状态的预测值；

本实施例中，为了提高系统状态的预测值的精准度，在系统中添加轮速计，通过扩展卡尔曼滤波器融合IMU和轮速计的数据，为更新步骤提供更精准的系统状态的预测值。

当仅有IMU数据时，根据IMU预测模型进行向前传播得到预测值，当接收到轮速计数据时，通过扩展卡尔曼滤波器融合IMU预测数据和轮速计数据，得到更精确的预测值，该部分称为IW模块，流程如图2。

步骤2：获取激光雷达点云数据，利用系统状态的预测值对每帧点云数据进行去畸变处理，基于去畸变处理后的点云数据得到点面匹配的信息矩阵，对获取的激光雷达点云数据处理得到强度图像；

对于激光雷达点云数据中的几何信息，首先利用系统状态的预测值对一帧点云进行去畸变处理，并去除距离激光雷达过近和过远的点，通过构建点面匹配，获得点面匹配的信息矩阵，然后对点面匹配的信息矩阵进行特征分解，得到六个特征向量，即为优化方向，之后在几何约束提取器中计算点面匹配在优化方向上的贡献值。

对于激光雷达点云数据的强度信息，首先将收到的当前帧点云按照球面投影模型投影生成强度图像，之后在强度图像中提取像素梯度大于设定阈值的像素块，待下一帧强度图像生成后，采用直接法得到两帧强度图像之间的光度误差，之后在强度约束提取器中计算强度图像帧间匹配在优化方向上的贡献值。

生成强度图像的过程是将激光雷达点云投影生成强度图像，图像像素点的像素值设置为经过处理后的点云的反射强度。

其中，对获取的激光雷达点云数据处理得到强度图像具体包括：

对于固态激光雷达，设置扫描一段时间得到的点云为一帧点云，对于一帧点云，根据固态激光雷达扫描时间达到时的水平分辨率和垂直分辨率，得到强度图像大小为；

获得点云中一个点的坐标，根据球面投影模型，得到点在强度图像上的坐标为：

，

其中，为点在强度图像上的列坐标，为点在强度图像上的行坐标，坐标四舍五入取整数值。

但由于固态激光雷达的扫描方式是非重复扫描，可能会有多个点在强度图像上的坐标相同，为了避免点在后续运动中被遮挡，取最后一个落在该坐标上的点的强度值作为像素值。

对于机械激光雷达，机械激光雷达在垂直方向上有多个激光线束同时扫描，扫描一圈为一帧点云，对于一帧点云构建大小为像素的强度图像，其中，为激光雷达线束数量，为根据激光雷达水平分辨率得到的一个线束扫描一圈的点数。

获得点云中一个点的坐标为。但由于激光雷达的接收器不在激光雷达的中心，如图2所示，在对点进行球面投影时需要考虑到偏移量，偏移后得到点相对于接收器的坐标为，并且，，其中为接收器相对于激光雷达中心的偏移量。

根据球面投影模型，可以得到点在强度图像中的像素坐标为：

，

式中，为激光雷达的水平分辨率，为激光雷达的垂直分辨率，为激光雷达的垂直视野，为点在强度图像上的列坐标，为点在强度图像上的行坐标。

本实施例中，为了避免强度图像过度曝光或低曝光，从而对后续匹配造成影响，需要对强度图像进行白化处理；图像白化是改变图像的平均像素值为0，方差为1，使图像的像素值更加均匀。首先求取强度图像的平均像素值和方差如下式：

，

，

式中，为强度图像中像素点的像素值（点的反射强度），白化后的强度图像的像素值为：

，

白化后的像素值会有正有负，需要把像素值映射到范围[0,255]，通过下式计算映射后的像素值：

，

式中和分别为经过白化处理后的强度图像中最大像素值和最小像素值。通过以上处理，得到最终的强度图像的像素点的像素值，处理后的强度图像的像素值变化更加均匀，能有效减少图像过度曝光或低曝光，便于后续提取特征像素块。

步骤3：结合点面匹配信息矩阵和几何约束提取器得到点面匹配在优化方向约束的贡献值，基于强度图像和强度约束提取器得到强度图像在优化方向约束的贡献值；

由于3D激光SLAM系统具有六个自由度，系统更新时主要在六个方向上更新，称为优化方向。为了使系统在六个方向上均得到较强的约束，同时为了补偿激光雷达测距的退化，在得到几何残差和光度误差后，设计了一种利用激光雷达反射强度信息实现对周围环境的额外观测的方法，构建一个几何约束和强度约束互补的强约束系统。

3D激光SLAM系统具有六个自由度，系统更新时主要在六个方向上更新，称为优化方向。为了使系统在六个方向上均得到较强的约束，在得到几何残差和光度误差后，分别设计了几何约束提取器和强度约束提取器。

几何约束提取器和强度约束提取器分别获得点云点面匹配和强度图像帧间匹配对六个优化方向的约束强弱，并用贡献值表示，贡献值越大表示约束越强。最后从几何约束提取器和强度约束提取器选择在优化方向上贡献值更大的一方，得到对系统的全约束观测。最后对全约束观测进行退化检测，并对退化方向进行处理，使系统在各方向均稳健。

在几何约束提取器中，首先对点面匹配的信息矩阵中仅与旋转变量和位移变量相关的子块进行特征分解，得到六个三维特征向量组成的特征向量矩阵，其中前三个向量仅与选择变量相关，后三个变量仅与位移变量相关。为了便于后续与强度约束提取器得到的贡献值进行比较，将这六个特征向量视为更新过程中主要优化的六个方向。

步骤301：结合点面匹配信息矩阵和几何约束提取器得到点面匹配在优化方向约束的贡献值，包括：

步骤3011、获得匹配成功的点和面的法向量，并得到约束信息矩阵和：

，

，

式中，表示向量的反对称矩阵，|·|表示向量的模。

步骤3012、根据约束信息矩阵和计算匹配成功的点面对优化方向的贡献值矩阵和：

，

，

因贡献值仅有正值，对和中的每一项取绝对值表示每对点面的贡献值。为了减少误差对最终结果的影响，设置贡献值阈值，对于和中的每一项，与比较，将小于的项设置为0，得到最终的贡献值矩阵。贡献值矩阵的每列之和即为一帧点云匹配在对应优化方向上的贡献值。

步骤302：基于强度图像和强度约束提取器得到强度图像在优化方向约束的贡献值，具体包括如下步骤：

步骤3021、在选择像素块后，给予激光雷达位姿（位置和姿态）一个微小扰动，计算像素块中心像素对应的激光点投影到强度图像上坐标对该扰动的雅可比矩阵，即为投影梯度，投影梯度在各优化方向上投影得到各优化方向上的投影梯度；

本实施例中，基于扰动的思想，给予系统位姿一个微小扰动（包含位置和姿态，为六个维度），计算提取的像素块中心像素对应的点投影到强度图像上坐标的变化，即为，为点云投影到强度图像的投影模型函数。通过求在上的投影，得到投影坐标在上的变化情况，即为投影梯度：

。

步骤3022、计算像素块在其中心像素点处的二阶矩，用二阶矩的最大特征值对应的特征向量表示像素块的像素梯度，这比直接使用像素梯度更稳定；

步骤3023、计算优化方向上的投影梯度在像素块信息方向上的投影，得到像素块的贡献值。

但是投影梯度会因点距离激光雷达的远近产生变化，为了规范像素块的贡献值，对投影梯度归一化，得到贡献值为：

·，

为了过滤过小的贡献值，设置阈值，对于小于的贡献值，将其重置为0。

步骤3023、计算所有像素块贡献值的和，得到强度图像对六个优化方向的贡献值为：

。

步骤4：比较点面匹配和强度图像在优化方向上约束的贡献值大小，在各优化方向上选择贡献大的一方约束该优化方向，得到全约束观测；

为了比较强度图像贡献值和点云点面匹配贡献值的约束效果，需要对强度图像贡献值乘以比例系数，使得当点云点面匹配贡献值和强度图像贡献值相等时具有近似的约束效果。为了得到对系统的全约束观测，对于每个优化方向，从几何约束提取器和强度约束提取器中选择贡献值更大的一方约束该优化方向。

在构建全约束观测时，包括计算强度图像的光度误差和点云数据的几何残差；

其中，采用直接法得到两帧强度图像之间的光度误差，然后通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

其中，对于该帧激光雷达点云中的点，在局部地图中搜索与之匹配的面，构建点面之间的距离残差，得到点云数据的几何残差。步骤5：对全约束观测进行退化检测，若发生退化，在退化方向上添加额外约束后进行全约束状态更新，否则直接进行全约束状态更新，得到全约束观测系统增量并更新系统状态。

在某些特殊场合，几何约束和强度约束可能均对某个或某些优化方向约束不足，为了避免在后续状态更新时影响精度，需要对全约束观测进行退化检测和退化处理。在以上步骤已经得到全约束观测在优化方向上的贡献值，在退化检测部分，通过设置退化阈值，如果全约束观测在某个优化方向的贡献值小于阈值，表明系统在这个方向上发生了退化，需要进行退化处理。

在退化处理部分，需要在误差状态迭代卡尔曼滤波更新步骤对系统在退化方向上添加额外的约束。获取更新步骤计算得出的系统状态变量增量中与系统姿态和位置有关的部分为：

，

式中，表示系统姿态增量，表示系统位置增量。

为了约束在退化方向上的更新，构造约束如下：

，

，

式中每一行构成一个约束平面，和为面上的点。在退化方向上，令和为0，在非退化方向上不添加约束。由于上述约束的维度是三维，需要将其展开成六维，得到约束方程如下：

，

，

式中和分别表示旋转和平移的退化方向的个数。

最后通过拉格朗日乘子法，计算在引入约束方程条件下重新得到的系统状态增量，避免系统在退化方向上更新状态变量时发生发散。

本实施例中，为了减少计算量，强度图像的帧间匹配采用直接法，在生成的强度图像上提取像素块。首先提取规定大小的无空洞像素，并且像素梯度大于设定阈值的像素块，为了避免计算量过大，设置像素块提取的最大数量为。得到足够数量的像素块后，保存强度图像和提取的像素块，将该帧强度图像称为特征图。待得到下一帧点云生成的强度图像后（称为匹配图），匹配对应的像素块，通过视觉里程计中的直接法最小化像素块间光度误差，实现对预测值的更新。

在获得点云几何残差和强度图像光度误差形成全约束观测并进行退化检测和退化处理后，需要根据误差状态卡尔曼滤波器更新系统状态，所述全约束状态更新具体步骤如下：

步骤501、生成强度图像，具体的生成过程和步骤2中的一致；

步骤502、特征提取：在生成的强度图像上提取大小为7×7的像素块。首先提取规定大小的无空洞像素，并且像素梯度大于设定阈值的像素块，为了避免计算量过大，设置像素块提取的最大数量为，得到足够数量的像素块后，保存强度图像和提取的像素块，将该帧强度图像称为特征图。待得到下一帧点云生成的强度图像后（称为匹配图），匹配对应的像素块，通过视觉里程计中的直接法最小化像素块间光度误差，实现对预测值的更新。

步骤503、观测部分残差计算：本实施例中，强度图像匹配的残差是特征图所选的像素块内每一个像素点的像素值和匹配图中对应像素点的像素值之差，残差为：

，

式中，为特征图中像素点的像素值，为匹配图中像素点的强度值。

步骤504、观测部分雅可比矩阵计算。本实施例中，强度图像匹配采用视觉里程计中的直接法，根据直接法的定义，可以得到观测的雅可比矩阵为：

，

其中为系统的误差状态变量，系统的状态变量为：

，

，

式中是特殊正交群，指三维空间的旋转，为旋转矩阵，表示IMU在世界坐标系下的姿态，是IMU在世界坐标系下的位移，是IMU在世界坐标系下的速度，和分别是激光雷达和轮速计坐标系到IMU坐标系变换的旋转矩阵，和分别是激光雷达和轮速计安装位置到IMU安装位置的位移，是陀螺仪的零偏，是加速度计的零偏，是重力加速度。

表示对应像素点处的像素梯度，像素梯度分为沿方向上的像素梯度和沿方向上的像素梯度。利用该像素点和与其在方向相邻的两个像素点和计算得到该方向上的像素梯度为：

，

利用该像素点和与其在方向相邻的两个像素点和计算得到该方向上的像素梯度为:

，

为点云投影模型的雅可比矩阵，在矩阵中令，当使用固体激光雷达时，根据固态激光雷达投影模型，经推导得到雅可比矩阵为：

，

使用机械激光雷达时，根据机械激光雷达投影模型，经推导得到投影模型的雅可比矩阵为：

，

是点云三维坐标对系统误差状态的导数，是点云在激光雷达坐标系下的坐标，其在世界坐标系下的坐标为，点云在世界坐标系和激光雷达坐标系下的坐标的转换关系为：

，

式中，是点云测量时刻IMU坐标系变换到一帧点云结束时刻IMU坐标系的旋转矩阵，是对应的位移。和是由IMU和轮速计预测的系统在一帧点云结束时刻的位姿。经过推导得：

，

式中，表示向量的反对称矩阵。

步骤505、融合点面匹配的残差和雅可比矩阵，形成全约束观测的残差和雅可比矩阵。根据误差状态迭代卡尔曼滤波器增益计算方法得到几何约束的系统增量和强度约束的系统增量，得到全约束观测系统增量为：

。

步骤506、若全约束观测没有检测到退化方向，直接使用更新系统状态；若全约束观测在某些方向上发生退化，则在退化方向上引入约束方程，通过拉格朗日乘子法约束在退化方向上的系统状态更新，得到最终的系统状态增量并更新系统状态。

经过以上步骤得到最终的系统状态更新增量，保证系统在各优化方向得到有效约束，实现各方向均稳健的SLAM系统。

实施例二

本实施例提供基于全约束的3D激光SLAM系统，包括：

数据获取模块，其用于获取激光雷达点云数据；

几何约束模块，其用于基于激光雷达点云数据得到点面匹配的信息矩阵，对点面匹配的信息矩阵中仅与旋转变量和位移变量相关的子块进行特征分解，得到多个特征向量，将每个特征向量作为一个优化方向，计算点面匹配在各优化方向上的约束贡献值；

强度约束模块，其用于基于激光雷达点云数据生成强度图像，通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

全约束构建模块，其用于比较点面匹配和强度图像帧间匹配在各优化方向上的约束贡献值大小，选择贡献值大的一方，构建得到全约束观测；

状态更新模块，其用于对全约束观测进行退化检测，若发生退化，在退化方向上添加额外约束后进行全约束状态更新，否则直接进行全约束状态更新，得到全约束观测系统增量并更新系统状态。

实施例三

本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述所述的基于全约束的3D激光SLAM方法中的步骤。

实施例四

本实施例提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述所述的基于全约束的3D激光SLAM方法中的步骤。

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

Claims (10)

1.基于全约束的3D激光SLAM方法，其特征在于，包括如下步骤：

获取激光雷达点云数据；

基于激光雷达点云数据得到点面匹配的信息矩阵，对点面匹配的信息矩阵中仅与旋转变量和位移变量相关的子块进行特征分解，得到多个特征向量，将每个特征向量作为一个优化方向，计算点面匹配在各优化方向上的约束贡献值；

基于激光雷达点云数据生成强度图像，通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

比较点面匹配和强度图像帧间匹配在各优化方向上的约束贡献值大小，选择贡献值大的一方，构建得到全约束观测；

对全约束观测进行退化检测，若发生退化，在退化方向上添加额外约束后进行全约束状态更新，否则直接进行全约束状态更新，得到全约束观测系统增量并更新系统状态。

2.如权利要求1所述的基于全约束的3D激光SLAM方法，其特征在于，基于激光雷达点云数据得到点面匹配的信息矩阵之前，利用系统状态的预测值对每帧点云数据进行去畸变处理，其中，系统状态的预测值的计算方法为：当仅有IMU数据时，根据IMU预测模型进行向前传播得到预测值，当接收到轮速计数据时，通过扩展卡尔曼滤波器融合IMU预测数据和轮速计数据，得到预测值。

3.如权利要求1所述的基于全约束的3D激光SLAM方法，其特征在于，所述通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值，包括：

在强度图像上选择像素块后，根据给予激光雷达位姿的扰动，计算像素块中心像素对应的激光点投影到强度图像上坐标对该扰动的雅可比矩阵，即为投影梯度，投影梯度在各优化方向上投影得到各优化方向上的投影梯度；

计算像素块在其中心像素点处的二阶矩，用二阶矩的最大特征值对应的特征向量表示像素块的像素梯度；

计算各优化方向上的投影梯度在像素块的像素梯度方向上的投影，即为该像素块对优化方向上的约束贡献值；

计算所有像素块的约束贡献值的和，得到强度图像在各优化方向上的约束贡献值。

4.如权利要求1所述的基于全约束的3D激光SLAM方法，其特征在于，基于激光雷达点云数据生成强度图像时，通过将激光雷达点云数据按照球面投影模型投影生成强度图像。

5.如权利要求1所述的基于全约束的3D激光SLAM方法，其特征在于，所述在退化方向上添加额外约束后进行全约束状态更新包括：

获取计算得到的系统状态变量增量中与系统姿态和位置有关的部分；

构建在退化方向上的更新约束，结合系统状态变量增量中与系统姿态和位置有关的部分，得到约束方程；

通过拉格朗日乘子法，计算在引入约束方程条件下重新得到的系统状态增量。

6.如权利要求1所述的基于全约束的3D激光SLAM方法，其特征在于，在构建全约束观测时，包括计算强度图像的光度误差和点云数据的几何残差；

其中，采用直接法得到两帧强度图像之间的光度误差，然后通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

其中，对于激光雷达点云中的点，在局部地图中搜索与之匹配的面，构建点面之间的距离残差，得到点云数据的几何残差。

7.如权利要求1所述的基于全约束的3D激光SLAM方法，其特征在于，全约束状态更新，包括如下步骤：

计算点面匹配的残差和强度图像的匹配残差；

基于计算得到的强度图像的匹配残差更新雅可比矩阵；

融合点面匹配的残差和更新后的雅可比矩阵，得到全约束观测的残差和雅可比矩阵；

基于全约束观测的残差和雅可比矩阵得到几何约束的系统增量和强度约束的系统增量，求和后得到全约束观测系统增量。

8.基于全约束的3D激光SLAM系统，其特征在于，包括：

数据获取模块，其用于获取激光雷达点云数据；

几何约束模块，其用于基于激光雷达点云数据得到点面匹配的信息矩阵，对点面匹配的信息矩阵中仅与旋转变量和位移变量相关的子块进行特征分解，得到多个特征向量，将每个特征向量作为一个优化方向，计算点面匹配在各优化方向上的约束贡献值；

强度约束模块，其用于基于激光雷达点云数据生成强度图像，通过强度图像帧间匹配计算得到强度图像在各优化方向上的约束贡献值；

全约束构建模块，其用于比较点面匹配和强度图像帧间匹配在各优化方向上的约束贡献值大小，选择贡献值大的一方，构建得到全约束观测；

状态更新模块，其用于对全约束观测进行退化检测，若发生退化，在退化方向上添加额外约束后进行全约束状态更新，否则直接进行全约束状态更新，得到全约束观测系统增量并更新系统状态。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一项所述的基于全约束的3D激光SLAM方法中的步骤。

10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7中任一项所述的基于全约束的3D激光SLAM方法中的步骤。