基于柱状特征与随机有限集的激光SLAM方法研究

廖光亮 1，傅春耘 1?，于水恩 2，李健 3，王建文 3; LIAO Guangliang1，FU Chunyun1?，YU Shuien2，LI Jian3，WANG Jianwen3

网刊加载中。。。

使用Chrome浏览器效果最佳，继续浏览，你可能不会看到最佳的展示效果，

确定继续浏览么?

复制成功，请在其他浏览器进行阅读

基于柱状特征与随机有限集的激光SLAM方法研究 PDF

- ORCID：
廖光亮 ¹
- ORCID：
傅春耘 ¹
✉
- ORCID：
于水恩 ²
- ORCID：
李健 ³
- ORCID：
王建文 ³

1. 重庆大学机械与运载工程学院，重庆 400044； 2. 重庆长安工业（集团）有限责任公司，重庆 401120； 3. 重庆长安汽车股份有限公司，重庆 400023

中图分类号： TP242.6

最近更新：2025-03-03

DOI： 10.16339/j.cnki.hdxbzkb.2025158

摘要

传统的基于数据关联的同时定位与建图（simultaneous localization and mapping，SLAM）方法易引起观测与目标之间的误匹配，进而导致位姿估计精度下降. 结合柱状特征提取方法和随机有限集理论，提出一种基于序贯蒙特卡罗实现的车辆3D激光SLAM方法. 利用M估计抽样一致性算法从分割后的点云中提取稳定的柱状特征，捕获单帧点云中的静态存活特征和新生特征；在Rao-Blackwellized-概率假设密度同时定位与建图（Rao-Blackwellized-probability hypothesis density-simultaneous localization and mapping，RB-PHD-SLAM）框架中引入两种特征，并运用序贯蒙特卡罗方法完成车辆轨迹概率密度和地图后验强度在帧间的传递，实现对环境特征和车辆位姿的同时估计. 模拟数据集和KITTI数据集试验结果显示，与经典的FastSLAM算法相比，本文算法使车辆定位精度提升44.99%，并使环境特征位置估计和环境特征数量估计的平均误差分别降低49.24%和56.22%，显著提升了SLAM的运行精度和鲁棒性，有助于保障智能汽车的运行安全.

关键词

智能车辆; 位置测量; 特征提取; 随机有限集; 序贯蒙特卡罗

在智能驾驶领域中，同时定位与建图（simultaneous localization and mapping，SLAM）是指未知环境中的车辆利用特定传感器获取环境信息，同时创建地图模型并估计自身运动的过程^［

1］. 与视觉传感器相比，激光雷达不受环境光照影响，能更准确测量环境深度信息，因此激光SLAM在自动驾驶领域得到广泛应用^{［参考文献 2-3}2-3］.

传统的基于滤波器的SLAM方法通常需要建立观测数据和特征状态之间的数据关联. 这些方法通常使用不同的滤波器估计车辆位姿，然后为每个特征分配一个扩展卡尔曼滤波器（extended Kalman filter， EKF）来估计地图特征的状态. 一些经典算法通常基于数据关联的方法实现，如EKF-SLAM算法^［

4］、FastSLAM算法^{［参考文献 5

百度学术}5］. 常用的数据关联方法包括最近邻数据关联（nearest neighbor data association， NNDA）^{［参考文献 6

百度学术}6］，联合相容分支定界（joint compatibility branch and bound， JCBB）^{［参考文献 7

百度学术}7］等. 采用传感器对智能汽车的行驶环境进行检测时，采集的数据常常会受到杂波的影响，基于数据关联的SLAM算法很难在这些杂波的影响下表现出较好的性能^{［参考文献 8-9}8-9］. 因此，在杂波影响下如何保证车辆良好的定位和建图效果成为SLAM领域的关键问题之一. 这一问题可以通过随机有限集（random finite sets， RFS）^{［参考文献 9

百度学术}9］的建模方法解决. 随机有限集可表示为由多个随机变量组成的集合，该集合的特点是变量个数和状态都是随机且无序的. 在基于RFS的SLAM算法中，地图特征和传感器量测将不再被建模为向量，而是采用有限集的形式进行描述. 因此消除了地图特征与传感器观测之间的数据关联过程，从而避免了由数据关联错误导致的定位和建图效果不佳的问题. 然而，地图特征和车辆轨迹的联合后验密度是耦合的，这制约了基于随机有限集的SLAM方法的实际应用. 因此， Grisetti等^{［参考文献 10

百度学术}10］、Adams等^{［参考文献 11

百度学术}11］采用Rao-Blackwellized粒子滤波器（Rao-Blackwellized particle filter， RBPF）对其进行解耦，解决了实际应用问题. 根据滤波器的不同类型，RFS-SLAM可以分为基于概率假设密度（probability hypothesis density， PHD）滤波器^{［参考文献 12

百度学术}12］及其变种方法的SLAM，基于多伯努利（multi-Bernoulli， MB）滤波器^{［参考文献 13

百度学术}13］及其变种方法的SLAM. 在以上算法中，车辆轨迹估计均依赖于粒子滤波器来实现，而地图特征的后验密度传递均依赖于基于随机有限集的滤波器来实现. 对于这些滤波算法的实现，常用的方法是利用高斯混合模型（Gaussian mixture model， GMM）^{［参考文献 14

百度学术}14］，该模型可视为由多个单一高斯模型组合而成. 使用高斯模型的原因在于其具备良好的数学性质和计算性能，但同时在处理非线性和非高斯问题上存在困难. 针对这一问题，Vo等^{［参考文献 15

百度学术}15］提出了基于序贯蒙特卡罗（sequential Monte Carlo， SMC）实现的概率假设密度滤波器，该方法不采用线性高斯近似模型，从原理上避免了GMM方法的局限性.

虽然基于滤波的SLAM技术已经取得了许多进展，但在智能汽车领域的应用中仍存在一些问题. 首先，智能汽车的行驶环境复杂，SLAM算法的核心之一是位置识别，该算法主要为规划层提供定位信息^［

