随着自动驾驶技术的发展, 同时定位与地图构建技术(simultaneous localization and mapping, SLAM)已成为近些年的研究热点. SLAM的主要目标是在运动中利用携带传感器的移动平台, 实现自主定位, 并逐步对周围环境进行建图^[1,2]. 目前, 无人车通常搭载多种传感器^[3], 包括激光雷达、惯性测量单元、视觉相机和全球定位系统. 与其他传感器相比, 激光雷达在障碍物检测、高精度的距离测量和对光照纹理不敏感等方面具有突出优势, 因此, 以激光雷达为传感器的激光SLAM算法在自动驾驶领域具有非常广阔的应用前景^[4].

基于特征的点云匹配方法主要涉及提取点云中的线、面特征, 随后对这些特征点云进行匹配. Zhang等^[5]提出的LOAM (LiDAR odometry and mapping)为目前广泛使用的激光SLAM算法框架, 该算法使用了基于特征的匹配方法最近点迭代法(iterative closest point, ICP). ICP算法的核心是最小化点与点之间的距离, 然而在激光SLAM中雷达是处于运动状态的, 并非每一处点云都有匹配点, 且计算所有点云会带来过高的计算复杂度. 因此LOAM算法中对其进行了改进, 将原点云中的点与待配准点云中提取出的直线与平面特征点进行匹配, 极大地减少了计算量, 在结构化场景下有很好的效果, 但在非结构化场景下会因特征难以提取而表现不佳. 后来, Shan等^[6]对LOAM进行轻量化的优化并增加后端优化模块, 引入地面特征, 通过提取地面点的平面特征以及非地面点的线条特征, 过滤掉无关的噪点, 有效减少了特征点的数量, 从而提高了点云匹配的精度和效率^[7]. 文献[5,6]提出的基于特征的激光SLAM算法可以减少点云规模并利用同类点之间配准来提高定位精度, 但由于对特征的强依赖性, 使得系统在处理社区、街道等开放的非结构化环境中会因为难以提取可靠稳定的特征而出现退化问题, 并且没有能够根据场景结构的不同切换对应的点云匹配算法.

因此, 为了减少对特征的强依赖性, 一系列的基于概率分布与数学优化的激光SLAM方法被提出. 文献[8]提出的HDL_GRAPH_SLAM用基于数学优化的正态分布变换(normal distribution transform, NDT)进行点云匹配, 使用高斯分布模型来重新构建点云, 但其需要对点云数据进行事先的网格化处理^[9], 导致计算复杂度高, 影响了点云匹配的效率. 文献[10]提出的相干点漂移算法CPD将一个点集看作是由一个概率密度函数生成的样本, 然后利用高斯混合模型来描述这个概率密度函数, 通过期望最大化算法来求解最优变换矩阵, 从而实现点云对齐. 能够处理非刚性变形、遮挡、噪声等问题, 并且在配准精度上也有一定的优势, 可以有效地运用于非结构化场景中. 但其需要通过期望最大化算法来求解最优变换矩阵, 计算复杂度高. 因此将CPD与ICP结合形成优势互补, 可以提高点云配准的精度和效率. 文献[11]提出了一种自适应的匹配方法, 当所提取的特征数量充足时, 该方法采用基于特征的匹配方法; 而当提取到的特征数量不足时, 则切换至基于概率分布的算法进行匹配. 然而, 仅凭特征数量无法准确区分场景结构, 因为特征的数量除了和场景的结构有关外, 还受到雷达的扫描线束以及点云重叠程度、场景几何特征等因素的影响. 此外, 基于数学优化的算法计算量大, 影响系统的实时性. 为了平衡系统的精度和实时性, 文献[12]提出了一种将NDT与ICP结合的算法, 该算法在使用ICP进行精细配准之前, 先添加NDT进行预配准, 有效地满足了系统的实时性, 但由于缺少对场景结构进行识别, 并根据不同的场景使用合适的高效算法, 难以适应开放复杂的实际应用场景.

