一种基于LiDAR-IMU-GNSS 系统同步进行车辆定位和路旁杆状物清查的方法

袁 超，潘文波，陈志伟，黄文宇，李源征宇，杨振宇

（中车株洲电力机车研究所有限公司，湖南 株洲 412001）

0 引言

路旁一般情况下主要存在着树木、路灯等杆状物，它们作为参照物在城市、城际和郊区道路环境和安全分析中发挥着重要作用［1］，但也给自动驾驶带来挑战。树木可能干扰卫星定位信号并阻碍传感器的可视性，导致出现定位精度下降、潜在的遗漏或错误检测等问题，甚至造成安全事故［2］。自动驾驶系统依赖对周围环境的全面理解以有效感知并应对复杂的道路情况，其中路旁杆状物作为重要的路边特征对车辆定位和安全分析具有重要影响。因此，开发一种可同时实现定位、高精地图创建以及路边杆状物识别和分类的有效方法对于提高驾驶安全至关重要。

传统测绘方式耗时、流程烦琐且设备昂贵。因此，航空影像、光检测与测距点云等遥感数据成为创建路边杆状物清单的重要资源［3］。移动激光扫描（mobile laser scanning，MLS）系统由于能够安全高效地获取准确的三维信息，因此在道路环境建模中变得越来越重要［4］。MLS系统通过收集激光脉冲的坐标来生成密集的点云，其速率可达每秒一百万个点，可见MLS 系统在提取道路环境（包括树木、交通标志和其他特征）信息方面具有强大的能力［5-6］。MLS 系统由高精度的全球导航卫星系统（global navigation satellite system，GNSS）、惯性测量单 元（inertial measurement unit，IMU）和测绘级激光扫描仪组成。虽然MLS 系统能够实现高精度的三维地图创建，但高昂的成本使其难以被广泛应用。此外，MLS 系统的地图创建和感知效率较低，难以满足自动驾驶的实时性需求。

虽然同步定位与地图构建（simultaneous localization and mapping，SLAM）技术对于车辆定位和实时路旁杆状物清单创建具有潜力，但目前仍缺乏能够实现同时进行车辆定位和路边杆状物清单创建的方法。主要的技术挑战如下：

1）长距离的树木遮挡会阻碍GNSS信号，显著降低卫星导航的精度。惯性导航系统（inertial navigation system，INS）依赖IMU进行车辆定位，而IMU累计误差大。

2）单独运行SLAM 算法和路旁杆状物提取算法会导致较高的计算成本。现有的SLAM 特征提取算法和路旁杆状物特征提取算法存在明显差异，很难采用统一的算法来同时实现车辆定位和实时路旁杆状物清单创建。

3）在道路条件复杂的地区，利用共享SLAM特征信息的杆状物检测算法容易受到环境干扰而产生误报。

基于杆状物结构识别、树木清单创建方法和SLAM 算法，本文提出了一个准确而稳健的车辆定位和路旁杆状物清单创建系统，其具有以下特点：

1）引入适用于长距离遮挡场景定位和地图构建的方案。该方案通过滑动窗口形式，紧耦合了激光雷达（LiDAR）、IMU和GNSS，减少了遮挡和退化场景中的误差积累，提高了系统的准确性。

2）采用两阶段的方法来最小化全局地图误差，通过GNSS辅助来修正累积的地图误差。

3）提出了一种杆状物检测方法，利用SLAM中的共享特征提取算法，从而最大化特征信息的利用。该算法减少了系统的计算成本，实现了同时定位和杆状物检测。

4）引入了一种利用方位角特征信息进一步减少误报的方法。

本文所提系统在城市和郊区进行了广泛的测试和评估。结果表明，该系统具有较好的准确性和稳健性，能够有效处理各种场景中的定位和杆状物清单创建任务。

1 研究现状

目前，使用MLS 自动提取灯杆、电线杆等形状单一的杆状物的信息已经有了快速、高效的解决方案，且准确率较高［5-6］。然而，树木的形状较为复杂，需要区分树干与树冠。在过去的十年中，关于从点云数据中进行个体树木分割也受到了重视，例如从密集的点云信息中获取树木高度、树干直径、胸径等属性信息［7-11］并基于点云数据进行3D目标检测［12］。此外，将相机与MLS点云结合起来添加图像信息的研究也受到了广泛的关注。例如，将全景图像与MLS点云融合，在分割过程中添加来自图像的颜色信息作为树木识别的行动准则［13］。类似的方法包括将多光谱图像信息与MLS点云融合以进行树木识别［14-16］。然而，这些依赖MLS 系统的方法需要昂贵的高精度GNSS/IMU定位设备和测量级激光扫描仪，限制了它们的大规模部署应用。此外，基于高精度点云数据进行自动树木识别和标定的方法实时性能较差。