16-17］，这要求算法在杂波、漏检等影响下仍能准确且稳定地估计环境特征和车辆位姿. 其次，实际应用于智能车辆的是3D激光雷达^{［参考文献 18

百度学术}18］，而传统的PHD-SLAM算法主要适用于2D激光雷达，难以适配. 最后，当前基于滤波的SLAM算法处理点云时会将所有点视作特征，这不仅引入了许多不稳定的特征，同时大量的3D点云数据也会在一定程度上影响算法的实时性.

为了解决上述问题，本文提出一种结合柱状特征与随机有限集的车辆激光SLAM 方法. 该方法无须进行观测与目标的数据匹配，能够同时准确估计环境特征和车辆位姿，具有高精度和鲁棒性. 本文的主要贡献包括：1）将传统针对2D激光雷达的SLAM扩展至3D，并采用基于SMC的RB-PHD-SLAM算法，实现智能汽车环境特征和车辆位姿的同时估计. 2）利用智能驾驶环境中稳定且丰富的柱状特征，规避了直接处理原始点云数据所带来的不稳定特征增多和实时性差的问题. 3）通过模拟数据集和KITTI数据集的验证，证实了该方法的有效性.

1 系统架构

本文提出的基于柱状特征与随机有限集的车辆激光SLAM系统架构如图1所示，主要包含4个模块. 在柱状特征提取模块中，鉴于3D激光雷达点云数据量庞大且包含噪声，直接将所有点视作特征会引入许多不稳定特征，因此首先对原始数据进行预处理，特别是设置了地面点去除和点云分割步骤，以最大限度保障柱状特征提取过程的效率和提取结果的准确性. 点云分割的结果是若干簇表示不同物体的点云集合，针对这些点云簇，接下来基于M估计抽样一致性（M-estimator sample consensus， MSAC）算法^［

19］提取柱状特征，并利用半径阈值筛选出真实的柱状物. 在位姿预测模块中，通过建立速度运动模型，将上一时刻的位姿递推得到当前车辆位姿的预测值. 在RB-PHD滤波模块中，考虑了存活目标和新生目标，通过预测、更新、多目标状态提取以及重采样步骤，利用加权粒子传递多目标后验概率密度的强度函数. 在位姿与地图更新模块中，首先对全局地图进行更新，然后对每个粒子的权重进行更新，通过更新的权重计算车辆位姿，使系统能够进入下一次迭代.

图1 基于柱状特征与随机有限集的车辆激光SLAM系统架构

Fig.1 Architecture of vehicle LiDAR SLAM system based on column features and random finite sets

2 位姿预测与柱状特征提取

贝叶斯滤波器中的运动模型描述了车辆状态随控制输入的变化关系. 考虑到智能汽车的实际运行场景，采用三自由度运动学模型^［

20］并引入高斯白噪声对运动进行描述. 智能汽车的运行环境一般为市区道路和高速路，这些场景通常包含大量稳定的柱状物体，如信号灯杆，行道树，交通指示牌立柱等. Ren等^{［参考文献 21

百度学术}21］采用了区域生长算法和聚类算法得到柱状点云类，表明柱状特征的稳定性. 稳定的特征有利于更精准定位，因此本文在特征提取阶段充分发掘柱状物体，并将其作平面投影后在滤波器中进行处理.

2.1 位姿预测模型

位姿预测模型示意图如图2所示，x、y、θ分别代表车辆横轴坐标、纵轴坐标和航向角，因此车辆位姿可表示为 $x = {[x, y, θ]}^{T}$ . $t$ 时刻车辆位姿的概率密度可表示为：

p (x_{t} | u_{t - 1}, x_{t - 1})

（1）

式中： $u_{t - 1} = [v_{t - 1}, w_{t - 1}]^{T}$ 为控制输入， $v_{t - 1}$ 为 $t - 1$ 时刻的车辆速度， $w_{t - 1}$ 为车辆的横摆角速度； $x_{t - 1}$ 为车辆位姿.

图2 位姿预测模型示意图

Fig.2 Pose prediction model diagram

本研究中用来预测汽车状态的三自由度运动学模型定义如下：

x_{t} = x_{t - 1} + [\begin{matrix} - \frac{{\hat{v}}_{t - 1}}{{\hat{w}}_{t - 1}} s i n θ_{t - 1} + \frac{{\hat{v}}_{t - 1}}{{\hat{w}}_{t - 1}} s i n (θ_{t - 1} + {\hat{w}}_{t - 1} Δ t) \\ \frac{{\hat{v}}_{t - 1}}{{\hat{w}}_{t - 1}} c o s θ_{t - 1} - \frac{{\hat{v}}_{t - 1}}{{\hat{w}}_{t - 1}} c o s (θ_{t - 1} + {\hat{w}}_{t - 1} Δ t) \\ {\hat{w}}_{t - 1} Δ t \end{matrix}]

（2）

式中： $Δ t$ 为 $t - 1$ 时刻到 $t$ 时刻的时间间隔； ${\hat{v}}_{t - 1} = v_{t - 1} + ξ_{t - 1}^{v}$ 和 ${\hat{w}}_{t - 1} = w_{t - 1} + ξ_{t - 1}^{w}$ 表明传感器量测数据存在一定的误差， $ξ_{t - 1} = [ξ_{t - 1}^{v}, ξ_{t - 1}^{w}]^{T}$ 表示均值为0的高斯白噪声.

2.2 点云处理与柱状特征提取

柱状特征主要代表市区和高速场景中大量稳定的柱状物体，如信号灯杆、行道树、交通指示牌立柱等. 由于激光雷达点云具有数据量大、噪声数据多的特点，柱状物体和周围环境融合且结构尺寸相对固定. 因此，需要对点云进行一系列处理，以提取高质量的柱状特征. 基于激光雷达原始点云和环境中柱状物体的特性，本文设计了以下点云处理程序：点云预处理、点云分割和柱状特征提取. 选择点云处理程序中的每一个步骤所使用的方法时，主要考虑不同方法的适应性、鲁棒性和复杂度差异.

2.2.1 点云预处理

1）点云降采样. 图3（a）显示了HDL-64E激光雷达采集的KITTI数据集单帧点云数据，该帧包含

125 347个点. 为满足实时性要求和保持柱状特征形状，采用随机下采样方法（random down-sampling method， RDM）^［

22］剔除一定比例的点云. 图3（b）表明了该方法在减小数据量的同时尽量保留原始数据的形状特征.

