基于NDT 配准与轮式里程计的激光雷达运动畸变补偿算法①

陈强 陈海波 张沥化

(浙江工业大学信息工程学院 杭州310023)

同时定位与地图构建(simultaneous localization and mapping,SLAM)目前普遍运用于移动机器人领域,其被认为是实现避障、路径规划、导航等应用的核心环节[1-3]。激光雷达因能提供精准可靠的测距信息,且不易受光照环境影响,所以被广泛应用于SLAM 测距[4-5]。目前激光雷达一般是旋转结构,导致其会逐步扫描周围空间。因此,激光雷达在扫描期间的任何移动都会导致运动畸变现象[6],影响SLAM算法输出的机器人位姿精度与构建地图的质量。

激光雷达的扫描配准是激光里程计算法的基础。当激光雷达快速运动时,运动畸变的点云成为影响激光里程计精度的主要因素。研究者们通过对雷达的运动估计从而处理运动畸变现象。迭代最近点(iterated closest point,ICP)[7]用于估计点云之间的刚性变换,并未考虑扫描中的运动畸变现象。Hong 等人[8]在ICP 算法的基础上引入雷达速度的估计,实现对雷达运动畸变补偿。如文献[9]和[10]所示,恒速模型是解决运动畸变的方法之一。此外,Bosse 和Zlot[10]提出了一种基于控制点的连续状态表示,并在控制点之间进行位姿插值,从而恢复运动估计。Furgale 等人[11]引入时间基函数的线性组合对运动轨迹建立连续状态模型。与传统离散模型相比,该框架能提供更准确的估计。Anderson 和Barfoot[12]介绍了另一种连续时间轨迹状态估计方法,其关键思想是在离散最大后验估计上使用计算简便的高斯过程回归,实现查询时间点的状态。

以上算法处理运动畸变的效果取决于假设模型的准确性,而运动模型的假设并不能代表系统的真实运动,因此在实际应用中比较困难。不同于传统纯估计运动方案,当前基于传感器的外部运动估计也是处理运动畸变的重要发展方向。Kuramachi 等人[13]提出激光雷达与惯性传感器相结合的方法,添加三自由度惯性信息从而实现点云校正和位置预测,对旋转运动具有较好的鲁棒性。Geneva 等人[14]给出了一种基于新平面表示的方法,该算法将激光雷达的原始数据与惯性测量单元(inertial measurement unit,IMU)的运动信息融合,从而估计系统轨迹。Levinson 等人[15]引入轮式里程计辅助的方法,借助轮式里程计提供的位置信息获得激光雷达在扫描周期内的位姿,实现校正点云和雷达校准。Gentil等人[16]介绍了一种三维激光雷达和IMU 外部校准的算法,利用IMU 连续表示的预积分来描述系统在任何时间点的运动。该方法在不依赖任何显示运动模型的条件下补偿运动畸变。

综上所述,基于传感器辅助的解决方案取得了一定的研究成果。IMU 传感器具有较高的角速度测量精度,但线加速度精度以及二次积分在局部的精度较低,而轮式里程计具有较高的局部位姿测量精度。在轮式里程计辅助方法的基础上,本文开展进一步研究并提出基于正态分布变换(normal distributions transform,NDT)配准与轮式里程计的激光雷达运动畸变补偿算法。本文算法通过轮式里程计外部运动估计获得低准确度的雷达运动状态,并设计基于NDT 点云配准算法的轮式里程计误差处理方法,优化运动畸变补偿。实验结果表明,本文算法可以实现对激光雷达运动畸变的精准补偿,有效提升SLAM 系统的准确性。

1 运动畸变与符号定义

1.1 运动畸变描述

激光雷达是通过电机驱动测距核心进行旋转,从而完成对周围环境的扫描测距。受限于电机的旋转速率,激光雷达扫描频率是恒定的。因此一帧扫描内的各个激光束并不是同时完成测量,而是随着电机旋转,按照时间顺序依次测量。图1 是激光雷达运动畸变示意图。当装载激光雷达的机器人处于静止状态,雷达获取的数据并不会产生误差。但在SLAM 系统中机器人通常处于运动状态,且激光雷达运动速度一般高于扫描速度,若机器人在扫描期间内运动,而雷达仍默认在同一位置收集一帧数据,则扫描结果将产生畸变,这一现象称为激光雷达的运动畸变。激光雷达运动畸变会降低SLAM 系统的精确性。为减小运动畸变对SLAM 的影响,本文算法实现精准补偿运动畸变。

图1 激光雷达运动畸变

1.2 符号与定义

