CN117872398B - 一种大规模场景实时三维激光雷达密集建图方法 - Google Patents

Abstract

本发明涉及机器人建图技术领域，公开了一种大规模场景实时三维激光雷达密集建图方法，包括：地图体素化；将激光雷达扫描到的雷达点通过全局姿态变换转换到全局地图坐标系中，得到三维点；基于全局地图原点和体素大小，将三维点分配到全局地图坐标系的体素中；将占用体素在全局地图中的全局索引作为哈希索引，建立哈希表；通过三维点到平面约束，对哈希表进行检索，获取局部地图体素集合中每个占用体素和全局地图体素集合中的每个占用体素，并进行配准；更新占用体素的质心点和法向量；基于点的移动立方体算法实现建图：本发明可以针对噪声和稀疏的雷达数据实时进行高效且准确的三维密集重建，且可以适用于大规模场景重建。

Description

一种大规模场景实时三维激光雷达密集建图方法

技术领域

本发明涉及机器人建图技术领域，具体涉及一种大规模场景实时三维激光雷达密集建图方法。

背景技术

随着三维激光雷达(3D LiDAR)同时定位与建图(Simultaneous Localizationand Mapping，SLAM)系统的发展，其在移动机器人领域的应用逐渐成熟，SLAM系统的基本任务是在未知环境中实时估计机器人的运动轨迹，并构建精准的环境地图。

传统SLAM系统在高质量密集地图构建方面仍然存在挑战，尤其是在处理大规模场景和不规则表面表示的方面。目前截断符号距离函数(Truncated Signed DistanceFunction，TSDF)结合Marching Cube(移动立方体)算法已成为一种常见的三维(3D)密集映射方法，然而在大规模场景中，对整个三维(3D)空间进行体素化会导致TSDF体素的维护效率下降，影响计算性能。此外，传统方法在计算有向距离场(signed distance field，SDF)值时常采用射线投影机制，对3D LiDAR噪声和稀疏性较为敏感，容易产生误差，且未充分利用表面法线信息。

现有技术中，SLAMesh(Real-time LiDAR Simultaneous Localization andMeshing，实时激光雷达定位与网格化模型构建)等方法专注于处理被占用的体素，通过投影和高斯过程估计表面函数，取得了显著的映射效率提升。然而，高密度的高斯过程计算仍然存在较大的计算负担，并且面生成是在每个体素中独立进行的，难以满足表面的平滑性要求。

发明内容

为解决上述技术问题，本发明提供一种大规模场景实时三维激光雷达密集建图方法，旨在结合基于TSDF的方法和类似SLAMesh的方法的优势，通过专注于被占用的体素来提高计算效率，并通过隐式移动最小二乘(Implicit Moving Least Squares，IMLS)表面表示方法计算SDF值，充分利用激光雷达点云和法线信息，以实现更高精度的密集映射。同时，本发明采用Marching Cube算法生成平滑、准确且少冗余的密集表面，为SLAM系统在复杂环境中的应用提供了更可靠的地图构建基础。

为解决上述技术问题，本发明采用如下技术方案：

一种大规模场景实时三维激光雷达密集建图方法，包括以下步骤：

步骤一，地图体素化：

将激光雷达扫描到的雷达点通过全局姿态变换转换到全局地图坐标系中，得到三维点；基于全局地图原点和体素大小，将三维点分配到全局地图坐标系的体素中，被分配三维点的体素称为占用体素，得到全局地图体素集合/>，将占用体素在全局地图中的全局索引作为哈希索引，建立哈希表；通过快速奇异值分解得到占用体素的质心点和法向量，进而得到的每个占用体素的参数，占用体素的参数包括被分配至占用体素内的三维点集、占用体素在全局地图中的全局索引、占用体素质心点和占用体素法向量；

步骤二，体素配准：

获取激光雷达当前扫描到的雷达点对应的三维点，结合三维点到平面约束，对哈希表进行检索，获取当前扫描到的雷达点对应的占用体素的集合，记为局部地图体素集合；将/>中每个占用体素/>和/>中的每个占用体素/>进行配准，具体包括：将/>中所有三维点的约束项相加形成代价函数，并利用列文伯格-马夸尔特法对代价函数进行参数优化，通过优化后的参数对激光雷达扫描的初始位姿进行校正；并更新占用体素/>的质心点和法向量/>；/>，/>，/>、/>分别为/>、/>中的占用体素的总数量；