2）车辆点、远距离点去除. 通过直通滤波器，清除来自采集车辆本身的数据（这些数据对算法性能无贡献且会产生额外干扰）. 同时，清除远距离的稀疏点云. 图3（c）展示了去除X轴±3.5 m、 $Y$ 轴±1.9 m以内， $X$ 轴±40 m、 $Y$ 轴±30 m以外的点云.

3）点云去噪. 柱状特征提取的过程对噪声较为敏感，因此采用半径滤波器去除离群点，得到的点云如图3（d）所示.

4）地面点去除. 地面点通常占据原始点云数据的一半以上^［

23］. 然而，对于定位与建图任务，提取的特征只针对地面上的物体. 大量的地面点不仅影响算法实时性，而且会对接下来的点云分割造成不利影响. 因此，需要对地面点云进行拟合和清除，本文采用随机抽样一致（random sample consensus， RANSAC）算法^{［参考文献 22

百度学术}22］对地面进行处理，如图3（e）所示.

2.2.2 点云分割

点云分割的目标是将具有相同或相似属性的点云数据分组，以减少后续特征提取阶段的计算量并提高准确性. 由于自动驾驶场景的复杂性和多变性，无法提前确定聚类数量，因此采用基于密度的含噪应用空间聚类（density-based spatial clustering of applications with noise，DBSCAN）^［

24］方法对点云进行分割. 然而，这种方法会分割出包含较少激光点的点云簇，这类点云簇通常稳定性较差，因此SLAM算法更关注环境中较大的物体. 由此，需要设置点云数量阈值来清除这类点云簇. 经过试验，本文选用30作为数量阈值，以保留环境的主要特征，同时去除包含较少激光点的点云簇，得到的点云如图3（f）所示.

（a）原始点云

（b）点云降采样

（c）车辆点、远距离点去除

（d）点云去噪

（e）地面点去除

（f）点云分割

图3 点云预处理和点云分割

Fig.3 Preprocessing and segmentation for point cloud

2.2.3 柱状特征提取

柱状特征提取的效果将直接影响SLAM算法的效果. 基于模型拟合的柱状特征提取方法效果较好，但对离群点较为敏感. 不过，在点云预处理和分割阶段已对离群点进行了抑制，因此本文选用MSAC算法提取柱状特征. 由于该算法能在几乎所有的点云簇中找到符合要求的圆柱，因此所得的圆柱体半径差别很大. 例如，一个表示墙面的点云簇可拟合出一个半径很大的圆柱. 为了剔除不符合实际的柱状物，本文设置了半径阈值为0.2 m，在KITTI数据集2011_09_30_drive_0027_sync下第878帧的试验结果中，得到了如图4所示的柱状特征. 对于每个柱状特征，筛选出圆柱体中轴线和水平面夹角大于85°的柱状物，并计算中轴线中点，用以近似表示该柱状物在二维平面上的位置.

（a）柱状特征1

（b）柱状特征2

（c）柱状特征3

（d）柱状特征4

图4 柱状特征提取

Fig.4 Extraction of column features

3 基于柱状特征与随机有限集的SLAM方法

车辆三自由度运动学模型表征了智能汽车的运动特性，而柱状特征则代表了在智能汽车运行环境中稳定的静态物体. 因此，本文将这两者融入RB-PHD滤波器框架，并利用SMC方法进行实现. 这一融合使得传统针对2D激光雷达的SLAM技术得以拓展至3D空间，在杂波和漏检的影响下仍然能够精确地估计智能汽车的环境特征和车辆位姿.

3.1 RB-PHD-SLAM算法流程

基于随机有限集的SLAM在贝叶斯框架下可描述为：已知车辆信息和传感器量测信息的前提下，估计环境中静态特征 $M_{t}$ 和车辆轨迹 $X_{0 : t}$ 的联合后验密度 $p_{t} (M_{t}, X_{0 : t} | Z_{1 : t}, U_{1 : t}, X_{0})$ . 定义如下缩写：

p_{t | t - 1} (M_{t}, X_{0 : t}) = p_{t | t - 1} (M_{t}, X_{0 : t} | Z_{1 : t}, U_{1 : t - 1}, X_{0})

（3）

p_{t} (M_{t}, X_{0 : t}) = p_{t} (M_{t}, X_{0 : t} | Z_{1 : t}, U_{1 : t - 1}, X_{0})

（4）

式中： $U_{1 : t - 1}$ 为车辆控制量； $Z_{1 : t}$ 为量测信息； $p_{t} (M_{t}, X_{0 : t})$ 为车辆轨迹与地图特征的后验密度； $p_{t | t - 1} (M_{t}, X_{0 : t})$ 为对应的预测后验密度.

基于RFS的地图和位姿估计的递归式为^［

8］：

\begin{array}{l} p_{t | t - 1} (M_{t}, X_{0 : t}) = f_{X} (X_{t} | X_{t - 1}, U_{t - 1}) \times \\ \int f_{M} (M_{t} | M_{t - 1}, X_{t}) p_{t - 1} (M_{t - 1}, X_{1 : t - 1}) δ M_{t - 1} \end{array}

（5）

p_{t} (M_{t}, X_{0 : t}) = \frac{g_{t} (Z_{t} | X_{t}, M_{t}) p_{t | t - 1} (M_{t}, X_{1 : t})}{g_{t} (Z_{t} | Z_{1 : t - 1}, X_{0})}

（6）

式中： $δ$ 定义了集合积分计算方法； $g_{t} (Z_{t} | X_{t}, M_{t})$ 表示获取量测信息的似然函数，该函数是在车辆位姿和地图已知的条件下进行的. 式（5）和式（6）已经包含了由运动过程噪声、传感器测量噪声和传感器检测准确度和虚警等引起的误差，即建立递归方程的过程已经考虑了特征状态和数量的可变性.

FastSLAM算法^［

5］采用解耦的思想解决了集合积分无法直接计算的问题. 基于此，本文采用RB-PHD滤波器将式（4）解耦为：

p_{t} (M_{t}, X_{0 : t}) = p_{t} (X_{0 : t} | Z_{1 : t}, U_{1 : t - 1}, X_{0}) p_{t} (M_{t} | Z_{1 : t}, X_{0 : t})