激光雷达以其对光照变化的抗干扰性和精确测距能力在物体感知和导航领域得到了广泛的应用［17］。激光雷达传感器在各种机器人应用中越来越常见，如自动驾驶车辆［18］、无人机［19］等。文献［20］将SLAM 姿态估计转化为最小二乘优化问题，利用泰勒展开将非线性目标函数线性化，并通过梯度下降、高斯-牛顿或列文伯格-马夸尔特法等方法进行求解。最为经典的3D激光雷达SLAM 算法——实时激光里程计和建图（lidar odometry and mapping in real-time，LOAM）根据曲率从每帧中提取角点和平面点，并基于角点构建特征线，基于平面点构建特征平面。基于执行点云配准并使用最小二乘优化方法解决姿态，LOAM通过低频率建图和高频率定位确保了准确的定位和良好的建图结果。然而，该方法缺乏回环检测模块［21］，无法修正累积误差且不能进行重定位。文献［22］改进了LOAM，对点云进行聚类和分割，成功地将地面点与其他点分离。其基于聚类方法，过滤掉了不可靠的点云，提高了特征点的质量；还提出了一种两步优化方法来加速姿态估计和收敛速度；此外，还引入了一种基于欧几里得距离的回环检测方法，以消除累积误差。这种方法比LOAM 更高效，更适合在自动驾驶系统中部署，同时在系统完整性方面也超过了LOAM［22］。2021 年，文献［23］提出了一种利用激光雷达点云中的几何和强度信息的新型SLAM 解决方案，其设计了一个基于强度信息的前端里程计估计和一个基于强度信息的后端优化的方法，该方法优于仅依赖几何信息的SLAM方法。文献［24］介绍了一种名为R3LIVE的新型激光雷达-惯性-视觉传感器融合框架，其利用激光雷达、惯性和视觉传感器的测量来进行强大而准确的状态估计。该框架还整合了视觉惯性传感器的数据并渲染地图纹理。文献［25］提出了一种实时、准确、稳健的激光雷达SLAM 方法，其将非重复扫描激光雷达、IMU、轮式里程计和GNSS 紧密耦合，用于姿态估计和同步全局地图生成。尽管当前基于激光雷达的SLAM 算法在许多场景中已经呈现出足够的精度和稳健性，但在退化和大规模环境中仍面临挑战。此外，上述提到的SLAM方法中的特征提取算法并未考虑到杆状物检测和识别的要求，使得同时实现车辆定位和路旁杆状物清单创建变得困难。

2 系统介绍

本文提出了一种可以同时实现车辆定位和道路旁杆状物清单生成的多传感器融合方案，其能够在长距离树木遮挡卫星信号和光照条件不理想等场景下稳定运行。在选择定位、建图和感知传感器时，常用的选项包括GNSS、IMU、激光雷达和相机等。然而，相机易受光照影响，而GNSS+IMU在长距离遮挡场景下会累积显著误差。因此，本文主要采用激光雷达和组合惯导作为主要传感器。车辆定位和路旁杆状物清单创建系统的工作流程如图1所示。

图1 车辆定位和路旁杆状物清单创建系统概述Fig.1 Overview of vehicle localization and roadside pole-shaped object inventory creation system

在本文提出的系统中，首先通过将激光雷达采集的点云数据与组合惯导中的测量值相结合，对激光点云进行运动补偿和畸变去除等处理，再从点云中提取边缘特征、平面特征等信息。接着，提取的特征点被分别应用于杆状物检测模块和迭代卡尔曼滤波模块的计算中。杆状物检测模块对树干特征进行聚类和检测，并将属于树木特征点的属性信息发送到全局地图，以增加这些特征点的树木属性信息。迭代卡尔曼滤波模块利用特征点和局部地图构建残差方程，并结合IMU预积分对位姿状态进行更新。如果误差状态收敛，则输出位姿信息；否则，继续迭代。然后，根据迭代卡尔曼滤波模块输出的位姿信息来更新特征点并将其添加到全局地图中。最后，在全局地图中根据树木特征点的方位信息进一步过滤树冠的误报，并通过GNSS-RTK 信息或回环检测进一步优化关键帧的位姿。

