基于先验地图/GNSS/IMU融合的自动驾驶车辆定位系统

熊华川

（重庆交通大学，重庆 400074）

1 前言

精确的定位是自动驾驶车辆实现自主导航的前提[1]。仅依靠单一传感器的传统定位方式无法克服复杂的环境条件，如全球卫星导航系统（Global Navigation Satellite System，GNSS）在隧道、建筑遮挡条件下极易失效，不能满足定位的稳定性需求。针对复杂多变的环境，将多个传感器的数据进行融合实现定位是目前主流的定位方式。

其 中 ，GNSS/惯 性 测 量 单 元（Inertial Measurement Unit，IMU）融合定位是目前较流行的定位方案[2]。李庆成等[3]利用IMU 进行位姿、速度等状态预测，再利用扩展卡尔曼滤波（Extended Kalman Filter，EKF）算法融合GNSS 和车速传感器数据来修正预测状态值，从而为矿用车提供精准的定位数据。Musavi 等[4]同样通过融合GNSS 和IMU数据实现精确定位，不同的是采用的融合算法为自适应神经观测器。李鹏等[5]在融合GNSS 和IMU 数据的基础上，提出丢弃恶化的观测量策略，以降低观测量精度较差时系统所受影响。因此，GNSS 与IMU 融合的定位算法均利用IMU 通过航位推算技术进行位姿估计，IMU 长时间积分会产生累积误差[6]，需通过GNSS 修正预测值。但当GNSS 失效或者受环境影响变得低效时，反而会降低定位系统的精度。

目前，利用激光雷达点云与先验地图匹配定位的方法同样是研究热点。可以通过实时定位与地图构建（Simultaneous Localization and Mapping,SLAM）方法来构建先验地图，再利用激光雷达扫描得到的点云与先验地图进行匹配获取位姿。基于迭代最近点匹配（Iterative Closest Point，ICP）算法是其中最常见的方法[7-8]，但该方法适用于少量点云的匹配运算。Droeschel 等[9]提出通过构建栅格局部地图与点云进行匹配，并通过分层优化的方式优化匹配结果。以上方法都是在点云特征的基础上进行计算优化，当出现大规模点云，以及两帧之间数据变化剧烈等情况时，点云配准均出现耗时长、准确率低的现象。因此，仅依靠激光雷达的地图匹配方法不能满足定位系统的要求。

针对以上问题，本文提出利用误差卡尔曼滤波（Error State Kalman Filter，ESKF）算法融合GNSS、IMU 和先验地图点云数据，再通过引入矩阵校正方法，避免GNSS 失效或先验地图匹配延迟等情况下系统无法获取有效观测值以修正位姿的问题，使系统保持稳定和高精度状态。

2 多传感器融合算法

本文算法的整体框架如图1所示。本文采用的数据融合算法为ESKF 算法[10]，相较于EKF 算法，其具有以下优势[11]：ESKF 主要针对误差状态量进行处理，误差状态量通常较小，因此计算量小，且总是在原点附近，可避免奇异值，保证线性化的合理性和有效性；在旋转处理方面，ESKF 可以用三维变量来表示旋转增量。

图1 多传感器融合定位系统框架

本文的整体框架分为3个部分：

a. 数据预处理。接收到IMU 原始数据和激光雷达数据后，根据IMU 推算出车辆在激光雷达帧内的位姿变化，进而完成点云去畸变。

b.激光与点云地图匹配。通过SLAM 算法构建先验地图，并将GNSS 信息作为先验地图的初始坐标，完成对多传感器输出位姿的坐标系统一，再利用正态分布变换（Normal-Distributions Transform，NDT）算法将每一帧去畸变的点云与先验地图进行匹配输出位姿。

c.滤波。利用ESKF 算法融合IMU 推算出的状态值、激光雷达与先验地图匹配输出的位姿以及GNSS 输出的位姿。通过观测信息修正IMU 推算出的状态值。

2.1 初始化

本文所使用的IMU 由陀螺仪和加速度计组成。根据二者特性可以对IMU 测量的加速度和角速度进行数学建模：

式中，am、wm分别为IMU 加速度、角速度的测量值；at、wt分别为IMU 加速度、角速度的真实值；ba、bw分别为加速度零偏和角速度零偏；ηa、ηw分别为加速度、角速度测量噪声；CNI为IMU 坐标系到惯性坐标系的变换矩阵；g为重力加速度。

在初始化阶段，对IMU 的加速度零偏和角速度零偏进行估计。具体而言，当车辆静止时，除加速度计的Z向加速度与重力加速度相等外，IMU 的数值均应为零，因此车辆静止时IMU 的加速度和角速度测量结果即可认为是IMU 的零偏值[12]。随后，使用激光雷达匹配先验地图输出的位姿作为基准，对初始位姿进行估计。这种方法使得系统能够在任何地点进行初始化，无需考虑GNSS的状态。

2.2 运动模型

ESKF 将系统状态分为不考虑噪声的名义状态和误差状态。二者相加即为真值状态：

式中，Xt为系统真值状态；Xn为系统名义状态；δX为系统误差状态；p、δp分别为名义位置状态量、误差位置状态量；v、δv分别为名义速度状态量、误差速度状态量；q、δθ分别为名义姿态状态量、角度误差状态量；δg为误差重力加速度状态量；δbw、δba分别为误差角速度零偏状态量、误差加速度零偏状态量。

根据IMU 航位推算算法，可以得到名义状态的离散形式：

式中，pk、vk、qk、ba(k)、bw(k)、gk分别为k时刻名义位置状态量、名义速度状态量、名义姿态状态量、加速度零偏状态量、角速度零偏状态量、重力加速度状态量。

