1、DOI:10 3969/j issn 2095 509X 2023 06 021面向城区自动驾驶的激光惯性里程计李伟1，叶鸣2，卢仲康2(1 华南理工大学机械与汽车工程学院，广东 广州510641)(2 广州华工机动车检测技术有限公司，广东 广州510640)摘要:针对城区自动驾驶车辆定位问题，提出一种基于 LiDA 与 IMU 融合的激光惯性里程计。LiDA 通过帧间点云配准进行定位，但对非线性运动的鲁棒性不足;IMU 通过对加速度和角速度积分进行定位，但长距离定位误差大。基于此，根据无迹卡尔曼滤波原理对积分位姿和配准位姿进行融合。将积分位姿作为 LiDA 配准的初始位姿;同时利用配准位姿修
2、正积分位姿，从而实现长距离持续定位。在某城市道路对所提方法进行实车实验，结果表明所提方法有效提高了里程计的整体定位性能。关键词:激光雷达;惯性测量单元;无迹卡尔曼滤波;车辆定位中图分类号:TP242;TN958 98文献标识码:A文章编号:2095 509X(2023)06 0109 04车辆的自主定位是汽车自动驾驶的关键技术之一，定位信息能够为规划和控制模块提供必需的输入信息1。因此，如何对车辆进行长距离持续定位，成为了自动驾驶领域的研究热点。然而在城市道路场景中，激光里程计受自动驾驶车辆非线性运动的影响，存在鲁棒性不足和长距离持续定位性能差的问题2。激光里程计的定位原理:以激光雷达在不同时
3、刻获得的观测点云进行配准得到两帧之间车辆的位姿变换矩阵，从而对车辆的位姿进行更新，通过对历史变换的不断累加，从而实现车辆定位3。然而，车辆进行非线性运动时，帧间位姿的差异较大，且初始位姿较差，因此配准算法难以收敛到最优解，将导致精度降低。由于使用单激光雷达进行定位，无法在场景多变的城区自动驾驶场景下稳定输出定位结果，因此需要研究基于传感器融合的定位方法。惯性测量单元是测量载体加速度和角速度的传感器，通过对其输出的测量值进行积分可实现位姿推算，且短期精度较高，但受积分误差的影响，长期定位容易发生定位失效问题 4。贾晓辉等 5 基于松耦合框架，利用 LiDA 与惯性测量单元(inertialmea
4、suring unit，IMU)进行融合定位，实验结果表明基于卡尔曼滤波原理的激光惯性里程计能够有效减少定位误差，从而证明了松耦合框架的有效性，但研究仅在室内场景进行实验，无法在室外应用。针对里程计长距离定位产生漂移、受非线性运动影响而精度降低的问题，本文提出一种基于 Li-DA 与 IMU 融合的激光惯性里程计。根据城市道路场景的特点，将积分位姿作为 LiDA 配准的初始位姿;同时利用配准位姿修正积分位姿，从而实现长距离持续定位。1激光里程计激光里程计是一种利用激光雷达点云所携带的三维信息进行定位的方法。通常情况下，分为点云预处理和点云配准两个部分6。其中，预处理是为了降低计算复杂度、减少点
5、云的随机误差，配准是为了得到帧间位姿变换从而实现定位。1 1点云预处理多线激光雷达的数据量过大，对算力的要求较高，且一般含有随机噪声，因此需要对原始点云进行预处理，包括离群点剔除和体素下采样，从而在保证算法效果的前提下提高数据处理的效率。首先，进行体素下采样，分为两步:1)栅格化输入点云;2)计算每个栅格里所有点的重心坐标，将体素内的所有点用该重心点表示。在第二步中，使用立方栅格的重心而不是其几何中心，从而在下采样的同时，保持原始点云分布的一致性。其次，对下采样后的点云进行离群点剔除。多线激光雷达的输出属于稠密点云，因此离群点的主要特点是数量少不成簇和出现位置随机，使用半径收稿日期:2021
6、10 08作者简介:李伟(1996)，男，硕士研究生，主要研究方向为自动驾驶，743159544 qq com9012023 年 6 月机械设计与制造工程Jun 2023第 52 卷 第 6 期Machine Design and Manufacturing EngineeringVol 52 No 6滤波器进行离群点剔除:以点云任意某点为中心，统计位于其半径内的点数，保留点数大于阈值的点、否则剔除。经过上述预处理之后，点云分布与处理前保持一致，能够避免点云稀疏化对后续配准过程的影响。1 2点云配准估计位姿正态分布变换匹配通过求解最优化问题以确定两簇点云间的最优匹配，从而实现定位7。为了避免直
7、接搜索邻近点对，首先根据栅格里局部点云簇的概率密度函数描述点云表面的位置、方向和平滑度，再根据特征值大小的不同组合，将点云分为线型、平面型、球型。具体的配准步骤如下:1)设定分辨率 S 划分当前帧点云，得到三维空间中若干个边长相等的栅格。2)遍历所有栅格，分别计算栅格中点云集合的坐标均值 和协方差:=1mmi=1xi=1mmi=1(xi)(xi)T(1)式中:m为栅格内的点数，xi为点的三维坐标向量，i 为栅格中点的序号。3)根据初始变换 T 进行配准初始化，一般设置成单位矩阵。设经过初始变换后得到目标点为xi=f(xi，T)。4)变换源点云，计算栅格重叠率 P(xi):P(xi)=exp(x
8、i)T(xi)2(2)5)对每个栅格的重叠率求和并作为配准得分，设为目标函数。6)采用优化算法迭代求解上述目标函数，输出使配准得分最高的变换矩阵。在车辆行驶过程中，将当前帧点云作为源点云、上一帧点云作为目标点云进行配准，以输出里程计的估计位姿。2激光惯性里程计2 1位姿异步融合过程位姿异步融合的主流实现方式为滤波器算法。原始卡尔曼滤波器假设系统状态、观测量、预测量均为线性函数，且系统在下一时刻的状态可以由上一时刻的历史状态经过某种线性变换得到。然而，自动驾驶车辆的运动是非线性的，无法满足上述假设。为了解决该问题，扩展卡尔曼滤波器(extendKalman filter，EKF)利用泰勒展开引入
9、线性化假设，然而其线性化误差随系统的非线性度和不确定度递增。无迹卡尔曼滤波器(unscented Kalman filter，UKF)是一种松耦合融合框架，其不需要计算雅可比矩阵，且线性化误差更小。利用 UKF 进行数据融合的基本步骤为:1)采样(2n+1)个满足高斯分布的随机点，分布的均值、协方差 由人工先验设定:0=i=+(n+)ii=1，ni=(n+)ini=n+1，2n(3)式中:0 为第 1 个采样点的坐标，i 为第 i 个采样点的坐标，n 为状态向量维度，为参数，i 为采样点的序号。2)对随机点的权重进行初始化:Wm0=n+Wc0=n+1 2+Wmi=Wci=12(n+)i=1，2
10、n(4)式中:Wm0为第 1 个点在计算均值时的权重;Wc0为第一个点在计算方差时的权重;和 为高斯分布的拟合参数，对于正态分布而言，104，1且=2;Wmi为均值项的权重;Wci为方差项的权重。3)根据预测方程对点集进行状态变换，得到每个点的预测状态:i=g(ut，t1i)(5)式中:i为经过预测方程处理后的采样点坐标，g()为预测方程，ui为控制向量，t 1i为上一时刻的有效最优估计状态向量，t 为当前时刻。4)更新下一时刻状态向量和协方差矩阵，以及观测向量和观测协方差矩阵。5)当观测状态更新时，根据观测方程计算平差权重，即卡尔曼增益参数 Kt:x，zt=2ni=0Wci(i t)(Zi
11、zt)TSt=2ni=0Wci(Zi zt)(Zi zt)T+QtKt=x，ztS1t(6)式中:x，zt为协方差矩阵，t为经过预测方程处理后的采样点均值向量，Zi为第 i 个采样点的观测方0112023 年第 52 卷机械设计与制造工程程结果，zt为系统加权观测向量，S1t为矩阵求逆运算，Qt为系统噪声协方差矩阵。6)利用实际观测量 zt修正预测值，输出最优估计:t=t+Kt(zt zt)t=t+KtStKTt(7)基于此，分别将 IMU 的运动预测模型和里程计位姿作为 UKF 的预测方程和观测方程，就可以实现位姿的异步融合:首先，在各传感器时间同步的条件下，将同一帧点云变换到第一束激光的发
12、射坐标系下进行点云畸变校正，利用校正点云和惯性预测位姿对前端里程计进行初始化，以提高位姿跟踪精度;其次，将激光里程计的配准位姿作为观测量消除预测位姿的积分误差。位姿异步融合由预测阶段和更新阶段组成，如图 1 所示。图 1位姿异步融合过程预测阶段为基于惯性测量单元的航迹推算。通过推导惯性测量单元的位姿转移方程，对加速度和角速度测量值进行时间积分;使用预测位姿对激光雷达点云进行非线性运动畸变校正，同时输出定位结果。更新阶段为基于里程计配准位姿的积分误差校正。首先，为了提高正态分布变换(normal distri-butions transform，NDT)前端里程计的配准精度，将前述预测位姿作为其
13、初始位姿，以避免车辆旋转运动的影响;其次，将配准位姿输入无迹卡尔曼滤波器，以更新融合状态。2 2基于 IMU 预测位姿的里程计初始化惯性测量单元输出载体坐标系下的加速度值和角速度值，且系统噪声分为满足高斯分布的零偏误差和恒为常数的静态漂移误差，其状态向量可写作:Tt=pt，qt，vt，b(8)式中:Tt为 t 时刻的状态向量，pt为 t 时刻载体坐标系原点在导航坐标系下的坐标，q 为 t 时刻姿态的四元数描述，vt为 t 时刻载体坐标系下的速度，b为 IMU 的系统漂移误差。因此，基于 IMU 测量值的车辆运动预测方程为:pt+1=pt+vtqt+1=qt I，(wt bw)TTvt+1=vt
14、+(qt(at ba)g)t(9)式中:pt为 t 时刻的位姿，v 为速度向量，t 为时间间隔，qt为 t 时刻的姿态四元数，I 为单位矩阵，wt为 t 时刻的角速度向量，vt为 t 时刻的速度向量，at为 t 时刻的加速度向量，ba为 IMU 的加速度测量静态误差，bw为 IMU 的角速度测量静态误差，g 为重力加速度。将式(9)代入式(5)得到 UKF 的状态转移方程，随后将上述预测模型输出的位姿信息作为激光里程计的初始位姿，用于 UKF 的预测更新过程。2 3基于 LiDA 配准位姿的积分误差校正激光里程计的配准位姿记为 p，q，其中:p 为载体坐标系原点在导航坐标系下的坐标，q 为姿态
15、的四元数描述。因此，将车辆位姿变换的平移、旋转向量分别作为观测向量可得:pt+1=pt+Ntqt+1=qt+Nt(10)式中:N 为不同模块的频率倍数关系。将式(10)代入式(7)中得到 UKF 的观测校正变换方程。3实验结果与分析对所提方法进行实车验证，如图 2 所示，采用安装了 32 线激光雷达和组合导航设备的测试车辆，在城市道路进行定位实验。图 2数采平台实验在搭载 CPU 为 Intel Xeno E5 2620v32 4 GHz，GPU 为 NVIDIA GTX TitanXP，内存大小为 64 GB，装有 Ubuntu 16 04 和 OS Kinetic 的服务器上进行。1112
16、023 年第 6 期李伟:面向城区自动驾驶的激光惯性里程计为了验证融合框架的有效性，选取 NDT 前端里程计和融合后的定位精度进行对比。测试路段的总长度为 1 524 m，以组合导航输出的定位结果作为真值。所提算法的定位轨迹如图 3 所示。其中，分别在大曲率弯道(A点)和轨迹末端(B 点)将融合前轨迹与融合后轨迹进行对比，融合后的定位轨迹与真值轨迹的重叠度更高、偏移程度更小。图 3实车定位轨迹可以看出，融合前后的定位轨迹均在弯道处出现了不同程度的偏移。但是，引入融合框架后，定位轨迹与真实轨迹的偏离显著减少，且在直道上的定位稳定性有所提高。定位轨迹的累积误差曲线如图 4 所示，融合后终点处的最大累积误差由11 192 m 减小至 2 661 m。图 4定位轨迹的累积误差曲线融合前后的定位误差见表 1。实验结果表明，引入 IMU 的激光惯性里程计避免了位姿跟踪误差的放大，而纯激光里程计在类似工况下无法持续提供准确的定位结果。表 1定位误差对比单位:m方法平均值最大值标准差融合前6608111923523融合后1912266107474结束语本文对城区自动驾驶的定位方法进行了研究，提出了基于