（7）

根据式（5）和式（6），联合后验密度的递归过程可以表示为先传递轨迹的后验密度，然后再传递地图特征的后验密度的过程，因此式（7）右侧两项可表示为：

\begin{array}{l} p_{t} (M_{t} | Z_{1 : t}, X_{0 : t}) = \frac{g_{t} (Z_{t} | M_{t}, X_{t}) p_{t | t - 1} (M_{t}, X_{0 : t})}{g_{t} (Z_{t} | Z_{1 : t - 1}, X_{0 : t})} \times \\ p_{t | t - 1} (M_{t} | Z_{1 : t - 1}, X_{0 : t}) = \int f_{M} (M_{t} | M_{t - 1}, X_{t}) \times \end{array}

p_{t - 1} (M_{t - 1}, X_{0 : t - 1}) δ M_{t - 1}

（8）

\begin{array}{l} p_{t} (X_{0 : t} | Z_{1 : t}, U_{1 : t - 1}, X_{0}) = g_{t} (Z_{t} | Z_{1 : t - 1}, X_{0 : t}) \times \\ \frac{f_{X} (X_{t} | X_{t - 1}, U_{t - 1}) p_{t - 1} (X_{1 : t - 1})}{g_{t} (Z_{t} | Z_{1 : t - 1})} \end{array}

（9）

式（8）表示地图特征状态的后验强度，式（9）表示车辆轨迹的后验概率密度. 以上过程已采用解耦的思想解决了集合积分无法直接计算的问题. 此处将地图特征和量测信息都表示为RFS，因此任意多个目标状态后验概率密度的传递过程可以同时进行.

3.2 基于SMC实现的RB-PHD-SLAM算法

本研究中，对于式（8），采用基于SMC实现的PHD滤波器来进行传递；对于式（9），采用粒子滤波来进行传递. $t - 1$ 时刻的联合后验密度为：

{\{w_{t - 1}^{(k)}, X_{0 : t - 1}^{(k)}, M_{1 : t - 1}^{(k)} (\cdot | X_{0 : t - 1}^{(k)})\}}_{k = 1}^{N}

（10）

式中： $w_{t - 1}^{(k)}$ 表示第 $k$ 个粒子的权重； $X_{0 : t - 1}^{(k)} = [X_{0}^{(k)}, X_{1}^{(k)}, X_{2}^{(k)}, \dots, X_{t - 1}^{(k)}]$ 表示第 $k$ 个粒子在不同时刻估计得到的车辆位姿； $M_{1 : t - 1}^{(k)} (\cdot | X_{0 : t - 1}^{(k)})$ 为从开始到 $t - 1$ 时粒子 $k$ 的特征集合. $M_{1 : t - 1}^{(k)}$ 由若干粒子进行加权，即

M_{1 : t - 1}^{(k)} = {\{w_{t - 1, m}^{(i)}, m_{t - 1}^{(i)}\}}_{i = 1}^{N_{t - 1, m}}

（11）

{\{w_{t}^{(k)}, X_{0 : t}^{(k)}, M_{1 : t}^{(k)} (\cdot | X_{0 : t}^{(k)})\}}_{k = 1}^{N}

（12）

式中： $N_{t - 1, m}$ 为粒子数量； $w_{t - 1, m}^{(i)}$ 和 $m_{t - 1}^{(i)}$ 分别为地图特征的后验强度中某个粒子的权重和状态.式（12）为按照同样的方式得到的t时刻的联合后验密度.

在算法中引入位姿预测模型和柱状特征，实现从式（10）至式（12）的递归过程. 根据式（1）的运动模型预测 $t$ 时刻第 $k$ 个粒子的车辆状态：

X_{t | t - 1}^{(k)} \sim p (\cdot | X_{t - 1}^{(k)}, U_{t - 1})

（13）

式中： $X_{t - 1}^{(k)}$ 和 $U_{t - 1}$ 分别表示 $t - 1$ 时刻车辆位姿和控制量. 对于每一个预测的车辆位姿，将车辆坐标系下的量测信息转换到世界坐标系下：

Z_{t, v e h} \overset{X_{t | t - 1}^{(k)}}{\to} Z_{t, w o r l d} = \{z_{1, w o r l d}, z_{2, w o r l d}, \dots, z_{n_{t - 1}, w o r l d}\}

（14）

SLAM问题中往往只关心环境中的静态物体，因此对特征状态的预测只考虑存活和新生特征，则重采样后有：

p_{t - 1} = {\{τ_{t - 1}^{(i)}, l_{t - 1}^{(i)}\}}_{i = 1}^{L_{t - 1}}

（15）

式中： $L_{t - 1}$ 表示 $t - 1$ 时刻重采样后的粒子数量； $τ_{t - 1}^{(i)}$ 表示第 $i$ 个粒子的权重； $l_{t - 1}^{(i)}$ 表示第 $i$ 个粒子的状态； $p_{t - 1}$ 为重采样后的粒子集合.

对于地图存活特征预测，因为传感器噪声和算法本身存在误差，因此在对存活特征状态进行预测时，需要在重采样的粒子状态基础上人为叠加噪声. 则式（15）中的粒子集合在 $t$ 时刻的预测状态为：

\begin{array}{l} p_{t | t - 1} = {\{τ_{t | t - 1}^{(i)}, l_{t | t - 1}^{(i)}\}}_{i = 1}^{L_{t - 1}} \\ l_{t | t - 1}^{(i)} = l_{t - 1}^{(i)} + ε_{t} \end{array}

（16）

式中： $p_{t | t - 1}$ 表示对存活特征进行预测得到的粒子集； $ε_{t}$ 为高斯白噪声，且有 $τ_{t | t - 1}^{(i)} = τ_{t - 1}^{(i)}$ .

对于地图新生特征预测，需要预先知道特征可能出现的位置，但智能汽车的运行特点导致通常难以获得有效的先验信息. 因此，本文采用Mullane等^［

25］提出的将上一时刻的量测集合作为当前时刻的先验信息的策略. 此外，若量测信息缺失，本文则根据最近的、信息齐全的那次量测进行位置预测. 因此，

t

时刻新生特征产生的粒子集合可表示为：