综上所述, 为了能够在复杂场景下保证SLAM系统的精度和效率, 本文提出了一种联合CPD面向复杂场景的自适应激光SLAM算法CPD-LOAM. 首先根据场景结构的不同, 提出了场景特征变量(scene feature variable, SFV)的概念对场景结构进行初步判断, 并从几何特征的角度利用表面曲率进行验证, 来增强对场景结构辨识的准确性. 此外, 在非结构化场景下添加CPD算法进行点云预配准来解决该场景下难以提取可靠稳定的特征而出现退化的问题, 然后使用ICP算法进行点云再配准来提高SLAM系统的精度. 通过这种智能化的自适应机制, 可以避免将CPD算法应用于结构化场景, 有效减少配准过程中的误差和计算量, 提高点云配准的精度和效率, 有效地适应了场景的复杂多变.

1 CPD-LOAM算法原理

为了解决传统激光SLAM算法无法正确区分场景结构并且在面对非结构化场景时会因为特征难以提取而失效的问题, 本文提出了场景特征变量与表面曲率用于判断和验证场景结构, 进而自适应地选择是否添加CPD进行点云预配准来解决非结构化场景下特征提取不佳的问题, 以适应场景的复杂多变. 算法框架如图1所示, 主要分为点云的特征提取、帧间匹配scan-to-scan配准和后端优化scan-to-map配准3个阶段.

在点云的特征提取阶段, 通过对原始激光数据进行运动去畸变和离群点移除操作, 将处理后的数据作为特征点云输出, 用于后续的点云配准算法中. 在帧间匹配scan-to-scan配准阶段, 通过提取出来的特征点云进行帧间匹配, 首次迭代时需要对场景结构进行判断, 计算场景特征变量SFV, 并通过验证表面曲率G来区分场景结构, 若为非结构化场景则需使用CPD算法来进行点云的预配准来解决特征的退化问题并为ICP算法提供良好初值以增加点云匹配的准确性. 第2次迭代则使用LOAM中的ICP算法, 因为ICP算法在精细配准方面表现良好, 它通过迭代寻找最近的点对来优化匹配结果, 但容易受到初始位置、噪声和离群点的影响, 在非结构化场景下表现不佳. 而CPD算法则在处理高噪声和离群点的情况时更加稳定和鲁棒, 通过最大化点云间的概率一致性来找到匹配, 对初始位置不太敏感. 所以在第1次迭代之后ICP算法有了良好初值, 第2次迭代则直接使用ICP算法即可. 然后更新参考帧的点云信息, 并通过当前帧与上一帧的匹配得到激光里程计数据. 在后端优化scan-to-map配准阶段, 先通过CPD算法进行点云预配准, 然后使用ICP算法进行点云再配准, 以便于更好地纠正里程计的漂移, 从而输出全局地图, 保证了地图的全局完整性.

1.1 特征提取

由于点云数据会包含噪音与离群点^[13–15], 对后续的算法处理产生干扰和误差, 并且点云数据非常庞大, 大规模点云数据在进行复杂算法处理时会消耗大量的计算资源, 因此需要去除离群点和减少数据量来提高计算准确性和效率. 本文通过对原始激光数据进行离群点移除以及体素滤波下采样操作, 将处理后的数据作为特征点云输出, 用于后续的点云配准算法当中.

1.1.1 离群点去除

为了消除离群点对点云匹配算法产生的误差与干扰, 从激光雷达接收点云数据之后, 估计出每个点采集的时刻雷达所处的位置, 然后将一帧内所有点变换到统一坐标系下^[16,17]. 当帧间运动估计完成后, 用该估计进行时间插值, 获得每个点所在采集时刻的雷达位姿, 从而将点云都转换到统一坐标系下. 每个点采集时刻的估计位姿采用线性插值实现, 设$ {T_{k - 1}} $表示第k–1帧的帧间运动, 在对第k帧点云去畸变时, 默认在帧周期内, 激光雷达的运动是均匀的, 其插值方式表示如下:

$ {T}_{k-1, i}=\frac{{t}_{k-1, i}-{t}_{k-1}}{{t}_{k}-{t}_{k-1}}\text{ }\cdot\text{ }{T}_{k-1} $

(1)

其中, $ {T_{k - 1, i}} $表示第$k-1 $帧点云中的第i个点采集时刻的雷达位姿估计, $ {t_k} $, $ {t_{k - 1}} $分别表示$k-1 $帧点云周期的起始时间和结束时间, $ {t_{k - 1, i}} $表示$k-1 $帧点云中第i个点的时刻.

1.1.2 体素滤波下采样

本文采用体素滤波的方法来减少计算资源的损耗, 将原始点云数据划分为小的边长为0.6 m的立方体单元, 每个立方体单元即为一个体素, 对于每个体素, 选择离体素中心最近的点作为采样点^[18]. 将选择出的采样点组合起来, 形成新的点云数据.

1.2 帧间匹配scan-to-scan

为了能够在复杂场景下正确区分场景结构, 本文提出一种基于预判和验证相结合的场景结构辨识方法, 首先引入场景特征变量, 对场景结构进行初步判断, 然后从几何特征角度通过表面曲率对其进行验证. 并且在非结构化场景下引入CPD算法进行点云预配准来解决特征提取不佳而退化的问题, 从而有效地适应场景结构的复杂多变. 帧间匹配阶段首次迭代时计算场景特征变量SFV, 并通过验证表面曲率G决定是否使用CPD算法来进行点云的预配准. 第2次迭代则使用LOAM中的ICP算法, 然后更新参考帧的点云信息, 并通过当前帧与上一帧的匹配得到激光里程计数据.

1.2.1 CPD算法

在面对非结构化场景时, 由于特征难以提取会出现退化, 因此需要添加CPD算法进行点云的预配准. CPD算法的基本思想是通过最大化概率密度函数来估计点云之间的配准变换. 给定两个点集$X=\{ {x_1},{x_2}, $${x_3},\cdots,{x_N}\} $, $ Y=\{{y_1}, {y_2}, {y_3},\cdots, {y_M} \}$, 两个点集之间的变换关系为$X=T(Y, \theta) $, 以点集Y为混合高斯模型的各个分模型(高斯模型的中心), X中的数据点即为由该模型生成的数据点. 然后添加均匀分布的权重$ \omega $来减少冗余噪声的干扰, 因此, 点$ {x_n} $的概率分布为:

$ P({x_n}) = (1 - \omega )\sum\limits_{m = 1}^M {\frac{1}{M}p({x_n}\mid{y_n})} + \omega \frac{1}{N} $

(2)

当点集Y中$ {y_m} $为先验点时, GMM的单个高斯基函数可以写成:

$ p({x_n}\mid{y_m}) = \frac{1}{{{{(2\text{π} {\sigma ^2})}^{D/2}}}}{{\mathrm{e}} ^{ - \frac{{{{\left\| {{x_n} - {y_m}} \right\|}^2}}}{{2{\sigma ^2}}}}} $

(3)

其中, x是参考点云, y是待配准点云, n、m分别为点集X、Y的规模, $ \left\| {{x_n} - {y_m}} \right\| $为参考点云与待配准点云之间的欧氏距离.

混合高斯模型的中心在配准过程中与变换参数$ \theta $相关, 为了求得最优变换对$T(Y, \theta )$, 在每次迭代时需要极小化参考点云中数据点的负对数似然函数:

$ E(\theta , {\sigma ^2}) = - \sum\limits_{n = 1}^N {\log \sum\limits_{m = 1}^{M + 1} {P({y_m})p({x_n}\mid {y_m})} } $

(4)