设X={xi}(i=1,…,n) 为一帧点云集合,xi表示第i个点的二维坐标向量,Xk表示第k帧点云,x(k,i)表示第k帧中的第i个点。记t为当前时间戳,则是第k帧的起始时间,由于激光雷达连续扫描的特性,故第k帧的结束时间与k +1 帧的起始时间是相等的。激光雷达坐标系{L} 是一个二维坐标系,其原点位于激光雷达的几何中心,第k帧雷达坐标系{Lk} 中的第i个点表示为。p=[p1p2p3]T=[x y θ]T表示位姿向量。世界坐标系{W} 是一个二维坐标系,在初始位置与{L} 重合,第k帧世界坐标系{Wk} 中轮式里程计ti k时刻的位姿表示为。

2 运动畸变补偿算法设计

2.1 轮式里程计运动估计

假定雷达运动的角速度和线速度随时间平滑且连续。随着旋转激光雷达在空间中移动,每个扫描点在不同运动位姿下收集。为对激光雷达运动畸变进行补偿,故为雷达收集的每个点匹配准确的运动位姿。轮式里程计数据输出频率较高,因此采用轮式里程计外部运动估计方式计算任一时刻的雷达运动位姿[17]。

世界坐标系为W,雷达坐标系L。第k帧起始时间与终止时间分别为和,轮式里程计在对应时间区间内的测量位姿为{pi},i=1、…、m,表示第k帧世界坐标系中轮式里程计位姿集合,p(k,i)是在区间内时刻的位姿。是第k帧对应的轮式里程计位姿。借助线性插值可得时刻的雷达估计位姿,如式(1)所示。

如果激光雷达在扫描周期内处于静止状态,则每个点的测量坐标系相同。由于雷达一般在移动中扫描,故Xk中每一点在不同的位姿下采集。运动中雷达位姿用一个3 ×3 的变换矩阵T表示,如式(2)所示。

其中,R为2 ×2 的旋转矩阵,t为1 ×2 的平移向量。

对不同帧间坐标系的点x(k,i)使用匹配的轮式里程计位姿进行帧间变换从而校正畸变的点云。如图2 帧间变换所示,激光雷达在时刻采集的点使用式(3)映射到雷达的起始帧中。

图2 帧间变换

轮式里程计高频测量的外部运动估计使运动畸变得到一定程度补偿。但轮式里程计存在漂移,无法准确反映真实运动的情况[18-19]。由于传感器的噪声是不可忽略的,故基于式(1)的轮式里程计辅助方法只能获得低精确度的雷达运动估计。为提高雷达运动估计的准确性,本文算法将基于轮式里程计漂移对当前雷达运动估计位姿进一步优化。

2.2 优化运动畸变补偿

基于点云配准的激光里程计具有高精度的特点。与ICP 激光点云配准算法相比,NDT 算法对初始值不敏感,且匹配结果更精确,但不需要消耗大量计算作为代价求解估计值[20]。轮式里程计的测量误差通常服从线性分布模型,因此针对轮式里程计漂移的影响,本文将轮式里程计外部运动估计与NDT 点云配准算法相结合。通过轮式里程计校正部分运动畸变,然后运用NDT 配准算法进行点云配准,将帧间匹配的结果作为精确值,从而处理轮式里程计的漂移。

NDT 将每次扫描中离散的二维点云转换成二维平面内定义的分段连续可微的概率密度,且概率密度由易于计算的正态分布组成,然后通过相邻扫描帧间匹配,获得最优运动估计[21]。首先,机器人周围的二维空间被规划地分为大小相同的单元。其次,收集在二维空间内第k帧点云Xk,找出包含扫描点的单元,分别计算该单元中样本x(k,i)的均值qk和协方差矩阵∑k。最后,建立该单元中扫描点样本概率密度p(x) 的正态分布模型N(qk,∑k):

其中,p为待估计的位姿向量参数。

由于优化问题一般被描述为最小化问题,故将函数s(p) 表示为-s(p)。Newton 法可以寻找令函数最优的参数p。采用Newton 法迭代求解方程,方程如式(6)所示。

其中,H和g分别是s(p) 的Hessian 矩阵和梯度向量。为简化公式,令梯度g中的元素gi为

等式(6)的解是增量Δp,在每次迭代中将Δp添加到当前估计位姿:

通过对当前帧估计位姿迭代优化,计算出精准位姿参数p。将p引入式(1),给出如式(10)形式。

其中,n表示第k帧中点x(k,i)的数量,p′表示轮式里程计位姿增量。