Z_{t - 1} = {z_{1}, z_{2}, \dots, z_{n_{t - 1}}}

J_{t} = n_{t - 1} \cdot J_{r s p}

b_{t | t - 1} = {\{τ_{t | t - 1}^{(i)}, l_{t | t - 1}^{(i)}\}}_{i = 1}^{J_{t}}

（17）

式中： $Z_{t - 1}$ 表示最近一次不为空的量测； $J_{r s p}$ 表示每次量测产生的粒子数量； $b_{t | t - 1}$ 为预测的新生特征粒子集； $J_{t}$ 表示粒子数量； $n_{t - 1}$ 为进行预测的帧的量测数量；权重 $τ_{t | t - 1}^{(i)}$ 在1到 $J_{t}$ 之间的和定义了 $t$ 时刻新生特征的预期数量.

基于最新观测对滤波器中的粒子权重进行更新：

τ_{t}^{(i)} = [1 - P_{D} (l_{t}^{(i)}) + \sum_{z \in Z_{t, w o r l d}} \frac{ψ_{t, z} (l_{t}^{(i)})}{k_{t} (z) + C_{t} (z)}] τ_{t | t - 1}^{(i)}

（18）

C_{t} (z) = \sum_{i = 1}^{L_{t}} ψ_{t, z} (l_{t}^{(i)}) τ_{t | t - 1}^{(i)}

ψ_{t, z} (l_{t}^{(i)}) = P_{D} (l_{t}^{(i)}) g (z | l_{t}^{(i)})

（19）

式中： $P_{D} (l_{t}^{(i)})$ 为传感器检测到该粒子的概率大小； $g (z | l_{t}^{(i)})$ 描述了量测与真实特征状态的似然函数； $τ_{t | t - 1}^{(i)}$ 表示 $t - 1$ 时刻第 $i$ 个粒子的权重； $k_{t} (z)$ 为杂波强度，且有 $k_{t} (z) = λ_{c} f_{c} (z)$ ， $f_{c} (z)$ 表示杂波状态的概率分布.

对于特征状态提取，首先需要对粒子进行分类，目标的状态则由某类粒子求平均得到，本文采用 $K$ 均值算法实现粒子分类：

\begin{array}{l} {\hat{N}}_{t | t} = [\sum_{i = 1}^{L_{t}} τ_{t}^{(i)}] \\ F_{t} = {\{w_{t, f}^{(i)}, f_{t}^{(i)}\}}_{i = 1}^{N_{t, f}} \end{array}

（20）

式中： ${\hat{N}}_{t | t}$ 表示 $t$ 时刻特征数目的估计值； $F_{t}$ 表示聚类算法提取的 $t$ 时刻的特征集合； $N_{t, f}$ 为特征数量； $w_{t, f}^{(i)}$ 为特征权重； $f_{t}^{(i)}$ 为特征估计状态.

对于全局地图特征更新，总的地图特征为：

\begin{array}{l} M_{1 : t} = M_{1 : t - 1} ⋃ F_{t} = {\{w_{t - 1, m}^{(i)}, m_{t - 1}^{(i)}\}}_{i = 1}^{N_{t - 1, m}} ⋃ \\ {\{w_{t, f}^{(i)}, f_{t}^{(i)}\}}_{i = 1}^{N_{t, f}} = {\{w_{t, m}^{(i)}, m_{t}^{(i)}\}}_{i = 1}^{N_{t, m}} \end{array}

（21）

式中： $w_{t, m}^{(i)}$ 和 $m_{t}^{(i)}$ 分别为粒子 $i$ 的权重和状态； $N_{t, m} = N_{t - 1, m} + N_{t, f}$ 为粒子数量.

重采样的目的是解决迭代过程中的粒子退化问题. 重采样粒子数 $N_{p a r t i c l e}$ 和每个粒子的权重 $τ_{t}^{(i)}$ 的计算方法如下：

\begin{array}{l} N_{p a r t i c l e} = J_{r s p} \times \sum_{i = 1}^{L_{t}} τ_{t}^{(i)} \\ τ_{t}^{(i)} = \frac{1}{N_{p a r t i c l e}} \sum_{j = 1}^{L_{t}} τ_{t}^{(j)} \end{array}

（22）

式（10）中的权重更新方法如下：

{\hat{w}}_{t}^{(k)} = \sum_{i = 1}^{N_{t, r}} \sum_{j = 1}^{N_{t, f}} \frac{w_{t, f}^{(j)}}{\sqrt[]{2 π} σ} e^{- \frac{(x_{t, f}^{j} - x_{t, m}^{i})^{2} + (y_{t, f}^{j} - y_{t, m}^{i})^{2}}{2 σ^{2}}}

w_{t}^{(k)} = {\hat{w}}_{t}^{(k)} / \sum_{i = 1}^{N} {\hat{w}}_{t}^{(i)}

（23）

式中： ${\hat{w}}_{t}^{(k)}$ 表示第 $k$ 个粒子的权重； $w_{t, f}^{(j)}$ 和 $(x_{t, f}^{j}, y_{t, f}^{j})$ 分别为第 $j$ 个特征的权重和估计状态； $w_{t}^{(k)}$ 为所有粒子权重进行归一化处理的结果.

计算有效粒子数 $N^{*}$ ，当 $N^{*}$ 小于 $N / 2$ 时进行重采样：

N^{*} = 1 / \sum_{i = 1}^{N} {w_{t}^{(i)}}^{2}

（24）

综上所述，算法需要输入 $t - 1$ 时刻车辆轨迹和环境特征的联合后验密度、 $t$ 时刻车辆坐标系下的量测信息和车辆控制量，经过车辆位姿预测、量测信息坐标转换、静态地图PHD滤波和粒子权重更新步骤后，输出 $t$ 时刻车辆轨迹和环境特征的联合后验密度，然后进入下一次循环，其中是否需要重采样取决于计算的有效粒子数.

4 试验与结果分析

为了评估本文提出的基于柱状特征与随机有限集的激光SLAM方法在车辆定位精度和特征估计精度方面的性能，在模拟数据集和KITTI数据集上进行了试验，并将其与基准算法（FastSLAM算法）进行了比较. 为了验证所提方法的实用性和泛化性，在4.1节中赋予特征漏检与检测到两种状态，且在检测到的特征上叠加高斯噪声；在4.2节中进行50次蒙特卡罗试验.

