智轨电车多传感器融合定位研究

潘文波，李源征宇，龙 腾，陈志伟，黄文宇，秦小辉

（1.中车株洲电力机车研究所有限公司， 湖南 株洲 412001；2.湖南大学 汽车车身先进设计制造国家重点实验室，湖南 长沙 410082）

0 引言

随着城市的建设发展及常住人口的不断增多，交通拥堵、城市运输能力不足及交通运输所带来的城市污染等问题愈发显著。相较其他交通工具，智轨电车具有高效、快捷、绿色且成本较低的特点，近年来已相继在株洲、宜宾等城市展开试点商业运行。

车辆定位技术是智能轨道快运系统的核心技术之一。对车辆的精确定位可以有效提升车辆决策控制效率与准确率，保障车辆行车安全。多数有轨列车具有专用的交通路线和开阔的交通环境，而智轨电车的运行路线环境则相对复杂。大部分智轨电车采用混合车道和半封闭车道，路权相对而言更加开放，因此不能照搬现有有轨列车的定位方法。乘用车自动驾驶作为近十年来在世界范围迅速发展的技术，正深刻影响着现有交通系统［1］，由于其与智轨电车运行环境相似，感知、决策和定位等相关技术可进行移植。乘用车智能驾驶定位技术根据输出形式可分为两类：全局定位和相对定位。

典型乘用车智能驾驶定位技术多采用基于全球导航卫星系统（global navigation satellite system，GNSS）的全局定位方法。该方法虽可有效抑制全局定位误差，但其中单点定位精度为米级，因此无法满足自动驾驶的需求［2］。全球导航卫星系统的实时动态（real time kinematic，RTK）载波相位差分技术可全天候提供相对于全局坐标系的厘米级精准定位［3］，且无累积定位漂移也是差分定位技术的显著优势。该技术虽在自动驾驶领域被广泛应用，但仍存在致命弱点，即由于多径干扰，其无法锁定基准站。此外，在城市楼区、隧道等有遮挡的环境，信号强度会大大衰减［4］，基于GNSS-IMU组合导航的定位方法可在短暂失去卫星信号的环境中提供定位服务，却不可避免地引入了累计误差，若长时间失去卫星信号，其定位精度会下降。

为弥补上述定位技术的不足，近年来乘用车智能驾驶定位技术多引进同步定位与建图（simultaneous localization and mapping，SLAM）技术，其可通过车载传感器收集环境信息以构建地图，并同步推算当前车辆位姿［5-7］。SLAM技术一般采用激光雷达和摄像头作为主要传感器，其中激光雷达具备厘米级的远距离测距精度，相对于传统的相对定位方法，SLAM技术定位更加准确，是里程计技术研究的热点［8-10］。如基于场景识别算法将当前帧点云描述信息与先验点云地图进行匹配，利用匹配信息对最近点迭代（iterative closest point，ICP）算法进行精确修正，以获取当前帧全局位姿，有效修正了累计定位漂移，提升了推算精度［11］。基于3D点云正态分布（3D-NDT）求解定位信息的方法，相较于ICP算法，其通过构建目标点云和参考点云正态分布模型，利用模型距离代替点-点距离，降低了ICP计算量，提升了定位系统的鲁棒性［12］。在提升点云匹配精度、增加特征匹配效率和减少算力需求等方面，均有相关研究论文［13-15］。目前基于相机传感器的图像处理技术比较成熟，且相机传感器的成本较低，也是常用的SLAM传感器，存在多种基于该传感器的位姿估计研究［16-18］，但测距精度不足，导致视觉里程计精度较低，难以满足高阶自动驾驶厘米级定位精度需求。此外，使用多传感器融合来实现相对局部定位的方法成为新的研究热点，如利用惯性传感器（inertial measurement unit，IMU）测量值来完成相机尺度的初始化以及利用IMU预积分数据来预测相机位姿，以提升特征跟踪算法的匹配速度和鲁棒性［19］，实现基于IMU测量值和视觉特征紧耦合的高精度视觉-惯性里程计［20］。但是基于SLAM的定位方式（包括融合IMU和轮速信息）在远距离、长时间建图定位过程中，累计误差会带来定位漂移，因此急需探索消除累计误差的新方法。