根据名义状态与误差状态的关系可以推算出误差状态的离散形式：

式中，δpk、δvk、δθk、δba(k)、δbw(k)、δgk分别为k时刻误差位置状态量、误差速度状态量、角度误差状态量、误差加速度零偏状态量、误差角速度零偏状态量、误差重力加速度状态量；ηv、ηθ、ηba、ηbw为随机噪声；Δt为IMU采样时间。

根据系统误差状态方程，可以得到ESKF 预测方程和预测的协方差矩阵：

2.3 观测模型

本文的观测数据分别由GNSS 和激光雷达与先验地图提供。二者的观测模型构建为：

式中，YGNSS、YLiDAR为GNSS 观测量、雷达匹配观测量；h()、f()分别为GNSS 观测方程、雷达匹配观测方程；V、R分别为GNSS 观测量的协方差矩阵、雷达匹配观测量的协方差矩阵。

根据二者的输出为位姿，可以将观测模型统一写作：

式中，Y为观测量；F为观测方程系数矩阵；Pobs为观测量的协方差矩阵；H为观测方程对误差状态的雅可比矩阵。

根据ESKF 的修正公式，可以分别得到对误差状态的修正值和协方差矩阵：

式中，K为卡尔曼增益；Xcertify(k+1)为(k+1)时刻修正后的状态量；Xn(k+1)为(k+1)时刻名义状态量；⊕表示广义相加。

随后，通过将修正后的误差量与名义状态量进行广义相加即可得到最终的系统真值状态量。这一过程能够有效减小预测值的误差，提高系统的精度。

2.4 校正矩阵

GNSS 信号在高楼、隧道等环境下极易受到干扰，导致定位精度下降甚至失效，因此不能作为观测值使用。激光雷达虽然可以克服环境的影响，但其匹配过程耗时较长，无法满足车辆对实时定位的需求。另一方面，IMU 的采样频率远高于GNSS 和激光雷达的采样频率，不同频率下的观测值同样不能实时更新，如图2所示。针对以上问题，本文提出一种矩阵校正方法，以在GNSS 失效或激光雷达匹配延迟的情况下近似估计系统的状态，避免系统因累积误差快速发散。具体地，利用修正位姿与预测位姿之间的相对位姿作为校正矩阵，当下一帧没有合适的观测值时，则利用校正矩阵修正系统位姿，以弥补GNSS 和激光雷达的局限性，提高系统的实时性和定位精度：

图2 传感器输出对比

式中，ΔC为校正矩阵；Ccertify、Cpredict分别为位姿的修正矩阵和预测位姿。

3 试验验证

3.1 试验设备

本文试验由基于吉利博瑞改装的智能车实现，如图3所示。智能车搭载的试验设备有：车顶前、后放置的2根GNSS蘑菇头天线，车顶中央处3颗16线激光雷达，后备箱处的GNSS信号接收机和IMU设备。

图3 试验用智能车

3.2 试验数据

为验证本文算法的有效性，首先通过试验车在校园内进行GNSS、IMU 和激光雷达数据采集，采集路线如图4所示，分别通过空旷路段、遮挡和隧道路段，来验证当GNSS 失效或低效状态下系统状态的稳定性。

图4 数据采集路线

根据采集的激光雷达数据，通过SLAM 算法构建定位系统中的先验地图，如图5所示。

图5 先验地图

3.3 试验结果分析

为验证定位系统的有效性，根据采集的数据分别进行IMU航位推算、GNSS/IMU组合定位和先验地图/GNSS/IMU组合定位的算法验证以及结果对比。

如图6 所示为IMU 进行航位推算的结果，可见IMU 在积分时速度随着误差累积而发散，导致最终的轨迹和以四元数[qw,qx,qy,qz]T表示的位姿均随之发散，由式（5）可知，发散的速度成指数级增长，因此需要其他数据来抑制IMU的误差。

图6 IMU航位推算轨迹、速度和位姿

分别利用GNSS/IMU组合定位算法和先验地图/GNSS/IMU 组合定位算法生成定位轨迹，如图7 所示。其中包括树林遮挡、隧道等影响GNSS 状态的路段，如图8 所示。图9 所示为整条轨迹中GNSS 状态变化曲线，其中GNSS状态定义如表1所示。由图9 可以看出，经过特殊路段时，GNSS 的状态都不为4，根据表1，GNSS 其处于精度较差甚至失效状态。从图7中可知，在空旷路段，两类算法输出的轨迹几乎重合，且平滑性好。差别体现在GNSS 低效处，即图7中特殊路段处，GNSS/IMU 组合算法明显较先验地图/GNSS/IMU 组合算法具有更大的误差偏移。并且值得注意的是，当GNSS 失效后，轨迹并没有立即如图6一样发散，而是随着GNSS失效后的时间推移而逐渐发散，这是校正矩阵作用的结果，校正矩阵可有效延缓系统状态的发散。

表1 GNSS状态定义

图7 轨迹对比

图8 特殊路段场景

图9 GNSS状态

4 结束语

本文通过ESKF 算法融合GNSS/IMU/先验地图构建自动驾驶车辆实时定位系统，并引入校正矩阵提高了系统的鲁棒性。试验结果表明，在GNSS 低效甚至失效的情况下，先验地图/GNSS/IMU 组合算法能够很好地抑制系统因误差累积而发散，从而为自动驾驶车辆提供稳定、精准的定位结果。

但是，本文提出的方法依赖于高精地图，高精地图的制作和维护将限制其应用。下一步的改进方向应是从高精点云地图中提取道路交通信息，以构建矢量地图。矢量地图的维护成本远小于高精点云地图的维护成本，且道路交通信息不会轻易改变。