均方根误差（root mean square error， RMSE）可以描述轨迹之间的偏离程度，因此采用RMSE定量评估算法的定位精度. RMSE的表达式如下：

R_{R M S E} (k) = \sqrt[]{\frac{1}{T} \sum_{t = 1}^{T} {‖{\hat{x}}_{t} - x_{t}‖}^{2}}, k = 1,2, \dots, N

（25）

式中： $k$ 表示蒙特卡罗试验序号； $N$ 表示试验总次数； $T$ 为单次试验的总步数； ${\hat{x}}_{t}$ 和 $x_{t}$ 分别为车辆位置的估计量和真实值.

为了量化评估不同SLAM算法的特征估计效果，采用最优子模式分配（optimal sub-pattern assignment， OSPA）^［

26］方法作为地图特征估计精度评价指标. 对于状态空间中的两个有限子集

X

和

Y

，其势分别为

m

和

n

. 若

m \leq n

，则两个子集的OSPA距离为：

\begin{array}{l} {\bar{d}}_{p}^{(c)} (X, Y) = \\ {\{n^{- 1} [\underset{σ \in Ω_{n}}{m i n} \sum_{i = 1}^{m} (d^{(c)} (x_{i}, y_{σ (i)} {))}^{p} + c^{p} (n - m)]\}}^{1 / p} \end{array}

（26）

式中： $p$ 为均值阶数； $Ω_{n}$ 为排列的集合； $y_{σ (i)}$ 表示两个子集中对应元素形成最优点分配； $d^{(c)} (\cdot, \cdot)$ 表示两个点之间的距离； $c$ 表示最小截止距离.

4.1 模拟数据集试验

4.1.1 试验环境搭建及参数设置

图5展示了模拟数据集试验环境，车辆的真实运行轨迹用绿色曲线表示，真实的固定特征用绿色圆点表示，传感器的量测结果用黑色小点表示.

图5 模拟数据集试验环境

Fig.5 Testing environment of simulated dataset

图6（a）表示图5中轨迹的真实角速度. 试验过程中，将实际的车辆角速度和速度分别和高斯函数叠加后当作传感器获得的信息. 图6（b）为图5中红色圆圈所代表的传感器检测范围，黑色小点表示真实特征产生的量测. 若某特征未被检测到，则形成漏检；若被检测到，则在真实位姿上叠加高斯噪声输出. 红色小叉表示服从泊松分布的杂波，在检测范围内均匀生成. 更多数据集试验参数见表1.

表1 数据集试验参数

Tab.1 Dataset experimental parameters

参数	数值
初始位姿	0，0， $π$ /2
速度/（m·s^-1）	8
速度方差/（m·s^-2）	0.005
角速度	见图6（a）
角速度方差/（rad·s^-2）	0.001
传感器方差/（m·s^-2）	0.4
传感器检测半径/m	26
特征被检测到的概率	0.8
仿真步长/s	0.1
仿真时间/s	100
杂波数量的泊松均值	5

（a）模拟数据集角速度

（b）数据量测示意图

图6 参数设置及量测示意图

Fig.6 Parameters configuration and measurement diagram

4.1.2 模拟数据集试验结果及分析

图7为一次蒙特卡罗试验的车辆轨迹和地图特征对比. 相比于FastSLAM算法，本文算法的车辆轨迹与真实轨迹更加贴近；本文算法的地图特征绝大部分与真实特征更加吻合. 以上结果说明，本文算法在杂波和漏检环境下能够更准确地估计车辆位姿和地图特征，显示出更强的鲁棒性和准确性. 本文算法同时估计特征的数量和位置，避免了单独管理特征带来的内存和效率问题.

图7 模拟数据集车辆轨迹和地图特征对比

Fig.7 Comparison of vehicle trajectories and map features in simulated dataset

一次蒙特卡罗试验结果往往是随机的，统计结果能更加综合地评价算法的性能. 为了更准确地对比不同算法的性能，本文在同样试验条件下，分别对算法开展了50次蒙特卡罗试验，并采用RMSE和OSPA分别对车辆定位精度和特征估计精度进行评估.

图8展示了进行50次蒙特卡罗试验后，车辆定位精度对比. 由图8可以看出，相较于FastSLAM算法，在大部分试验中，本文算法的轨迹保持更低的RMSE误差. 此外，在50次试验中，FastSLAM算法的均值误差为3.89 m，而本文算法为2.14 m，降低了44.99%. 因此，本文算法产生的轨迹误差更小、更稳定，可有效地提高车辆定位精度.

图8 车辆定位精度对比

Fig.8 Comparison of vehicle localization accuracy

图9展示了进行50次蒙特卡罗试验后，地图特征估计精度对比. 图9中，OSPADist为估计的特征与真实的特征集合间总的OSPA误差，OSPALoc为相应的位置误差，OSPACard为相应的数量误差. 对于这3个指标，本文算法仅在少数试验中高于FastSLAM算法，而在绝大多数试验中明显低于FastSLAM算法. 具体地，对于OSPADist、OSPALoc和OSPACard的平均值，本文算法分别为5.600 m、1.643 m和3.334 m，FastSLAM则为11.200 m、3.237 m和7.615 m. 本文算法在3个指标上分别降低了50%、49.24%和56.22%. 因此，可以明显看出，本文算法更为精确地估计了地图特征的位置和数量，基于本文算法可以有效提升地图特征的估计精度.

图9 地图特征估计精度对比

Fig.9 Comparison of map feature estimation accuracy

4.2 KITTI数据集试验

4.2.1 试验环境介绍

本研究基于真实场景采集的KITTI数据集2011_09_30_drive_0027_sync（以下简称drive_0027）和2011_09_26_drive_0036_sync（以下简称drive_0036）进行算法对比验证. 其中算法估计的地图特征是环境中真实柱状物体在地面上的投影，因为KITTI数据集缺乏对环境柱状物位置的详细描述，因此本节仅针对车辆轨迹进行对比分析.

4.2.2 KITTI数据集试验结果及分析