步骤三，基于点的移动立方体算法实现建图：

基于占用体素的质心点和法向量，使用隐式移动最小二乘表面表示方法更新每个占用体素/>的有向距离场值；

遍历所有的占用体素，通过移动立方体算法进行如下操作：将占用体素/>和周围七个更新过有向距离场值的占用体素作为顶点，构建一个立方体；根据立方体顶点的有向距离场值在立方体每个边上进行插值，生成物体表面的顶点，提取有向距离场值为零的物体表面，实现建图。

进一步地，步骤一中，所述基于全局地图原点和体素大小，将三维点分配到全局地图坐标系的体素中，被分配三维点的体素称为占用体素，将占用体素在全局地图中的全局索引作为哈希索引，建立哈希表，具体包括：

通过预定义的体素大小和全局地图原点，得到三维点所属的占用体素的局部索引/>：

；

分别为三维点/>的x坐标值、y坐标值和z坐标值；/>,/>,/>分别为全局地图中的占用体素的最小x坐标值、最小y坐标值和最小z坐标值；/>为占用体素大小；/>和分别为沿x坐标方向和沿y坐标方向的占用体素数量，/>表示向下取整；

再基于局部索引得到占用体素的全局索引/>，将占用体素的全局索引/>作为哈希索引，实现哈希表的建立：

；

其中，

；

；

；

为中间变量；/>表示取模运算；/>是哈希索引的系数。

进一步地，步骤二中，所述将中所有三维点的约束项相加形成代价函数，并利用列文伯格-马夸尔特法对代价函数进行参数优化，通过优化后的参数对激光雷达扫描的初始位姿进行校正，并更新占用体素/>的质心点和法向量，具体包括：

对于中的第/>个三维点/>的约束项/>，将/>中所有三维点的约束项相加形成代价函数，并用列文伯格-马夸尔特法对代价函数的参数/>优化，并使用最优的参数/>来校正激光雷达扫描的初始位姿，实现占用体素/>和占用体素/>的配准；其中，上标/>表示转置；

分配至占用体素的三维点集记为/>，分配至占用体素/>的三维点集记为/>；

将配准后的合并到/>中，并通过直接将/>中的三维点添加到/>中实现占用体素的质心点和法向量的更新。

进一步地，步骤二中，占用体素与占用体素/>每配准K次，进行一次占用体素的质心点和法向量的更新；K为设定的数值。

进一步地，步骤三中，基于占用体素的质心点和法向量，使用隐式移动最小二乘表面表示方法更新每个占用体素/>的有向距离场值时，具体包括：

对于全局地图体素集合中的占用体素/>，令/>表示/>的质心点，计算/>对应的有向距离场值/>：

；

其中，为分配到占用体素/>的三维点集，/>表示三维点集/>中的第/>个三维点，/>是点/>的法向量，/>是给定/>时计算的/>的权重。

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

本发明可以针对噪声和稀疏的LiDAR数据实时进行高效且准确的3D密集重建，且可以适用于大规模场景重建；可以通过动态去除不稳定体素而有效地去除动态物体产生的鬼影区域；提出的基于点云的Marching Cube算法只在占用空间中体素化，且通过IMLS隐式表示只更新体素中心的SDF值，在大规模环境下效率较高；遍历更新所有体素，针对每一个体素，根据其相邻体素生成特定TSDF立方体，对其立方体执行Marching Cube算法生成顶点和面，生成的密集表面更加平滑、精确且冗余较少。

附图说明

图1为本发明的流程示意图。

具体实施方式

下面结合附图对本发明的一种优选实施方式作详细的说明。

本发明提供了一种大规模场景实时三维激光雷达密集建图方法，总体流程如图1所示，包括以下步骤：

步骤一，地图体素化：

(1)每个体素由预定义的全局地图原点、体素大小以及离散化的三维坐标系唯一确定，对于当前雷达扫描中的每个已通过全局姿态变换估计转换到全局地图坐标系的三维点计算其对应的局部索引/>：

；

其中，为三维点/>的x坐标值、y坐标值和z坐标值；/>,/>,/>是全局地图中占用体素的最小x坐标值、最小y坐标值和最小z坐标值；/>为占用体素大小；/>和/>分别为沿x坐标和沿y坐标方向的占用体素数量，/>表示向下取整，上标/>表示转置。