由于变换参数$ \theta $包括旋转、平移等多个参量, 难以通过求导直接计算, 因此通过期望最大化算法EM求解, 反复进行期望步骤(E-Step)与极大步骤(M-Step)迭代来获得最优变换对$T(Y, \theta )$. 得到参数$ \theta $后应用到待配准点云y中, 实现最终配准并输出最佳变换矩阵作为ICP算法的位姿初始值.

1.2.2 ICP算法

ICP算法的核心思想是通过最小化点云之间的距离来实现配准, 计算简便且精度较高, 然而在初始值不精确时会收敛到局部极值^[19]. 因此在CPD算法进行点云预配准得到最佳变换矩阵后, 将其作为ICP算法的位姿初始值进行再配准, 从而避免ICP算法陷入局部极值. 本文采用传统LOAM算法中ICP的配准方式, 利用点云中的线特征或面特征进行匹配. 在进行匹配时, 我们通过迭代的方式来优化残差, 其中残差是由点到线或点到面的距离构造而成的^[20].

原点云中的边缘点到目标点云中的边缘线、原点云中的平面点到目标点云中的平面的距离计算公式如下:

$ {d_{\rm edge}} = \frac{{\left| {(\hat X_{(k + 1, i)}^L - X_{(k, j)}^L) \times (\hat X_{(k + 1, i)}^L - X_{(k, l)}^L)} \right|}}{{\left| {X_{(k, j)}^L - X_{(k, l)}^L} \right|}} $

(5)

$ {d}_{\rm plane} = \frac{\left|({\hat{X}}_{(k+1, i)}^{L} - {X}_{(k, j)}^{L})\text{ }\cdot\text{ }[({X}_{(k, j)}^{L} - {X}_{(k, l)}^{L})\times ({X}_{(k, j)}^{L}-{X}_{(k, m)}^{L})]\right|}{\left|({X}_{(k, j)}^{L}-{X}_{(k, l)}^{L})\times ({X}_{(k, j)}^{L}-{X}_{(k, m)}^{L})\right|} $

(6)

其中, j, k, l, m为其在相应边缘点与平面点的集合中的特征标识, $ \hat X_{(k + 1, i)}^L $为原点云中的第k+1帧的边缘点或平面点, $ X_{(k, j)}^L $, $ X_{(k, l)}^L $, $ X_{(k, l)}^L $为目标点云中的第k帧的匹配点.

将式(5)、式(6)构建的两个距离函数约束统一为:

$ \mathop {\mathit{loss}}\limits_{R, t} \left[ {\sum\limits_{\hat X_{(k + 1, i)}^L} {{d_{\rm edge}}} , \sum\limits_{\hat X_{(k + 1, i)}^L} {{d_{\rm plane}}} } \right] $

(7)

其中, R, t为当前帧与上一帧的相对位姿变换. 这样, 就可以把问题转换为求距离函数统一后的最小化的问题:

$ R, t = \mathop {\arg \min }\limits_{R, t} \frac{1}{2}{\sum\limits_{i = 1}^{{F_n}} {\left\| {\mathop {\mathit{loss}}\limits_{R, t} \left[ {\sum\limits_{\hat X_{(k + 1, i)}^L} {{d_{\rm edge}}} , \sum\limits_{\hat X_{(k + 1, i)}^L} {{d_{\rm plane}}} } \right]} \right\|} ^2} $

(8)

其中, $ {F_n} $表示匹配到的特征点数量. 可以通过高斯-牛顿迭代法(Gauss-Newton, G-N)求解此非线性优化问题, 计算残差的雅可比矩阵用于迭代优化, 最终得到R, t的增量, 最小化残差的平方之和.

1.2.3 判断场景结构

CPD算法可以很好地应对非结构化场景, 但其需要通过期望最大化算法来求解最优变换矩阵, 计算代价高. 而在结构化场景下使用ICP算法进行配准可以提高系统运行的效率与精度, 二者结合可以形成优势互补. 因此, 为了将对应的算法应用到合适的场景中, 对于场景结构的判断尤其重要. 场景结构判断的流程图如图2所示.