针对复杂城市环境对遮挡、长时间和长距离定位的需求，本文提出一种基于激光雷达-全局地图匹配的长时、长距、高精度定位系统，其定位主要包含两个步骤：全局地图构建和地图匹配定位。通过引入GNSS绝对位置约束，消除激光里程计累计误差，以构建全局一致的高精度地图；通过融合全局地图匹配定位信息和动态局部地图匹配定位信息，降低环境变化、车辆移动等因素对定位精度的影响，从而有效提升定位系统的精度和鲁棒性。

1 基于多源传感器融合的建图与定位系统框架

基于多源传感器融合的建图与定位系统框架由建图模块和定位模块这两个部分组成，如图1所示。

图1 建图与定位系统框架图Fig.1 Mapping and localization system frame diagram

建图模块首先利用GNSS时间信息和激光点云时间信息对相邻两GNSS定位数据进行插值处理，以获得与激光点云时间对齐的GNSS定位信息。之后，判断GNSS定位信息状态，根据状态信息分别采用不同策略求解车辆位姿。若存在RTK定位数据，则根据RTK定位数据计算当前车辆位姿信息。若不存在RTK定位数据，则利用历史帧位姿信息，构建局部点云地图；提取当前帧激光点云特征点，并与局部地图相匹配，以估算当前车辆位姿信息；待恢复RTK定位模式，紧耦合RTK定位信息和激光里程计位姿估计，优化位姿参数，完成车辆位姿求解。最后，利用位姿信息对各关键帧点云进行投影，对点云地图进行滤波，以降低地图数据量，并存储全局点云地图。

定位模块用于判断GNSS定位信息状态。若存在RTK定位数据，则根据RTK定位数据计算当前车辆位姿信息。若不存在RTK定位数据，则结合历史帧位姿信息、当前帧激光点云特征点和建图模块存储的全局点云地图，计算当前车辆位姿信息。

2 建图与定位

GNSS RTK可实时提供厘米级定位精度，但市区的高楼、绿化种植等物体易遮挡GNSS信号。尽管组合惯导在GNSS或RTK丢失路段信息时仍可持续提供定位数据，但受惯性测量单元噪声和零偏的影响，累计漂移逐渐增大，使得定位精度难以满足项目需求。虽然基于全局地图匹配的定位算法既不依赖外界信号又无累计误差，其凭借实时、鲁棒、精准等特性被广泛应用于自动驾驶领域，但是全局地图匹配需预先构建全局地图，而如何构建精准的全局地图仍具有挑战性。

2.1 全局地图构建

全局地图由多帧环境数据依据空间位姿叠加而成。为减少地图重影、构建清晰的点云地图，需获取准确的位姿估计。本文通过融合GNSS RTK定位信息和激光SLAM位姿估计信息，获得连续、稳定、准确的车辆位姿参数，从而提升全局点云地图构建精度：

（1）对于GNSS RTK良好工况，为充分利用GNSS RTK定位精度高、无累计定位误差的优点，根据激光点云的时间信息对相邻两GNSS RTK定位数据进行插值处理，以获得与激光点云时间同步的全局定位信息。

（2）对于GNSS RTK异常工况，若GNSS RTK信号质量较差，则利用激光SLAM前端里程计估计车辆位姿并提取关键帧。在GNSS RTK信号较弱的周期内，激光定位具有一定的累积定位误差；待GNSS RTK信号恢复正常，结合GNSS RTK在这一周期始末阶段的观测约束和激光里程计的约束，利用梯度下降算法迭代优化激光雷达帧的位姿参数，以减少累计位姿误差，进而提升地图构建精度。

