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

技术领域

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

背景技术

随着三维激光雷达(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系统在复杂环境中的应用提供了更可靠的地图构建基础。

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

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

步骤一，地图体素化：

将激光雷达扫描到的雷达点通过全局姿态变换转换到全局地图坐标系中，得到三维点

步骤二，体素配准：

获取激光雷达当前扫描到的雷达点对应的三维点，结合三维点到平面约束，对哈希表进行检索，获取当前扫描到的雷达点对应的占用体素的集合，记为局部地图体素集合

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

基于占用体素

遍历所有的占用体素

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

通过预定义的体素大小和全局地图原点，得到三维点

；

分别为三维点/>

再基于局部索引

；

其中，

；

；

；

为中间变量；/>

进一步地，步骤二中，所述将

对于

分配至占用体素

将配准后的

进一步地，步骤二中，占用体素

进一步地，步骤三中，基于占用体素

对于全局地图体素集合

；

其中，

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

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

附图说明

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

具体实施方式

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

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

步骤一，地图体素化：

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

；

其中，

(2)将每个三维点分配到局部索引

；

其中：

；

；

；

为中间变量；/>

(3)对于当前雷达扫描到的雷达点，其相应的占用体素集合称为局部地图体素集合

步骤二，体素配准：

(1)为获得当前雷达扫描到的雷达点更准确的位姿变换，使用点到平面约束，对

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

(2)使用列文伯格-马夸尔特(Levenberg-Marquardt)方法对

(3)

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

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

(1)在雷达扫描配准之后，使用隐式移动最小二乘(IMLS)方法更新每个占用体素的SDF值，对于全局地图体素集合

；

其中

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

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

利用Marching Cube算法提取物体表面时，立方体是在三维图像中由相邻的八个占用体素组成的，其中每个占用体素(除了边界上的之外)都为8个立方体所共享，根据每个占用体素在立方体内位置的不同，将8个占用体素进行编号，编号为0-7，用相同思路为立方体边进行编号，编号为

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

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