由于特征的数量不仅与场景的结构相关, 同时还与激光雷达的扫描线束以及点云重叠程度、场景几何特征相关, 而原始的点云特征提取仅仅依靠特征的数量多少来区分场景结构, 这样是不准确的. 所以为了增加对场景结构判断的准确性, 本文使用了基于预判和验证相结合的场景结构辨识方法, 把每一帧点云配准算法之前的特征匹配的总数与其对应帧点云的数量的比值, 再将这个比值乘以十倍的激光雷达的线束构成场景特征变量SFV. 在进行首次迭代计算场景特征变量SFV之后, 需要计算当前帧的表面曲率G并通过判断场景几何特征对场景进行验证. SFV的计算公式为:

$ \mathit{SFV}=10{K}_{\rm line}\cdot\frac{{F}_{n}}{P{C}_{n}} $

(9)

其中, $ {F_n} $为每一帧点云配准算法之前的特征匹配的总数, $ P{C_n} $为其对应帧点云的数量, $ 10{K_{\rm line}} $为放大系数, 因为点云在提取特征之前进行了体素滤波下采样, 使得SFV数值过小难以区分, $ {K_{\rm line}} $为所使用激光雷达的线束. 由于非结构化场景具有多样性、不规则性以及存在遮挡和噪声, 因此可提取出的特征点远少于结构化场景, SFV的值也会更小.

除了用SFV对场景结构进行初步判断外, 本文还结合表面曲率对其进行验证来提高对场景结构判断的准确性. 在每帧激光扫描下采样程度相同以及机器人的运动速度恒定的情况下, 理论上存在一个阈值$ {V_0} $. 在SFV与$ {V_0} $进行过比较之后, 再计算表面曲率G进行验证, 若G小于设定的阈值$ {G_0} $, 则说明场景较为平滑; 若大于等于设定的阈值$ {G_0} $, 则场景不规则. 通过计算当前帧点云数据P的协方差矩阵C, 再计算协方差矩阵C的特征值来获取主曲率, 即点云数据P处的最大和最小曲率值, 从而得到表面曲率G. 表面曲率G的计算公式如下:

$ C = \frac{1}{n}\sum {(N\times {N^{\rm T}})} , \;\;{\lambda _1}, {\lambda _2} = \lambda (C) $

(10)

$ G = {\lambda _1}\times{\lambda _2} $

(11)

其中, N为所计算点云P的法线方向向量, n表示最近邻点的数量, $ {\lambda _1} $和$ {\lambda _2} $为协方差矩阵C的特征值.

当SFV小于$ {V_0} $且G大于$ {G_0} $时, ICP容易收敛到局部极值, 处于非结构化场景. 此时先引入CPD算法进行预配准, 以降低系统运行的绝对误差. 然后通过ICP算法进行再配准, 以提高系统运行的精度和效率.

1.3 后端优化scan-to-map

由于里程计累积误差的存在, 当前扫描帧的估计位姿与实际位姿之间存在较大偏差^[21], 这会导致ICP算法匹配的初始值过大, 从而无法成功达到收敛状态. 而CPD算法收敛域较广, 因此在后端优化阶段首先使用CPD算法进行初步的非刚性形变配准, 然后再使用ICP算法进行精细的刚性变换优化. 通过组合这两种算法, 可以充分利用它们各自的优势, 有利于更好地纠正里程计的漂移, 提高点云配准的效率和精度. 最后将输出的位姿进行融合并更新, 最后输出轨迹和全局地图, 为后续的路径规划以及导航任务提供支持.

2 实验与分析

为了评估算法的精度和效率, 本文进行了大量公共数据集实验和真实环境下的实车测试^[22]. 实验平台为具有16 GB运行内存, 硬件环境为Intel i7处理器, 操作系统环境为Ubuntu 20.04, ROS (机器人操作系统)版本为noetic的个人笔记本电脑, 激光雷达为velodyne的16线雷达. 通过与LOAM, LeGO-LOAM, LIO-SAM算法对比, 来验证CPD-LOAM算法的精度和效率.