(2)将每个三维点分配到局部索引对应的体素中，完成雷达点到体素的映射，完成后当前雷达扫描被表示为一组占用体素，每个占用体素都有相应的三维点，对于每个占用体素，计算其在全局地图中的全局索引/>作为哈希索引：

；

其中：

；

；

；

为中间变量；/>是哈希索引的系数。

(3)对于当前雷达扫描到的雷达点，其相应的占用体素集合称为局部地图体素集合，其中每个占用体素/>包含对应的全局索引/>和已分配至占用体素/>的三维点集/>；对于全局地图，其相应的体素集合称为全局地图体素集合/>，其中每个占用体素/>包含全局索引/>、已分配至占用体素/>的三维点集/>、占用体素/>的质心点/>和法向量/>。/>、/>分别为/>、中的占用体素的总数量。

步骤二，体素配准：

(1)为获得当前雷达扫描到的雷达点更准确的位姿变换，使用点到平面约束，对和/>进行精准配准，定义/>中第/>个三维点/>的约束项/>，其中/>是待优化参数，也称为位姿变换矩阵，将所有三维点的约束项相加，得到代价函数。

点云配准的工作原理是激光雷达由于受到环境等各种因素的限制，在点云采集过程中单次采集到的点云只能覆盖目标物表面的一部分，为了得到完整的目标物点云信息，就需要对目标物进行多次扫描，并将得到的点云进行坐标系的刚体变换，把目标物上的局部点云转换到同一坐标系下。

(2)使用列文伯格-马夸尔特(Levenberg-Marquardt)方法对进行优化，以最小化代价函数，优化后的/>用于校正初始位姿，以获得更准确的雷达扫描的位姿变换，进而实现/>和/>的配准。

(3)和/>精准配对后，将/>合并到/>中，对于每个合并后的占用体素，将/>中的点云直接添加到/>中，并更新合并后的占用体素的质心点和法线。

(4)对于仅由当前扫描观察到的新体素，将其直接添加到全局地图中，在每进行K次配准后执行一次占用体素的更新以提高计算效率。

步骤三，基于点的移动立方体算法实现建图：

(1)在雷达扫描配准之后，使用隐式移动最小二乘(IMLS)方法更新每个占用体素的SDF值，对于全局地图体素集合中的占用体素/>，令/>表示/>的质心点，计算/>对应的SDF值/>：

；

其中是三维点/>的法向量，/>是给定/>时计算的/>的权重，/>表示三维点集/>中的第/>个三维点。

(2)根据更新后的SDF值，设置相邻的七个更新后的占用体素为立方体的顶点，为避免重复计算，仅当所有顶点具有有效的SDF值时立方体才有效，该立方体的顶点被用于应用Marching Cube算法。

(3)通过迭代，对所有占用体素执行Marching Cube算法，对每个边上的SDF值进行插值，生成面的顶点，最终提取SDF值为零的物体表面，同时筛除掉点数不够或被观察次数较少的占用体素以保证映射质量，仅计算当前视点一定距离内的占用体素以提高映射效率，此外，在线密集映射的网格模型每20帧更新一次，以平衡实时性和视觉效果。

利用Marching Cube算法提取物体表面时，立方体是在三维图像中由相邻的八个占用体素组成的，其中每个占用体素(除了边界上的之外)都为8个立方体所共享，根据每个占用体素在立方体内位置的不同，将8个占用体素进行编号，编号为0-7，用相同思路为立方体边进行编号，编号为-/>；将SDF值为负的占用体素称为实点，将SDF值为正的占用体素称作虚点，立方体的8个占用体素每个都可能是实点或虚点，因此一个立方体一共有2的8次方，即256，种可能的情况，利用256种可能情况进行立方体内等值三角面抽取；八个占用体素均为实点的称为实立方体，八个占用体素均为虚点的称为虚立方体，既含有实体元又含有虚体元的称为边界立方体，在边界立方体中用三角形拟合等值面，实现SDF值为零的物体表面的提取。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立技术方案，说明书的这种叙述方式仅仅是为了清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (5)

1.一种大规模场景实时三维激光雷达密集建图方法，包括以下步骤：

步骤一，地图体素化：

