SLAM激光点云整体精配准位姿图技术

闫 利，戴集成，谭骏祥,刘 华，陈长军

1. 武汉大学测绘学院，湖北 武汉 430079； 2. 武汉大学测绘遥感信息工程国家重点实验室，湖北 武汉 430079

激光扫描(light detection and ranging,LiDAR)技术作为一种重要的三维空间数据获取技术，可以直接快速地获取物体表面三维信息[1]，广泛应用于测绘[2]、机器人[3]和无人驾驶[4]等领域。SLAM技术源于机器人领域，中文译为同步定位与制图技术[5]。将LiDAR与SLAM相结合，通过集成LiDAR扫描仪、IMU(inertial measurement unit)等传感器可在扫描平台移动过程中实时获取位置、姿态信息以及场景的空间信息。基于SLAM技术的激光扫描可以高效完成室内空间或室外小场景的三维点云数据获取，近年来在测绘领域得到了广泛的关注和应用[6]。

由于SLAM定位对实时性的要求，无法充分利用多余观测信息对位姿信息和点云数据进行优化，致使激光点云数据中存在内部不一致问题，影响点云后续处理与应用。实时SLAM点云优化，通常先对点云进行分段处理，然后通过闭环检测找出重叠点云构成的闭合环，以此构建位姿图进行优化[7-10]。这类算法存在闭合环检测错误的可能，且无法处理局部区域内部的误差[11]。测绘领域对三维激光点云数据精度要求高，对数据实时性要求较低，可以对点云进行离线的全局优化，从而改善点云内部不一致，提高点云的相对精度。目前的点云全局优化主要针对地面静态式扫描激光扫描点云，方法是将一站点云视为一个视图，利用闭合环条件约束对多视点云进行配准和优化。文献[12—13]提出一种基于光束法区域网平差进行点云整体配准方法，但需要利用靶标；文献[14]提出了一种严密的闭合条件约束配准方法，使用点云间的坐标转换参数构建误差方程，进行优化；文献[15—16]提出一种通过将点云分为两份分别配准，比较转换参数的点云配准质量的评价方式；文献[17]提出了一种利用所有重叠点云对应点作为观测值同时解算配准结果的全局优化的算法。

SLAM点云为移动式实时扫描点云，每一帧点云为一个视图，点云视图数多，重叠关系复杂。为了充分利用重叠点云中的冗余信息对SLAM点云进行精度优化，本文提出一种基于位姿图优化理论[18-19]的离线整体精配准方法。该方法以实时获取的平台轨迹的位姿信息为初始条件，通过迭代最近点算法(iterative closest point,ICP)[20]对在不同时刻扫描的重叠点云进行配准，形成对扫描平台轨迹的约束并对其进行优化，利用优化后的轨迹对点云进行修正。为提高计算效率，本文采用分层优化策略，即将优化分解为局部区域优化和整体优化两个层次。

1 位姿图优化理论基础

1.1 位姿图表达

图优化理论是一种将图论和非线性优化相结合的理论，通过将优化问题转化为图形式进行表达，从而直观地反映出优化变量与约束条件之间的关系，便于拓扑结构操作和构造目标函数[21]。用G={V,E}表示图结构，V代表图的顶点，E表示边。针对优化问题，图的顶点代表待优化参数，边代表参数之间的约束条件。图优化的目标是最小化所有约束代价，进而通过优化解法获得最佳待求参数。位姿图(见图1)是优化图的一种具体形式，本文将其用于解决具有复杂点云视图结构的轨迹优化问题。

图1 位姿图示例Fig.1 Pose graph

1.2 优化求解

(1)

式中，n为轨迹中的位姿总数。如果顶点vi、vj间不存在边ei,j作为约束时，则残差r(vi,vj,ei,j)=0。

位姿图优化作为一种全局优化方法，其中每一条边都会直接或间接对所有顶点的估计值产生影响，一个错误的约束就足以导致优化质量的下降，同时降低识别粗差的能力[22]。在构建位姿图的边时，需要对大量点云进行配准，周围环境中存在的对称、重复结构，噪声，点云重叠度低等因素，都会造成配准失败。为提高解算的稳健性，减小异常值对结果的影响，本文通过附加损失函数对式(1)进行改进

(2)

式中，ρH为Huber损失函数[23]。

由于目标函数为非线性函数，可使用L-M(Levenberg-Marquardt)优化方法[24]进行解算，获得扫描平台轨迹的最优位姿参数，进而根据优化后的轨迹对点云进行修正，达到改善点云质量的目的。

2 基于位姿图的点云优化

本文所使用的点云优化方法是通过对扫描平台的轨迹进行全局优化以提高点云精度，方法流程如图2所示。过程包括数据预处理、局部区域点云优化和整体点云优化3部分。预处理的目的是去除对点云配准贡献小的点，如离群点，并估计点云配准所需的法向量。预处理后按一定的时间间隔将点云和轨迹分为若干小段，每一小段视为一个局部区域，其含有若干帧点云。为了处理局部区域内点云误差，以单帧点云为配准单元，通过外包盒寻找点云之间的重叠关系，对重叠点云进行配准。SLAM激光点云经过实时算法的处理，点云间可认为已经完成粗配准，可以直接使用ICP算法进行配准。利用配准结果对轨迹进行约束，构建位姿图对轨迹进行优化，完成局部区域内点云优化。最后将优化后的局部区域点云合并，合并后的每一段点云视为配准单元，采用类似局部区域优化的方式完成整体点云的全局优化。