2.1 算法耗时分析

正常情况下雷达扫描频率为5–20 Hz, 为了满足算法的实时性要求, 前端scan-to-scan帧间匹配算法的执行时间是不能超过100 ms的. 图3为无人车搭载velodyne 16线激光雷达在4种不同的点云配准算法下的匹配时间随着雷达扫描帧的变化数据, 录制的场景为室内结构化场景. 其中ICP和CPD是单独的点云配准算法, LOAM为原始算法, CPD-LOAM为本文提出的算法.

图3很明显看出, 单独的点云配准算法ICP和CPD平均耗时均达到100 ms以上, 超时严重, 无法满足算法的实时性. ICP算法在点云精细配准方面表现良好, 但容易受到初始位置和噪声的影响, 且在寻找对应点时可能会产生较大的计算量, 特别是在非结构化场景的点云数据以及大规模点云数据中. 而CPD算法通过最大化点云间的概率一致性来找到匹配, 对初始位置不太敏感, 可以减少在寻找最近点时的计算复杂度. 两种算法均需要经历多次迭代, 所以耗时高. 结合使用ICP和CPD时, 先通过CPD进行预配准, 为ICP提供一个较好的初始位置, 然后利用ICP进行再配准, 两者形成优势互补, 从而减少整体的迭代次数和计算时间. 图3中可以看出, CPD-LOAM算法由于结合了ICP和CPD, 耗时最短, 可以满足实时性要求.

2.2 场景特征变量以及表面曲率效果验证

本文选用室内图书馆结构化场景和室外社区非结构化场景来验证场景特征变量和表面曲率对于区分场景结构的可行性, 各场景的SFV以及表面曲率G随雷达扫描帧的变化如图4所示. 其中Library为图书馆室内结构化场景, 路径中大多都是规则形的桌椅、图书柜等; 而Community为社区室外非结构化场景, 包含了大量的建筑物以及树木草地等. 图4(a)显示, 结构化场景的SFV是明显超过非结构化场景的, 进而表明了SFV在初步区分场景的效果, 并且阈值在3.5–4.0之间. 图4(b)说明了表面曲率在验证结构化与非结构化场景时效果尤其明显, 图书馆室内结构化场景的表面曲率G接近于0, 而社区室外非结构化场景的表面曲率G在4.4左右, 表面曲率在结构化与非结构化场景中区分尤其明显, 证明了表面曲率验证场景结构的有效性.

为了找到合适的阈值作为判断条件来区分场景结构, 本文通过线性插值的方法, 不断地调整$ {V_0} $与$ {G_0} $, 在室外社区非结构化场景下进行大量实验获得绝对位姿的最大误差Max, 误差中位数Median, 平均误差Mean, 最小误差Min以及均方根误差RMSE, 数据如表1所示. 可以看出, 当$ {V_0} $不断增大时, 绝对位姿误差会呈现出先减小再增大的趋势, $ {V_0} $的值越小, 说明前端里程计部分添加CPD进行预配准的帧数越少. $ {V_0} $的值越大, 说明前端里程计添加CPD进行预配准的帧数越多, 算法的精度也会越高. 根据线性插值得到一系列的插值点, 再利用这些插值点进行牛顿差分法递推, 构造插值多项式并得到插值函数的极值点是3.682, 即阈值$ {V_0} $. $ {G_0} $同样使用线性插值与牛顿差分法, 最终得到插值函数的极值点为1.253.

2.3 定位与建图实验分析

为了验证CPD-LOAM算法定位与建图的精度以及鲁棒性, 本文首先将CPD-LOAM与LOAM进行对比, 再与经典的激光SLAM算法LeGO-LOAM, LIO-SAM对比, 采用了KITTI数据集的00序列进行实验, KITTI数据集包含了采集自乡村、城市和高速公路等多个场景的真实数据, 而00序列为大范围的城市环境, 包含大量的建筑物、树木与汽车, 同时也存在较多分岔路口、颠簸道路与直道, 结构化与非结构化场景交替混合, 对于算法稳定性的验证具有极高的可靠性. CPD-LOAM算法与LOAM算法的轨迹相对于KITTI地面真值轨迹的对比如图5所示.