2.1 特征提取

本节对激光里程计特征提取模块进行介绍，主要针对点云中面特征和线特征候选点筛选展开描述。

2.1.1 候选点计算

激光雷达感知周围环境信息，形成三维点云，一帧点云往往包含上万至数十万的点，如果全部用于计算，会极大消耗计算资源，且实时性也无法得到满足。如图2 所示，空间中存在大量的特征点云，如面点和线点，通过对该类点云进行提取，可以极大地减少计算过程中使用点的个数，从而节约计算时间［21-22］。

图2 激光雷达点云Fig.2 LiDAR point cloud

现有的特征点计算算法，如 LOAM 和 Lego-LOAM 等，通过计算同一线激光点中相邻点的空间距离，得出该点的线曲率，并以该线曲率作为分类特征点的标准。该方法可以有效地通过计算得到面点和线点，能很好地检测到路灯和电线杆，但是特征提取方法不适用于树木检测，且当激光入射角度和距离变化时，特征提取将变得不稳定。例如，由于现有的SLAM的特征提取方法在树叶密集情况下容易将树叶点云作为无效特征点滤除，导致后续基于特征点的树冠部分点丢失。当激光雷达入射角正对树叶且距离适中时，不仅有部分激光点云被表层树叶反射，还有相当一部分穿过表层树叶，被后面的树叶或树枝反射回来。在这种情况下，可以较好地提取出角点。当入射距离比较近且树叶茂密时，可能连续出现在同一线束上多个点被表层树叶反射的情况，计算得到的线曲率将明显变小，甚至会误判成面点。因此，本文将重点介绍如何解决树木检测的问题。

本文通过自适应空间几何候选特征计算方式，对面点候选集合P1和线点候选集合L1进行初步筛选。

记一帧完整点云为Q，待判断点为qi。首先对点云进行预处理操作，去除无效点后，根据点云中各点的行列索引，将点云映射至序列图上，如图3所示。点云中各个有效点都将被映射到序列图上，以方便后续查询操作。若点qi的行属性为m、列属性为n，则其将被映射到序列图中第m行、第n列，索引符号被记作indexm，n。

图3 点云序列映射图Fig.3 Point cloud sequence mapping

通过序列图对点进行索引，得到点pi。对点pi进行特征初步判断时，不再通过选取固定个数领域点的方式计算曲率，而是根据距离阈值 Δd对周围点进行筛选，并对与pi距离为 Δd的同一条线上的点pi～pi-m和pi～pi+n构建特征判断函数，具体如下：

式中：ci——候选点判断值；pi+j，pi-j——pi点前j点和后j点。

Nj计算公式如下：

计算得到ci后，对ci作如下判断，将其分类为P1和L1：

式中：Δc——平面特征点的阈值。

2.1.2 特征点筛选

根据式（1）～式（3）计算，得到P1和L1；进一步对L1进行筛选，得到筛选后的线点候选集合L2，并建立体素栅格，对密集点云进行采样，完成面特征点集合P和线特征点集合L的筛选。

针对L1构建分类，如图4所示。

图4 线点候选集合分类筛选Fig.4 Classification and filtering of candidate line feature point cloud sets

记L1中线点pli，其周围a×b邻域内线特征点个数记为k，线特征点邻域内同类点个数的阈值为ΔK，则根据式（4）对pli筛选：

图4中，橘色索引表示当前待筛选点pli，对其邻域 5 × 3 格内的点进行查询，比如索引为index4，4的点，其周围查询到 6 个同类点；而索引为（index4，n-1）的待筛选点，其周围仅一个同类点。设置ΔK=5，并代入式（4），则删除（index4，n-1）索引点，并在L2中加入index4，4索引点。

对P1和L2进行体素网格采样，首先根据各点坐标（x，y，z）计算各点所处体素栅格坐标（coordinateX、coordinateY、coordinateZ），假设体素栅格长、宽、高分别为l、w、h，则

一个体素栅格中可能存在多个特征点，这些聚集的特征点如果全部被用于位姿解算，不仅不会提升定位精度，反而会加大时间消耗。若某一体素网格中，点云簇过于聚集，则对其进行体素采样，每个体素网格保留单个特征点，用于位姿解算过程。通过体素采样的方式，能够解决点云簇过于聚集的问题，最终得到特征点集合P和L。