图2 SLAM点云整体精配准方法Fig.2 Global fine registration method of SLAM point

2.1 预处理

为了提高配准的可靠性和效率，首先采用距离滤波和统计距离阈值滤波对点云数据进行预处理，剔除扫描距离过大点和孤立点。随着扫描距离的增大，点云的精度和密度都会下降，同时噪声增多，根据扫描仪的有效扫描距离和扫描环境，设置距离阈值Dmax，去除扫描距离大于Dmax的点。为了去除孤立点，避免其对之后的法向量计算造成影响，对点云进行统计距离阈值滤波[25]。该方法计算每个点到最邻近点的距离的平均值μ和标准差σ，去除距离最邻近点距离超出μ±σ的点。最后，在预处理中计算每个点的法向量，通过对各点邻域内坐标的协方差矩阵进行特征值分解，其中最小的特征值对应的特征向量为该点的法向量[26]。

(4)中秋节，将近正午的光景，在北平曾家旧宅的小花厅里，一切都还是静幽幽的，屋内悄无一人，只听见靠右墙长条案上一架方棱棱的古老苏钟迟缓低郁地迈着他“嘀塔嘀嗒”的衰弱的步子，屋外，主人蓄养的白鸽成群地在云霄里盘旋，时而随着秋风吹下一片冷冷的鸽哨响，异常嘹亮悦耳，这银笛一般的天上音乐使久羁在暗屋里的病人也不禁抬起头来望望：从后面大花厅一排明净的敞窗望过去，正有三两朵白云悠然浮过蔚蓝的天空。

2.2 位姿图构建

由于SLAM激光点云在获取时存在定位误差，导致对同一区域扫描时获取的不同帧点云之间无法完全重合，通过对这些点云进行配准可以构建对轨迹中位姿的约束。针对室内环境高度结构化的特点，本文选择使用点-切平面ICP算法对点云进行配准[27]，通过采用最小化点到目标点云对应点切平面的距离完成配准。作为一种两视点云精配准的方法，相比于普通ICP算法，点-切平面ICP能够建立更精确的同名点，收敛速度较快、精度更高[28]，其算法模型为[29]

(3)

式中，si=[sixsiysiz]T是待配准点云中的点；di=[dixdiydiz]T是目标点云中的对应点；R代表旋转矩阵；t代表平移参数；ni=[nixniyniz]T为点si的法向量。

(4)

式中，R(q)表示四元数q对应的旋转矩阵；边ei,j的实际意义为在点云ci对应的局部坐标系下，两视配准后的点云cj与点云ci的扫描中心的相对位姿关系。

rtran(vi,vj,ei,j)=R(qi)T(ti-tj)-pei,j

(5)

(6)

式中，rtran(vi,vj,ei,j)、rrota(vi,vj,ei,j)分别为平移和旋转对应的残差；pei,j和qei,j分别为边ei,j的平移和旋转部分；vec(q)为四元数的虚数部分[qxqyqz]。

结合平移和旋转残差的总体残差为

r(vi,vj,ei,j)=wt‖rtran(vi,vj,ei,j)‖2+

wr‖rrota(vi,vj,ei,j)‖2

(7)

式中，wt和wr分别为平移和旋转的权重。

2.3 分层次优化

由于SLAM激光点云包含大量的扫描帧数，且相互之间存在大量重叠，若要一次性构建整体点云的位姿图，需要在各帧点云间进行大量配准，导致优化计算耗时过长。因此，通过将点云优化分解为局部区域和整体两个层次进行，以减小构建位姿图时的计算量，提高算法效率，过程如图3所示。

预处理后，将点云和轨迹按时间间隔ts分段，每一小段视为一个局部区域，其中含有若干帧点云。在局部区域层次上进行点云优化时，首先使用分段后的轨迹构建局部区域内的位姿图顶点。然后，在每个分段区域内通过对每帧点云的外包盒进行相交检测，找出具有重叠关系的点云，使用点到切平面ICP对重叠点云进行配准，每次成功进行重叠点云的配准后，使用2.2节中描述的方法构建位姿图中对应顶点间的一条边。局部区域内的位姿图构建完成后，对位姿图进行解算，获得优化后的分段轨迹，对分段内点云进行修正。

完成局部层次的优化后，对每个分段内的点云进行合并，结果如图3(b)所示。以合并后的点云为配准单元进行配准，计算配准时建立的同名点对数与目标点云点数的比值α作为有效重叠度，在整体点云位姿图构建时通过阈值αmin去除有效重叠度低的点云间的不可靠配准结果。位姿图解算后，根据优化后的整体轨迹对点云进行修正，完成对整体点云的优化。

3 试验与分析

