基于固态雷达的SLAM研究与实现

胡丽娜 曹政

摘要：未知环境下的地图构建技术是智能机器人实现路径规划、智能导航的前提与关键。考虑到目前传感器领域的发展态势，文章主要针对固态雷达的SLAM算法进行探究，使用览沃Hap雷达搭建平台采集数据并验证算法。首先，文章对研究背景以及近年来相关传感器、算法进行了介绍。其次，这篇文章对算法的整体流程进行了概述，并对基于主成分分析（Principal Component Analysis，PCA） 和点云分布的特征提取、基于因子图的位姿估计技术细节进行了介绍。最后，使用实车采集了实验数据，并以效果图和相对位姿误差（Relative Pose Error，RPE） 定性、定量地对结果进行了分析。结果表明，此文提出的方法具备一定的实用价值。

关键词：机器人；自动驾驶；同步定位与建图；固态雷达；前端里程计

中图分类号：TP391      文献标识码：A

文章编号：1009-3044（2023）31-0125-03

开放科学（资源服务）标识码（OSID）

0 引言

同时定位与地图构建，即Simultaneous Localization and Mapping （简称SLAM） ，是一种在机器人、计算机视觉领域广泛应用的关键技术[1]。该算法的目标是让机器人在位置环境中估计自身位置并构建环境地图，无须事先知晓所处环境的结构或地图信息。它是无人驾驶车辆、无人飞行器、工业自动化等领域智能系统实现自主感知、决策与行动的核心问题[2]。在实现该技术的基础之上，才能使机器、智能系统能够理解并与复杂的现实世界互动，才能为各个领域带来更高效、更安全、更智能化的解决方案。使用单个传感器实现SLAM大体上可分为两个方向：基于相机（视觉）的SLAM技术[3]与基于激光雷达SLAM技术[4]。一般来说，视觉SLAM成本较低，但是易受到环境光照、天气等因素影响导致建图精度不高。激光雷达因其能够感知距离、不受环境因素干扰的特性以及逐年降低的价格，使其成为当前SLAM框架中主流的传感选择。

当前基于激光雷达的SLAM技术多采用旋转式激光雷达，它通过旋转机构在不同的方向上进行多次扫描，形成一个水平或垂直切片的激光点云。基于这一特性，LOAM[5]通过在点云数据每一个水平环线上提取角点、面点，并构建点到线与点到面距离来估计相邻帧间的位姿变化，从而完成地图的构建。LeGO-LOAM[6]在前者的基础上增加了地面约束以获取更高的建图精度。近年来，固态雷达相对于旋转式雷达，以其可靠性高、体积小、功耗低等优势引起了从业人员的广泛关注。基于固态雷达的相关研究尚处于起步阶段，本文使用览沃Hap固态激光雷达采集实验数据，并利用主成分分析法（Principal Component Analysis， PCA） [7]和点云分布特性提取特征实现地图构建，随后与惯导设备实测数据对比进行误差分析，最后对误差和建图效果进行分析。

1 算法设计

1.1 算法整体框架

基于固态雷达的建图系统框架如图1所示，算法部分主要包括特征提取与特征筛选、前端里程计、IMU（惯性测量单元）预积分[8]、GTSAM优化[9]与全局地图五个部分。

1） 特征提取与特征筛选：Hap雷达采集的点云信息，经由该算法提取点云特征，并对所提取的特征进行筛选、过滤。

2） 前端里程计：特征提取阶段获取的特征，经由该算法，解算出Hap雷达点云相邻两帧相对位姿[Rp=x y z θr θp θy]。

3） IMU预积分：惯导IMU单元测得的加速度、角速度信息，经由该算法估算机器人相對移动，辅助前端里程计以获取更高精度位姿。

4） GTSAM优化：前端里程计及IMU预积分计算得到的位姿，经由该算法进行图优化，解算全局最优位姿。

5） 全局地图：点云数据按求得的最优位姿进行堆叠形成全局地图（如图1） 。

1.2 硬件设备

实现数据采集及算法部署的设备配置如下：使用览沃Hap固态雷达生成点云数据，如图2所示；使用英特尔志强E52680 v2处理器、8G内存、操作系统为ubuntu 18.04的电脑，完成点云、IMU、GNSS（真值）数据的采集及建图算法的运行。

1.3 基于PCA与点云分布的特征提取

由于Hap固态雷达旋镜式成像特点，相比旋转式机械雷达缺少传统的帧与环线的概念。因此，本文选择使用PCA完成点云特征的提取。首先，对于点云P中一个点[pi]，以距离L将点云划分为k个簇，并对每个簇计算其中心，公式如下（1） ：

[pki=1ninpki] （1）

随后，对于每一个簇计算协方差矩阵Cov及其特征值，公式如下（2） ：

[Cov=1ni=1n（pki-pki）⋅（pki-pki）T，Cov⋅eij=λij⋅eij， j∈{0，1，2}.]  （2）

当[λi0-λi1λi0]与[λi1-λi2λi0]满足设定阈值时，这些点被分别选为角、面特征点。此外，将所有提取特征送入前端里程计算会增加计算时长、降低算法效率。因此，出于点云近处稠密，不需要过多特征点，远处稀疏，点云发散不适宜用作特征的考虑，本文对距离雷达60米以内以及100米以外的特征点进行随机滤除，对该区域以内的特征点进行保留。通过上述方式，完成了特征的提取与筛选。