在经典的激光SLAM建图算法中，如LOAM（Lidar odometry and mapping）算法［5］，其构建的地图为激光单帧点云逐渐累积的结果。虽然此算法对点云进行体素滤波处理，但仍存在特征不明显、点云过于密集且地图存储量较大等问题。因此，在有效降低点云地图存储量的前提下，为了提升地图的质量，本算法采用SLAM中的关键帧处理方法。其根据车辆位姿变化量提取关键帧，对每一帧关键帧内的点云进行聚类；在进行点云滤波时，对非特征点云类和特征点云类采用不同的滤波体素栅格值，以增强一帧点云中特征点云聚类的影响，优化点云的质量。最后，将各关键帧的激光点云通过重投影进行叠加和点云滤波处理，以获得更加均匀、丰富的点云地图。

2.1.1 特征提取

智轨电车的运行环境十分复杂，道路两边的墙体具有聚集的面特征，城市道路设施（如照明路灯杆、广告牌杆等）具有强烈的线特征，而行道树以及聚集的人群等具有不规则的点云聚集特征，会增加角点和面点的误检测率，降低定位精度。因此，在特征提取阶段，利用激光雷达采集的点云信息来提取环境中的面、线等信息；为提升计算效率，激光点云采用深度图格式。深度图格式类似于图像，有固定的行和列，图元素与激光点云相对应。综合考虑计算时长和里程计精度，设定空间区域待选特征点提取数量并以均匀特征分布；根据聚类信息对边线待选点进行过滤，获取较为稳定的边线特征点。根据六自由度可视性筛选平面特征点，如沿空间三轴平移和绕空间三轴旋转。图2示出平移可视性示意，沿平面法线n平移具有良好约束；沿法线n垂直的任意方向t产生无约束滑移。图3示出旋转可视性示意，绕平面法线n旋转，产生无约束旋转［9］。

图2 平移可视性Fig.2 Translation visibility

图3 旋转可视性Fig.3 Rotational visibility

特征点提取方法如下：

（1）计算深度图行数量和列数量

根据激光雷达线束数量和水平分辨率，计算深度图行数量和列数量：

式中：nr——深度图行数量；nc——深度图列数量；nl——激光雷达线束数量；rhor——激光雷达点云水平分辨率。

图4示出空间激光点云示意图。

图4 空间激光点云示意图Fig.4 Schematic diagram of space LiDAR point cloud