2.2 杆状物检测

为了确保车辆自动驾驶的安全性，需要实时检测杆状物的信息。路灯杆形状单一，易被检测；而树木形状复杂且需检测出树干部分。因此，本文首次提出直接利用SLAM中的特征点进行树木检测，以提高检测效率。大部分树木特征点云位于特征点集合L中，因此本文对集合L中的特征点进行聚类和提取树干特征。在提取树干之前，根据车辆姿态和雷达的安装高度，直接滤除地面以下和接近地面的特征点，这样可以进一步减轻计算负担；此外，这些接近地面的点通常代表灌木丛和路旁设备，可能在树干提取步骤中引起错误提取的问题。

树干可被定义为从根部或靠近地面的地平线开始向上延伸到一个或多个不确定点的树木的主干。树干的提取分为两个步骤，即聚类和树干识别。

基于密度的聚类（DBSCAN）方法将具有相似距离和角度的特征点分组到同一个聚类中。针对树干特征提取，本文对DBSCAN算法进行了改进，主要体现在搜索方向权重的调整和树干聚类的优化两个方面。首先，考虑到树干主要向上（即z方向）生长，雷达在z方向上测量的点之间的距离较大，而在x和y方向上较为密集，因此，在x、y和z方向上赋予不同的权重ωx、ωy和ωz。改进后的聚类算法中的搜索距离如式（6）所示。

式中：Dij——目标i与目标j的搜索距离；xi、yi、zi——目标i的坐标；xj、yj、zj——目标j的坐标。

调整权重系数，使得ωx=ωy＞ωz，此时Dij受x和y方向上距离的影响将大于z方向，这也符合树干现状特性及反射点在x、y方向和z方向上的差异情况。

为了满足实时性的需求，需要快速聚类出树干。首先选择高度约为1.5 m的特征点作为DBSCAN聚类的初始点。由于树干部分的点云比树冠部分更密集，特别是在x和y方向上的距离差异较大，因此可以根据树冠和树干之间特征点的差异将树干单独聚类出来。

树干反射的雷达点具有独特的形状特征，可以将其与其他目标区分开。因此，本文系统主要依靠聚类后的形状估计来综合判断目标是否为树干，即对一组聚类点进行形状估计，从而得到一个矩形框，通过矩形的高宽比来判断目标是否为树干；同时，将树干上方一定范围内的角特征点视为树冠点云。由于树冠点云较为稀疏，在地图构建的过程中，本文系统通过叠加多帧特征点来进一步识别树冠。

2.3 前端里程计

本文所提地图构建算法利用信号质量良好的历史GNSS-RTK 定位信息构建关键帧局部地图，以丰富点云地图历史信息。前端里程计利用位姿优化初值将当前提取的帧特征点投影至局部地图，并进行最近点查询。若最近点距离大于预先设定的距离阈值，则该特征点为离群点，无法构建重投影误差；反之，构建点云重投影代价函数（点-面距离、点-点距离），利用非线性最小二乘法迭代优化位姿参数。最后，根据位姿更新量进行关键帧诊断，若为关键帧，则更新局部地图。本文对局部地图采用滑动窗口法，在保证实时动态更新的同时，达到减少数据量和计算量的目的。点-面重投影误差rpi，f、点-点重投影误差rpj，s如式（7）所示。点-面重投影误差和点-点重投影误差组成的优化目标函数f(R，t)如式（8）所示。

式中：rpk，f——平面特征点重投影误差；pk——当前帧激光点云平面特征点；R——待优化的旋转矩阵；t——待优化平移向量；pk，m——pk在局部地图中的最近点；nk——局部地图平面点pk，m所在平面单位法向量；pj——当前帧激光点云边线特征点；pj，m——pj在局部地图中的最近点；Pf——当前帧激光点云中所有平面特征点的集合；Ps——当前帧激光点云中所有边线特征点的集合。

2.4 后端优化