1.4 基于GTSAM的地图构建

特征提取完成之后，则根据角点特征和面点特征分别构建线到线距离与面到面距离，来求解一个合适的相对位姿使得两帧点云之间的上述距离达到最小。此外，惯导采集得到的加速度与角速度数据，在一段时间内做积分处理，可以分别得到该时间段内机器人在位移、欧拉角上的变化程度。最后，本文采用因子图模型，并使用增量平滑优化库GTSAM，将上述由里程计算法求得的帧间相对位姿和IMU预积分得到的位姿进行融合求解最优结果。该过程的基本思想是将多状态的联合估计，转化为一个求解联合概率分布函数的最大后验概率分布值的问题，可以用数学公式表达为（3） ：

[P（X，O，U）=iLP（oi|xk，xk-1）jMPuj|xk，xk-1] （3）

其中，X、O、U分别代表待优化的位姿、里程计因子和预积分因子。在计算出每一时刻最优位姿之后，将当前固态雷达获取的点云与上一时刻获取点云进行拼接，可累加式地得到点云地图。

2 实验与评估

为了验证提出方法的建图效果，本文在乘用车车顶架设固态雷达，并在校园内采集了一段路径总长487m的数据。采集区域为学生宿舍区，包含宿舍楼、小广场、树丛等特征较为复杂的场景。最终的建图效果如图3所示。

图3中，图a与图b分别是建图整体效果的俯视视角和侧视视角。从整体上来看，生成的点云地图未产生扭曲、形变的情况，未出现明显的漂移现象。图c是对广场区域和宿舍区域的特写，从该图中可以看出不管是广场中的灯柱还是宿舍立柱，其轮廓均横平竖直，无任何重叠。图d是地图中树林的特写，它们同样外形清晰易辨别，无重影、鬼影的情况。上述可视化结果均表明，本文提出的方法具备较高的建图精度。

此外，除了使用可视化手段对结果进行定性展示，我们还使用相对位姿误差（Relative Pose Error，RPE） [10]对结果进行了定量分析。其计算方式为（4） ：

[rpei=T-1i∙Ti-1-1E-1i∙Ei-1] （4）

它的含义是相邻位置真值T间相对位移和相邻估计位置E间相对位移之间的误差，反映的是位姿估计的准确程度。根据这一准则，在该段数据上本文算法的各项指标如表1所示：

从表1中可以看出，帧间平均RPE为0.483m，即物体在这段路径长487m的点云地图中的位置与实际位置的平均误差在半米以内。最小误差为0.076m，即建图的最小误差为7cm，这反映出本文提出的前端里程计的有效性。最大误差0.844m，这体现出前端里程计自身存在的累积误差问题。由于里程计的机理是计算相邻帧间的相对位置变化，每一时刻估计位置与实际位置都存在出入，导致误差随轨迹长度累积。

3 结束语

本文基于览沃Hap雷达设计了一种建图方法，通过PCA方法和Hap雷达点云分布特点提取特征，并实车采集数据对提出方法进行了验证。定性、定量的实验结果表明，该算法具备较高质量的建图性能，建图效果清晰、无重影，在一定距离范围内能保证较高的建图精度。实验也表明该算法同样存在其他激光雷达里程计所存在的不足，即长距离建图时地图精度会受累积误差的影响。在未来，可以通过设计新的适用于固态雷达的回环算法，通过构建跨帧间的位置关系修正地图，减小误差。

参考文献：

[1] GRISETTI G，KÜMMERLE R，STACHNISS C，et al.A tutorial on graph-based SLAM[J].IEEE Intelligent Transportation Systems Magazine，2010，2（4）：31-43.

[2] 赵梦成，黎昱宏，张宏宇.ROS的服务类移动机器人SLAM导航的研究[J].电脑知识与技术，2020，16（9）：274-276.

[3] ABASPUR K，AZEROUNI I，FITZGERALD L，DOOLY G，et al.A survey of state-of-the-art on visual SLAM[J].Expert Systems with Applications，2022，205：117734.

[4] 袁國帅，齐咏生，刘利强，等.一种基于因子图消元优化的激光雷达视觉惯性融合SLAM方法[J].电子学报，2023（8）：1-11.

[5] ZHANG J，SINGH S.LOAM：lidar odometry and mapping in real-time[C]//Robotics：Science and Systems X.Robotics：Science and Systems Foundation，2014.

[6] SHAN T X，ENGLOT B.LeGO-LOAM：lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.ACM，2018：4758-4765.

[7] 刘亚洲，邓安健.基于激光雷达的智慧交通信息采集算法[J].测绘与空间地理信息，2023，46（8）：93-97，102.

[8] 贾晓雪，赵冬青，肖国锐，等.基于精化预积分的GNSS/IMU/视觉多源融合定位方法[J].北京航空航天大学学报，2023（9）：1-10.

[9] ZHENG T T，WANG F，XU Z.An improved gtsam-based nonlinear optimization algorithm in ORBSLAM3[C]//2022 International Conference on Intelligent Transportation，Big Data & Smart City （ICITBS）.IEEE，2022.

[10] WU J Y，HUANG S H，YANG Y X，et al.Evaluation of 3D LiDAR SLAM algorithms based on the KITTI dataset[J].The Journal of Supercomputing，2023，79（14）：15760-15772.

【通联编辑：光文玲】