将激光雷达扫描到的雷达点通过全局姿态变换转换到全局地图坐标系中，得到三维点；基于全局地图原点和体素大小，将三维点分配到全局地图坐标系的体素中，被分配三维点的体素称为占用体素，得到全局地图体素集合/>，将占用体素在全局地图中的全局索引作为哈希索引，建立哈希表；通过快速奇异值分解得到占用体素的质心点和法向量，进而得到的每个占用体素的参数，占用体素的参数包括被分配至占用体素内的三维点集、占用体素在全局地图中的全局索引、占用体素质心点和占用体素法向量；

步骤二，体素配准：

获取激光雷达当前扫描到的雷达点对应的三维点，结合三维点到平面约束，对哈希表进行检索，获取当前扫描到的雷达点对应的占用体素的集合，记为局部地图体素集合；将中每个占用体素/>和/>中的每个占用体素/>进行配准，具体包括：将/>中所有三维点的约束项相加形成代价函数，并利用列文伯格-马夸尔特法对代价函数进行参数优化，通过优化后的参数对激光雷达扫描的初始位姿进行校正；并更新占用体素/>的质心点/>和法向量/>；/>，/>，/>、/>分别为/>、/>中的占用体素的总数量；

步骤三，基于点的移动立方体算法实现建图：

基于占用体素的质心点和法向量，使用隐式移动最小二乘表面表示方法更新每个占用体素/>的有向距离场值；

遍历所有的占用体素，通过移动立方体算法进行如下操作：将占用体素/>和周围七个更新过有向距离场值的占用体素作为顶点，构建一个立方体；根据立方体顶点的有向距离场值在立方体每个边上进行插值，生成物体表面的顶点，提取有向距离场值为零的物体表面，实现建图。

2.根据权利要求1所述的大规模场景实时三维激光雷达密集建图方法，其特征在于，步骤一中，所述基于全局地图原点和体素大小，将三维点分配到全局地图坐标系的体素中，被分配三维点的体素称为占用体素，将占用体素在全局地图中的全局索引作为哈希索引，建立哈希表，具体包括：

通过预定义的体素大小和全局地图原点，得到三维点所属的占用体素的局部索引/>：

；

分别为三维点/>的x坐标值、y坐标值和z坐标值；/>,/>,/>分别为全局地图中的占用体素的最小x坐标值、最小y坐标值和最小z坐标值；/>为占用体素大小；/>和/>分别为沿x坐标方向和沿y坐标方向的占用体素数量，/>表示向下取整；

再基于局部索引得到占用体素的全局索引/>，将占用体素的全局索引/>作为哈希索引，实现哈希表的建立：

；

其中，

；

；

；

为中间变量；/>表示取模运算；/>是哈希索引的系数。

3.根据权利要求1所述的大规模场景实时三维激光雷达密集建图方法，其特征在于：步骤二中，所述将中所有三维点的约束项相加形成代价函数，并利用列文伯格-马夸尔特法对代价函数进行参数优化，通过优化后的参数对激光雷达扫描的初始位姿进行校正，并更新占用体素/>的质心点和法向量，具体包括：

对于中的第/>个三维点/>的约束项/>，将/>中所有三维点的约束项相加形成代价函数，并用列文伯格-马夸尔特法对代价函数的参数/>优化，并使用最优的参数/>来校正激光雷达扫描的初始位姿，实现占用体素/>和占用体素/>的配准；其中，上标/>表示转置；

分配至占用体素的三维点集记为/>，分配至占用体素/>的三维点集记为/>；

将配准后的合并到/>中，并通过直接将/>中的三维点添加到/>中实现占用体素的质心点和法向量的更新。

4.根据权利要求1所述的大规模场景实时三维激光雷达密集建图方法，其特征在于，步骤二中，占用体素与占用体素/>每配准K次，进行一次占用体素/>的质心点和法向量的更新；K为设定的数值。

5.根据权利要求1所述的大规模场景实时三维激光雷达密集建图方法，其特征在于，步骤三中，基于占用体素的质心点和法向量，使用隐式移动最小二乘表面表示方法更新每个占用体素/>的有向距离场值时，具体包括：

对于全局地图体素集合中的占用体素/>，令/>表示/>的质心点，计算/>对应的有向距离场值/>：

；

其中，为分配到占用体素/>的三维点集，/>表示三维点集/>中的第/>个三维点，/>是点/>的法向量，/>是给定/>时计算的/>的权重。