从图5中可以很直观地看出, CPD-LOAM算法的轨迹与地面真值更加贴近. 通过表2进行分析, LOAM算法的轨迹均方根误差为8.472 m, CPD-LOAM算法的轨迹均方根误差为1.316 m, 定位精度提升了84.47%, 说明CPD-LOAM算法的精度远高于LOAM算法.

图6为CPD-LOAM与LeGO-LOAM、LIO-SAM在KITTI00序列数据集下的轨迹对比, 根据局部细节放大可以看出, CPD-LOAM算法轨迹最贴合地面真值, 表现最好, 其次是LeGO-LOAM, LIO-SAM效果相对较差, 因为该场景下道路较为颠簸, 车辆会产生震动以及多传感器的时间戳不同步, 因此00序列下的IMU数据表现不稳定, 易发生缺失与跳变, 而CPD-LOAM在这种情况下仍能够很好地完成定位与建图, 具有更高的鲁棒性. 结合表2和图7中可以看出, CPD-LOAM算法的轨迹均方根误差相较于传统LOAM算法以及LeGO-LOAM、LIO-SAM算法, 有明显降低, 相较LeGO-LOAM算法降低了55.88%, 相较LIO-SAM算法降低了30.52%.

LeGO-LOAM、LIO-SAM和CPD-LOAM的三轴轨迹对比图如图8所示, 可以看出, 3种算法的轨迹在x和z轴上与真值最为接近, 而在y轴上有一定差异, 这与00序列的道路颠簸以及遮挡物较多有关. LeGO-LOAM由于采取了地面约束以及点云分割, 因此在平坦的道路上表现良好, 在颠簸的道路上会产生偏差. LIO-SAM由于在00序列下车辆的颠簸、震动以及多传感器的时间戳不同步, IMU的数据会产生丢失和跳变, 无法获得足够准确的惯性信息来获取车辆在低频运动下的状态和姿态, 从而导致误差累积. 而CPD-LOAM由于在非结构化场景下会引入CPD配准算法, 可以很好地适应非刚性的形变与遮挡问题, 通过建立概率模型和最大似然估计进行优化, 从而实现点云的对齐, 具有更高的精度和鲁棒性.

图9为CPD-LOAM与LOAM、LeGO-LOAM以及LIO-SAM算法在KITTI00序列下的建图结果, 可以看出CPD-LOAM算法的地图全局完整性明显优于其他两个算法, 未发生严重漂移. 在局部的分岔路口也可以看出, CPD-LOAM算法的树木、车辆等点云更为真实, 不会出现模糊不清与重影的情况.

3 结论

针对传统激光SLAM算法在面对复杂场景时无法区分场景结构, 并且在非结构化场景下会因为难以提取可靠稳定的特征而出现退化的问题, 本文提出一种联合CPD面向复杂场景的自适应激光SLAM算法. 首先引入场景特征变量的概念对场景结构进行初步判断, 然后从几何特征的角度利用表面曲率对其进行验证, 来提高对场景结构辨识的准确性. 此外, 根据辨识出的场景结构自适应地添加CPD点云配准算法来解决非结构化场景下特征退化的问题. 通过这种智能化的自适应机制, 可以有效减少配准过程中的误差和计算量, 提高点云配准的精度和效率, 从而有效地适应场景结构的复杂多变. 最后在KITTI数据集进行实验的验证, 实验结果表明: CPD-LOAM算法相较于传统LOAM算法, 定位精度提升了84.47%, 和LeGO-LOAM、LIO-SAM相比, 精度、效率以及鲁棒性也得到了更好地提升, 能够很好地适应复杂场景.