为减小激光SLAM 前端位姿估计累计误差，SLAM 后端融合GNSS 观测约束和前端里程计约束，构建位姿图优化代价函数，迭代优化以减小前端累计位姿估计误差，提升定位精度。如图5 所示，变换矩阵TGs为在GNSS-RTK 信号丢失区段之前由GNSS-RTK定位模式输出的东北天坐标系下车辆位姿约束；变换矩阵Ti，i∈{0，1，2，…，k}为GNSS-RTK 信号丢失区段内待优化车辆位姿；变换矩阵TGe为GNSS-RTK信号恢复后由GNSS-RTK定位模式输出的东北天坐标系下车辆位姿约束；变换矩阵为激光SLAM前端里程计输出的相邻两帧位姿约束信息。待优化位姿目标函数如式（9）所示，其由位姿约束rT0、rTi，Ti+1、rTk组成。

图5 后端位姿优化图Fig.5 Diagram of pose optimization at rear end

式中：(·)˅——李代数到李群的映射。

3 实验结果与分析

为了评估所提出方法的性能，在城市和郊区道路环境中进行了广泛的测试。

3.1 实验设备

本系统选择的LiDAR 型号是RS-Ruby-80 线激光雷达，其最大测距范围为200 m；所使用的组合惯性导航系统（INS）是华测CGI-610，其提供（0.1+1×10-6D）m（D为INS 与定位基站的距离，单位为m）的GNSSRTK定位精度；采用SPAN-ISA-100C系统作为评估本文系统定位性能的基准参考；车载计算机配备了Intel i7-6820HQ 处理器，主频2.7 GHz，以及16 GB 的内存。此外，所有算法都是使用C++实现，并在Ubuntu Linux上使用机器人操作系统（robot operating system，ROS）进行执行。测试平台车辆和传感器的安装布置如图6所示。

图6 自动驾驶平台车辆和传感器安装Fig.6 Installation of sensors on autonomous driving platform vehicles

3.2 实验结果分析

在城市和郊区道路环境下的自动驾驶车辆平台上进行了同步实现车辆定位和路旁杆状物清单创建实验，实验涵盖了某市中心和郊区高速公路上的多个场景。自动驾驶汽车运行了2 300 s，行驶距离12.95 km。图7 显示了原始点云和杆状物检测的结果，可见本文系统能够清晰地提取杆状物。

图7 城市道路数据集的原始点云和杆状物提取结果Fig.7 Extraction results of raw point clouds and pole-shaped objects from urban road dataset

x、y、z方向的轨迹曲线如图8所示。灰色虚线表示SPAN-ISA-100C 设备提供的真实轨迹，蓝色曲线表示算法输出的关键帧轨迹。可以看出，轨迹误差在水平和垂直方向上都相对较小，并且表现出与真实情况相似的趋势。

图8 x、y、z 方向上的真实轨迹和关键帧轨迹Fig.8 Real trajectories and key frame trajectories in the x,y,and z directions

为了定量评价算法精度，选择轨迹的绝对位姿误差（APE）作为评价指标。仅考虑位置误差，而忽略方向误差，从而得到以米为单位的 APE 值，结果如图9 所示。图9统计结果表明，APE的最大值为0.223 m，最小值为0.001 m，平均值（mean）为0.027 m，均方根误差（RMSE）为0.035 m 。与高精度、高成本的定位设备SPAN-ISA-100C 设备提供的地面真实轨迹作对比，该算法的平均误差小于3 cm。这些定量分析结果表明，该算法具有较高的轨迹精度。

图9 APE 统计结果Fig.9 APE statistical results

4 结束语

为同步实现车辆定位和对道路环境的感知，本文提出了一种利用LiDAR-IMU-GNSS 融合系统同时实现车辆定位和路旁杆状物绝对位置清单创建的方法。其通过紧耦合LiDAR、IMU和GNSS信息，实现了在遮挡环境下准确的姿态估计；基于滑动窗口的优化融合定位算法，通过融合多传感器数据提高了系统的鲁棒性。此外，本文提出了一种与SLAM共享特征提取的树木检测方法，减轻了计算负载，使得系统能够实时地进行定位建图和树干检测。通过在城市和郊区的各种道路场景中进行广泛测试、评估，本文所提系统展示了厘米级定位精度，同时实现了实时自动创建路侧杆状物清单。总体来说，本文所提的融合LiDAR-IMU-GNSS系统为同时实现车辆定位和路旁杆状物清单构建提供了一种解决方案，有助于提高驾驶安全性和对道路环境的理解。但本文未剔除道路中运动的车辆和行人等因素，后续研究中需要在前端在线过滤动态物体，以进一步提高定位精度。