为了验证本文算法的有效性，本文采用Google公司的Cartographer算法[9]示例数据作为试验数据对本文算法的效果进行验证。该数据包含数据1和数据2两部分，由装备了两个VelodyneVLP-16LiDAR扫描仪和IMU的3D LiDAR背包获取，扫描地点为德意志科技博物馆，图4为所用试验数据的俯视图。数据1为单个楼层扫描结果，数据2为3个楼层扫描结果，并包含一个大型展厅，数据具体参数如表1所示。试验的计算机环境为IntelXeonE5-2660 v3 @2.60 GHz，128 GB内存。

表1 试验数据说明

在预处理阶段，根据扫描仪的性能和室内环境的尺度，将点云的最大有效距离阈值Dmax设为20 m，去除超出距离阈值的点。局部区域优化时，按时间间隔ts对预处理后的点云和轨迹进行分段，分别在每段点云内部通过ICP配准构建局部区域的位姿图。在位姿图残差计算时，参考Cartographer算法[9]中的设置，将平移和旋转代价的权值wt、wr分别设为1、100，将每段点云中第1帧点云对应的顶点设为基准，使用L-M算法进行解算。整体点云优化时，对局部区域内优化后的点云进行合并，在整体点云范围内搜索重叠点云进行配准，为了保证配准的可靠性，将最小有效重叠度αmin的值设为0.05，进行整体点云的全局位姿图构建。

图3 分层次配准过程(每种颜色代表一个分段区域内的点云)Fig.3 The process of hierarchical registration(each color represents the point cloud in one segmentation

图4 Google Cartographer算法德意志博物馆示例数据Fig.4 Google Cartographer sample data set in Deutsches Museum

根据优化后获得的轨迹对点云进行修正，得到整体精配准后的点云。以时间间隔ts=3 s时的点云优化结果为例，图5、图6为点云数据优化前后场景的定性对比。从图5(a)、(b)中1，(c)、(d)中5，图6(c)、(d)中5、6，(e)、(f)中8、9的对比可知，优化后点云中的物体轮廓更加清晰；从图5(a)、(b)中3、4，(c)、(d)中6，图6(c)、(d)中7，(e)、(f)中10、11的对比可知，点云中的墙面的厚度变薄；从图5(a)、(b)中2，图6(a)、(b)中1、2、3、4的对比可知，同一物体存在多个扫描结果的现象也得到了消除。

图6 数据2优化前后对比Fig.6 Data 2 scenes before and after optimization

(8)

(9)

为了分析不同的分段时间间隔ts取值对点云优化的用时和优化精度的影响，以1 s为间隔对ts取值进行点云全局优化，统计优化用时，并计算优化前后整体点云所有重叠帧间的对应点对距离RMSE，结果如图7和表2所示。

图7 不同时间间隔ts取值下优化用时Fig.7 The time cost before and after optimization in different values of ts

Tab.2TheRMSEbeforeandafteroptimizationindifferentvaluesofts

分段时间间隔ts/s数据1的RMSE/cm数据2的RMSE/cm优化前优化后优化前优化后110.54.814.26.6210.54.514.36.7310.74.314.46.7410.94.414.46.8511.04.514.57.0平均值10.74.514.46.8

由图7(a)、(b)可以看出，局部优化用时随分段时间间隔ts的增大近似线性增长，而整体优化用时则随着时间间隔ts的增大，开始时快速减小，之后减小速度逐渐减慢。在时间间隔ts等于2 s时，数据1和数据2优化总用时最少。从表2中可以看出，在不同的时间间隔ts下，点云的精度计算结果没有明显变化，说明了本文方法的优化效果受不同时间间隔ts的影较小。其中，优化前的RMSE的变化，是由于时间间隔ts的改变，计算得到的分段点云不同，导致获取的同名点对不同造成的。数据1优化后，不同时间间隔下平均点云RMSE从10.7 cm变为4.5 cm，减小了58%，在ts=3 s时优化后RMSE最小为4.3 cm；数据2优化后，不同时间间隔下平均点云RMSE从14.4 cm变为6.8 cm，减小了53%，在ts=1 s时，优化后RMSE最小为6.6 cm，说明优化后点云精度明显提高，验证了本文方法的有效性。

4 结 论

基于SLAM技术的激光扫描可以高效地获取目标场景的三维信息，但精度受限，可通过全局优化的方法提高SLAM激光点云的精度。由于SLAM激光点云具有重叠度高，帧数多的特点，导致配准关系复杂，计算量大，传统的点云优化方法不适用于SLAM激光点云的全局优化。本文针对SLAM激光点云相对精度优化问题，提出了一种分层次的点云整体精配准算法。通过对获取的点云进行分段将全局位姿图的构建分为两个层次。首先在分段内部使用点-切平面ICP进行配准，构建局部位姿图对点云进行优化，保证分段内部点云的一致性，再对分段点云进行配准，构建全局位姿图，对整体点云进行优化。通过试验，对算法的可行性和有效性进行了验证。试验结果表明，优化后点云同名点对间的距离中误差减小了约50%，点云中重复扫描间的差异减小，物体轮廓更加清晰，说明点云的内部一致性得以改善。