根据点云三维空间位置P(x，y，z)，计算竖直夹角和水平夹角。图4中，竖直夹角为PO（点云P与激光雷达坐标系原点O)连线与激光雷达坐标系XOY平面的夹角；水平夹角为PO连线在XOY平面内投影p与X轴方向的夹角。利用角度信息，计算该点在深度图中的行(r）和列（c）信息，并将其储存至深度图对应像素点(r，c)：

式中：Vu——激光雷达竖直视场角最小值；Hr——激光雷达水平视场角最小值；Dver——激光雷达垂直视场角；Dhor——激光雷达水平视场角。

（2）计算点云曲率，获取最终的特征点

将深度图各行进行切分，分别计算区域内点云曲率pcur。由于激光雷达垂直分辨率远低于水平分辨率，目前多选用水平临近点计算点云曲率。

为充分结合水平临近点和垂直临近点几何特征，本文首先利用水平临近点计算曲率，并选取待选特征点集。然后，引入点云垂直方向临近点，对待选特征点集进行过滤，获取最终的特征点。文中，水平临近点曲率计算是通过在点云左右两侧各取m个点，根据所选临近点集与点云的深度差异程度来表示曲率大小（差异程度越大，曲率越大），根据曲率大小对点云进行排序。按预先设定的曲率阈值，将曲率大于设定阈值为边线待选点，曲率小于设定阈值为平面待选点。为使特征点分布较为均匀，相邻两待选特征点间需间隔n个点，且在各分割区域待选平面特征点数量应少于阈值nflat，待选边线特征点数量应小于阈值nline。曲率计算如下：

本文预先采用深度图，结合深度图对提取的边线待选点进行聚类处理，可降低聚类过程中临近点搜索计算量。若聚类点云数量不少于设定阈值，表明该待选点稳定，保留该点；反之，则表明该待选点不稳定，剔除该点。该方法能有效剔除不稳定边线点，减少环境观测噪声，提升定位精度和鲁棒性。通过深度图查询其上下两行临近点和左右两列临近点，并利用空间平面方程对该点及其临近点进行平面拟合。若平面拟合成功，保留该平面待选点及其所在平面单位法向量；否则，删除该平面待选点。

2.1.2 位姿估计

为兼顾实时性和精确性，结合前端里程计和后端姿态优化进行位姿估计。前端里程计利用位姿优化初值，将所提取的当前帧特征点投影至局部地图进行最近点查询。若最近点距离大于预先设定的距离阈值，则该特征点为离群点，无法构建重投影误差；反之，构建点云重投影代价函数（点-面距离、点-点距离），利用非线性最小二乘法迭代优化位姿参数。最后，根据位姿更新量进行关键帧诊断，若为关键帧，更新局部地图（本文局部地图采用滑动窗口法，在保证实时动态更新的同时，达到减少数据量和计算量的目的）。后端姿态优化用于减少激光SLAM前端里程计累计误差，SLAM后端融合GNSS观测约束和前端里程计约束，构建位姿图优化代价函数，迭代优化以减少前端累计位姿估计误差，提升定位精度。

2.2 融合定位

GNSS RTK定位方法与点云-全局地图匹配定位方法均具有无累计定位误差的优点，且具有一定的互补性。智轨电车所运行的城市环境复杂多变，采用GNSS RTK定位技术，在较为空旷的无遮挡场景下，信号良好，可获得准确的定位信息，但在建筑较为稠密的道路场景，信号易被遮挡，无法获得连续稳定的定位信息；采用点云-全局地图匹配定位技术，在空旷道路场景，难以提取有效的平面点和边线点，无法实现特征点和点云地图的良好匹配，具有较大的位姿估计误差，但在建筑较为稠密的道路场景中，可提取丰富的平面点和边线点，实现特征点与点云地图的良好匹配，获得较为精准的车辆位姿信息。因此，本文提出一种车辆定位方案，其融合GNSS RTK和点云-地图匹配这两种定位方法，以弥补二者的缺点，从而获得实时连续、稳定的车辆位姿。该方案主要基于是否有稳定的GNSS RTK信号来进行切换，若GNSS RTK信号质量良好，将GNSS RTK定位信息作为当前车辆位姿最优估计；若GNSS RTK信号质量较差，利用车辆匀速运动模型计算待优化位姿初始值，提取当前帧激光点云特征点并与已知点云地图相匹配，构建点-面距离残差和点-线距离残差，计算相应雅克比矩阵，利用非线性优化技术迭代求解最优位姿估计。本文的融合定位方法具有普适性，在空旷的无遮挡场景以及建筑较为稠密的道路场景中均有准确的定位效果，能够适用于智轨电车运行的各种道路场景。

由于城市道路复杂多变，在环境变化较大的路段，若只采用全局地图匹配会产生误匹配，导致优化问题陷入局部最优，降低定位系统的稳定性。为有效克服环境变化对点云匹配算法的影响，本文引入实时动态局部地图，通过判断当前局部地图点云与全局地图点云的差异，对当前道路环境的变化进行判断；采用点云-局部地图匹配和点云-全局地图匹配松耦合技术，有效提升匹配定位精度和系统鲁棒性。

3 实车验证与结果分析

为验证GNSS RTK定位方法在实际运行路况中的可靠性和准确性，在环境复杂度和路径长度接近实际应用的道路上进行了算法相对位姿估计误差实验。测试采用智轨电车实车，融合定位系统主要硬件设备如图5所示，车顶选用三维激光雷达传感器、组合导航系统，并配置了工控机，配置信息见表1。运行路线中既有结构化场景又有非结构化场景，如图6所示。主要传感器及数据处理平台的基本信息见表1。在软件层面，验证平台采用Ubuntu操作系统，并采用机器人操作系统（robot operating system，ROS）进行开发。

图5 实验硬件平台Fig.5 Experimental hardware platform for autonomous rail rapid tram

表1 融合定位系统的硬件配置Tab.1 Hardware configuration of the fusion positioning system

全局地图构建的实验场景为开放道路，如图6所示，车道旁有树木与建筑物，整体呈现长方形形状。该环境条件复杂度较高，局部路段存在卫星信号弱的情况；大部分路段信号较好，GNSS定位精度可以达到厘米级。在该场景下，智轨电车保持30 km/h以下的车速行驶进行地图数据采集，通过融合GNSS RTK定位信息和激光SLAM位姿估计信息以获得连续、稳定、准确的车辆位姿参数，提升全局点云地图构建精度，实现高精度地图的建立。图7（a）和图7（b）分别是激光雷达点云的面特征和线特征提取效果图，其中彩色点云为提取的特征，白色为背景点云，能够清晰提取出路面的平面特征和周边树木、灯杆等的线特征。

图6 智轨电车行驶环境Fig.6 Driving environment of the autonomous-rail rapid tram

图7 激光雷达点云的特征提取Fig.7 Feature extracted from LiDAR point cloud

图8（a）为行驶过程生成的全景点云地图，一圈长度为2.7 km；图8（b）示出地图中具体环境放大的局部效果。可以看出，本算法利用融合GNSS RTK定位信息和激光SLAM位姿估计信息方法可以有效消除地图累计误差；根据生成地图的分辨率可知，本算法建图精度可达0.1 m，满足实际自动驾驶场景需求。

图8 行驶环境点云特征地图Fig.8 Feature map of the points cloud of the driving enviroment

为了对本文提出的高精度定位系统性能进行评价，实验采用高精度组合导航系统的定位、定向结果作为激光匹配定位、定向性能评价的对比数据。选用的组合导航系统在RTK稳定解且定位定向状态下的位姿信息作为真值，其定位精度可达到厘米级，航向角的定向精度达到0.1°。图9示出该场景下激光雷达匹配定位与GNSS RTK定位估计结果对比，其中蓝色曲线表示位置误差，红色曲线表示车辆速度。分析实验结果可以看出，在加减速、转弯等动作发生较少的路段，激光雷达匹配定位与GNSS RTK定位均有误差，但定位误差均在0.15 m以内；而平直道路，定位误差始终控制在0.07 m以内。这表明在该场景下激光雷达匹配定位算法可代替GNSS/INS实现高精度定位。同时，为了验证本算法定位精度提升的有效性，将其与基于（normal distribution transformation，NDT）的激光SLAM建图定位算法的定位精度进行对比。由图9（d）可以看出，基于正态分布变换NDT的激光SLAM建图定位算法随着时间和距离的增加，因受累计误差影响其精度逐步降低；而本文提出的方法，定位精度一直稳定能够实现高精度的定位。

图9 以GNSS RTK为真值的激光雷达位置估计误差Fig.9 Lidar position estimation error as GNSS RTK is true value

图10为该场景下激光雷达角度估计与GNSS RTK角度值结果对比。可以看出，在加减速、转弯等工况较少的路段，激光雷达角度估计与GNSS RTK角度值有一些区别，其俯仰角误差在0.2°以内，横滚角在0.25°以内，差距最大的航向角误差也在0.35°以内；而在平直道路，所有角度误差始终控制在0.2°以内。由此表明，在弱GNSS信号场景下本算法可代替GNSS/INS实现高精度姿态估计。

图10 以GNSS RTK为真值的激光雷达角度估计误差Fig.10 LiDAR angle estimation error as GNSS RTK is true value

4 结语

本文提出了一种鲁棒、实时的GNSS RTK与激光雷达融合的预建图定位系统。该系统基于线面特征的激光里程计和预建图匹配方式，将不同工况下的GNSS RTK数据与激光匹配融合技术应用于城市复杂环境下，实现构建全局一致的高质量点云地图并达到厘米级定位精度。此外，在地图构建时，采用基于深度图格式的线面特征提取方法，有效降低了智轨电车在行道树以及人群聚集等环境下对角点、面点的误检率，提升了智轨电车定位系统在复杂、动态城市环境下的定位精度。通过实车在城市环境下对不同时段数据进行评估，结果显示该系统具有与现有的公开可用方法相当或更好的定位准确性。目前该系统没有提取环境语义信息功能，后续将结合视觉信息实现环境语义提取和匹配的能力，从而提升车辆在长隧道、雾霾天等极端退化场景中的位姿估计性能。