图10展示了针对KITTI数据集中drive_0027和drive_0036场景下两种算法的车辆轨迹和地图特征的对比结果. 由图10可以看出，相较于FastSLAM算法，本文算法在这两个真实场景中所估计的车辆轨迹更接近真实轨迹. 为了更加直观地对轨迹结果进行对比，将轨迹投影到X轴和Y轴，如图11所示. 由图11可以看出，本文算法估计的轨迹在X和Y轴上都更接近真实位置，这进一步验证了该算法在车辆定位精度方面的优势.

（a） drive_0027

（b） drive_0036

图10 KITTI数据集车辆轨迹和地图特征对比

Fig.10 Comparison of vehicle trajectories and map features in KITTI dataset

（a） drive_0027的X轴偏移

（b） drive_0027的Y轴偏移

（c） drive_0036的X轴偏移

（d） drive_0036的Y轴偏移

图11 轨迹在X轴和Y轴上的投影对比

Fig.11 Comparison of trajectory projections on the X and Y axes

图12展示了两种算法在drive_0027和drive_0036场景中进行50次蒙特卡罗试验后的车辆定位精度对比结果. 由图12可以看出，在大多数试验中，相较于FastSLAM算法，本文算法保持更低的RMSE误差. 具体而言，对于50次试验，FastSLAM在drive_0027场景下的均值误差为4.53 m，在drive_0036场景下为6.05 m，而本文算法分别为2.08 m和2.56 m，分别降低了54.08%和57.69%. 这表明本文算法得到的轨迹误差更小，且更为稳定，使用本文算法能够有效提升车辆定位精度.

（a） drive_0027

（b） drive_0036

图12 50次蒙特卡罗试验的车辆定位精度对比

Fig.12 Comparison of vehicle localization accuracy across 50 Monte Carlo trials

4.3 运行效率分析

为了验证本文算法的运行效率，在同样的硬件平台和场景设置条件下，对本文算法和FastSLAM算法的运行时间进行对比. 硬件平台采用一台配备12th Gen Intel Core i5-12600KF处理器的台式电脑. 首先，在MATLAB平台上实现本文算法和FastSLAM算法；然后，在4.1节描述的仿真环境下进行50次蒙特卡罗试验，并记录每一帧的运行时间；最后，计算所有单帧运行时间的平均值，如表2所示. 由表2可知，相较于FastSLAM算法，本文算法的运行时间略有增加，增幅为13.60%. 这与算法本身的编写方式和较为严格的参数设置有关. 尽管如此，本文算法在保持运行时间增加不多的前提下，显著提升了算法的性能.

表2 运行时间对比

Tab. 2 Runtime comparison

算法	平均运行时间/s
FastSLAM	0.208 1
本文	0.236 4

5 结论

为了提升智能车辆的定位精度和特征估计精度，本文结合柱状特征提取方法和随机有限集理论，提出了一种基于SMC的车辆3D激光SLAM方法. 该方法首先进行点云预处理、点云分割和柱状特征提取步骤，以获取单帧点云中的静态存活特征和新生特征；随后，在 RB-PHD-SLAM框架中引入这两种特征，通过SMC方法完成车辆轨迹概率密度和地图后验强度在帧间的传递，实现同时估计环境特征和车辆位姿. 这种方法将传统基于PHD-SLAM框架的2D激光SLAM扩展到3D，并避免了处理原始点云所带来的计算量大和稳定性差的问题. 模拟数据集和KITTI数据集的试验结果表明，与经典的FastSLAM算法相比，本文算法对车辆定位精度的提升超过了40%，并使环境特征位置和数量的估计平均误差分别降低了40%和50%以上. 本文方法显著提高了SLAM的精确度和鲁棒性，有助于保障智能汽车的安全运行.

未来将尝试引入场景中的平面特征，以扩充用于定位和建图的特征类型和数量，从而进一步提升算法的性能.

参考文献

韩彦峰，李君君，肖科．基于优化Gmapping算法的巷道喷浆机器人建图研究［J］．湖南大学学报（自然科学版）， 2023， 50（6）： 118-126． [百度学术]

HAN Y F，LI J J，XIAO K．Research on mapping of shotcrete robot in roadway based on optimized Gmapping algorithm［J］．Journal of Hunan University （Natural Sciences），2023，50（6）：118-126．（in Chinese） [百度学术]

ZHENG X，ZHU J K．Efficient LiDAR odometry for autonomous driving［J］．IEEE Robotics and Automation Letters，2021，6（4）：8458-8465． [百度学术]

秦晓辉，周洪，廖毅霏，等．动态环境下基于时序滑动窗口的鲁棒激光SLAM系统［J］．湖南大学学报（自然科学版）， 2023， 50（12）： 49-58． [百度学术]

QIN X H，ZHOU H，LIAO Y F，et al．Robust laser SLAM system based on temporal sliding window in dynamic scenes［J］．Journal of Hunan University （Natural Sciences），2023，50（12）：49-58．（in Chinese） [百度学术]

SMITH R C， CHEESEMAN P. On the representation and estimation of spatial uncertainty［J］. The International Journal of Robotics Research，1986， 5（4）： 56-68. [百度学术]

MONTEMERLO M， THRUN S， KOLLER D， et al. FastSLAM： a factored solution to the simultaneous localization and mapping problem［C］// Proceedings of the AAAI National Conference on Artificial Intelligence， July 28-August 1， 2002. Edmonton， Canada： AAAI， 2002： 592-598. [百度学术]

SINGER R A， SEA R G. New results in optimizing surveillance system tracking and data correlation performance in dense multitarget environments［J］. IEEE Transactions on Edmonton Control， 1973， 18（6）： 571-582. [百度学术]

NEIRA J，TARDOS J D．Data association in stochastic mapping using the joint compatibility test［J］．IEEE Transactions on Robotics and Automation， 2001， 17（6）： 890-897． [百度学术]

MULLANE J，VO B N，ADAMS M D．Rao-Blackwellised PHD SLAM［C］//2010 IEEE International Conference on Robotics and Automation， May 3-7，2010. Anchorage，AK， USA： IEEE，2010： 5410-5416． [百度学术]

MULLANE J，VO B N，ADAMS M D，et al．A random-finite-set approach to Bayesian SLAM［J］．IEEE Transactions on Robotics，2011， 27（2）： 268-282． [百度学术]