针对函数s(p) 的求解,初始值采用轮式里程计估计的当前帧位姿,利用Newton 法对位姿迭代优化,由式(6)～(9)迭代至收敛或满足最大迭代次数,计算比较精准的当前帧位姿p。与文献[15]方法不同的是,本文算法考虑了轮式里程计漂移对雷达运动估计的影响,为降低轮式里程计累积误差的影响,如式(10)所示,由NDT 配准算法计算的位姿p作为准确值,将其与轮式里程计位姿p′的差值作为漂移估计值。轮式里程计的测量误差服从线性分布,将误差均匀分布到每帧,减小轮式里程计漂移对雷达运动估计的干扰,优化运动畸变补偿效果,进而降低SLAM 系统轨迹累积误差并生成全局一致地图。

3 实验结果与分析

本文采用文献[22]提供的公开数据集,并进行真实场景实验对算法测试和评估。本文实验所用计算机配置为:中央处理器为Intel i5-7300HQ,主频2.5 GHz,内存8 GB,操作系统为Ubuntu 18.04。为证明本文算法的有效性,将其与文献[15]提出的轮式里程计辅助方法对比。由于数据集来自室内且不包含地面真值,所以将轨迹闭合和环境建图效果作为评估算法性能的指标。轨迹闭合指机器人运动起点和终点为同一位置,从而令运动轨迹形成闭合回路[8,23]。在真实场景实验中,将算法估计轨迹与轨迹真值对比,并分析轨迹误差和建图效果。

3.1 数据集实验

数据集1 中激光扫描数据源于频率为40 Hz、运动速度为0.27 m∙s-1的激光雷达,数据集2 中激光雷达频率为30 Hz、运动速度为0.75 m∙s-1,所有数据集均包含高频轮式里程计信息。数据集1 室内环境平面由2 条直线走廊、1 条小圆弧和1 个半径约为8.5 m 的半圆组成。数据集2 室内环境面积为41 m×34 m。

图3 和图4 是本文算法和文献[15]算法使用不同数据集生成的运动轨迹。图3中,对比算法轨迹起点与终点间运动估计偏差量为0.15 m,而本文算法的偏差量是0.05 m。图4 中,本文算法起点与终点间偏差量是0.35 m,而对比算法偏差量为0.50 m。通过图3 和图4 轨迹终点偏差量对比,可以看出本文方法优化了运动轨迹估计,主要表现在偏差量下降,轨迹闭合效果更优。文献[15]算法与本文算法的建图效果如图5 和6 所示,图中所标注的位置显示对比算法在构建地图中存在建图不一致现象。由图5 和6 可以看出,本文算法的建图质量优于文献[15]算法,产生较好地图构建效果。

图3 数据集1 轨迹

图4 数据集2 轨迹

图5 数据集1 地图

图6 数据集2 地图

与对比算法相比,本文算法利用NDT 配准算法处理轮式里程计漂移,减小了轮式里程计累积误差影响,优化了运动畸变补偿效果,降低了轨迹累积误差并生成全局一致地图。

3.2 真实场景实验

机器人平台如图7 所示,移动机器人采用差速驱动方式,底盘部分向上位机反馈轮式里程计信息,且搭载扫描频率为10 Hz的思岚A1 激光雷达。机器人利用机器人操作系统(robot operating system,ROS)的分布式通讯实现底盘控制和传感器数据获取。

图7 实验平台

构建地图场景如图8 所示,场景1 和场景2 的环境面积分别为9 m×11 m 和43 m×55 m。雷达扫描期间机器人的运动速度为0.45 m∙s-1。通过控制机器人移动路径直至返回起点,使其运动轨迹形成完整的闭合回路。

图8 实验场景

图9 和10 是文献[15]算法和本文算法在不同场景中生成的机器人运动轨迹。算法轨迹与真实轨迹的误差结果如表1 所示,误差度量方式采用绝对平移误差。由表1 误差结果分析可得,在场景1 和场景2 中,本文算法相对于对比算法的误差分别降低了0.15 m 和0.48 m。由此可得本文算法的运动轨迹估计效果优于文献[15]。图11 和12 分别是由文献[15]算法和本文算法生成的地图。从图中可以看出,与对比算法生成的地图相比,本文算法降低了轮式里程计漂移对雷达运动估计干扰,实现了精准补偿运动畸变,进而创建出全局一致性更高的地图。

表1 绝对平移误差对比

图9 场景1 轨迹

图10 场景2 轨迹

图11 场景1 地图

图12 场景2 地图

4 结论

本文提出了一种基于NDT 配准与轮式里程计的运动畸变补偿算法,旨在解决激光雷达扫描中存在的运动畸变现象。利用轮式里程计以低精确度高频测量的方式估计雷达运动,补偿部分运动畸变;设计一种基于NDT 配准算法的误差处理方法,减小了轮式里程计漂移的影响,提高了雷达运动估计精度,优化了运动畸变补偿效果。实验结果表明,本文算法可使轨迹取得低漂移的精确度水平,并生成全局一致地图,实现对SLAM 系统准确性的提升。