GRISETTI G，STACHNISS C，BURGARD W．Improving grid-based SLAM with Rao-Blackwellized particle filters by adaptive proposals and selective resampling［C］//Proceedings of the 2005 IEEE International Conference on Robotics and Automation， April 18-22，2005. Barcelona， Spain： IEEE，2005：2432-2437． [百度学术]

ADAMS M，VO B N，MAHLER R，et al．SLAM gets a PHD：new concepts in map estimation［J］．IEEE Robotics & Automation Magazine， 2014， 21（2）： 26-37． [百度学术]

GAO L，BATTISTELLI G，CHISCI L．PHD-SLAM 2.0：efficient SLAM in the presence of missdetections and clutter［J］．IEEE Transactions on Robotics， 2021， 37（5）： 1834-1843． [百度学术]

VO B N， VO B T， HOANG H G．An efficient implementation of the generalized labeled multi-Bernoulli filter［J］．IEEE Transactions on Signal Processing，2017，65（8）：1975-1987． [百度学术]

VO B N，MA W K．The Gaussian mixture probability hypothesis density filter［J］．IEEE Transactions on Signal Processing，2006，54（11）：4091-4104． [百度学术]

VO B N，SINGH S，DOUCET A．Sequential Monte Carlo methods for multitarget filtering with random finite sets［J］．IEEE Transactions on Aerospace and Electronic Systems，2005，41（4）：1224-1245． [百度学术]

韩彦峰，唐超超，肖科．基于改进ORB-SLAM2算法的RGB-D稠密地图构建［J］．湖南大学学报（自然科学版），2023，50（2）：52-62． [百度学术]

HAN Y F，TANG C C，XIAO K．RGB-D dense map construction based on improved ORB-SLAM2 algorithm［J］．Journal of Hunan University （Natural Sciences），2023，50（2）：52-62．（in Chinese） [百度学术]

DAI A， LUND G， GAO G. PlaneSLAM：plane-based LiDAR SLAM for motion planning in structured 3D environments［EB/OL］. （2022-09-08）［2024-01-05］. https：//arxiv.org/abs/2209. 08248v2. [百度学术]

CHGHAF M，RODRIGUEZ S，EL OUARDI A．Camera，LiDAR and multi-modal SLAM systems for autonomous ground vehicles：a survey［J］．Journal of Intelligent & Robotic Systems， 2022， 105（2）： 1-35． [百度学术]

SONG F，SHAO Y T，GAO W，et al. Layer-wise geometry aggregation framework for lossless LiDAR point cloud compression［J］. IEEE Transactions on Circuits and Systems for Video Technology， 2021， 31（12）： 4603-4616． [百度学术]

KONG J， PFEIFFER M， SCHILDBACH G， et al. Kinematic and dynamic vehicle models for autonomous driving control design［C］//2015 IEEE Intelligent Vehicles Symposium （IV），June 28 -July 1，2015. Seoul，Korea： IEEE，2015：1094-1099． [百度学术]

REN Y， LIU B B， CHENG R， et al．Lightweight semantic-aided localization with spinning LiDAR sensor［J］．IEEE Transactions on Intelligent Vehicles，2023，8（1）：605-615． [百度学术]

EBADI K，PALIERI M，WOOD S，et al．DARE-SLAM：degeneracy-aware and resilient loop closing in perceptually-degraded environments［J］．Journal of Intelligent & Robotic Systems，2021，102（2）：1-25． [百度学术]

ANAND B，SENAPATI M，BARSAIYAN V，et al．LiDAR-INS/GNSS-based real-time ground removal，segmentation，and georeferencing framework for smart transportation［J］．IEEE Transactions on Instrumentation and Measurement，2021，70：8504611． [百度学术]

KRISHNA G S，SUPRIYA K，BAIDYA S．3DS-SLAM：a 3D object detection based semantic SLAM towards dynamic indoor environments［EB/OL］. （2023-10-10）［2024-01-05］.https：//arxiv.org/abs/2310.06385v1. [百度学术]

MULLANE J，VO B N，ADAMS M D，et al．A random set formulation for Bayesian SLAM［C］//2008 IEEE/RSJ International Conference on Intelligent Robots and Systems， September 22-26，2008. Nice， France： IEEE，2008：1043-1049． [百度学术]

ZHU Y，LIANG S，LI P，et al．Robust human pose quality assessment using optimal sub-pattern assignment［C］//2022 11th International Conference on Control，Automation and Information Sciences （ICCAIS），November 21-24，2022. Hanoi，Vietnam：IEEE， 2022： 419-423． [百度学术]

作者稿件一经被我刊录用，如无特别声明，即视作同意授予我刊论文整体的全部复制传播的权利，包括但不限于复制权、发行权、信息网络传播权、广播权、表演权、翻译权、汇编权、改编权等著作使用权转让给我刊，我刊有权根据工作需要，允许合作的数据库、新媒体平台及其他数字平台进行数字传播和国际传播等。特此声明。

关闭

首页

期刊简介

编委会

作者中心

下载中心

学术道德

常见问题

版权声明

联系我们

English

基于柱状特征与随机有限集的激光SLAM方法研究 PDF

摘要

关键词

1 系统架构

2 位姿预测与柱状特征提取

2.1 位姿预测模型

2.2 点云处理与柱状特征提取

3 基于柱状特征与随机有限集的SLAM方法

3.1 RB-PHD-SLAM算法流程

3.2 基于SMC实现的RB-PHD-SLAM算法

4 试验与结果分析

4.1 模拟数据集试验

4.2 KITTI数据集试验

4.3 运行效率分析

5 结论

参考文献

基于柱状特征与随机有限集的激光SLAM方法研究 PDF

摘要

关键词

1 系统架构

2 位姿预测与柱状特征提取

2.1 位姿预测模型

2.2 点云处理与柱状特征提取

3 基于柱状特征与随机有限集的SLAM方法

3.1 RB-PHD-SLAM算法流程

3.2 基于SMC实现的RB-PHD-SLAM算法

4 试验与结果分析

4.1 模拟数据集试验

4.2 KITTI数据集试验

4.3 运行效率分析

5 结 论

参考文献

5 结论