基于IMU与激光雷达紧耦合的混合匹配算法①

周志全, 刘 飞, 屈婧婧,2, 匡 兵, 景 晖, 邹钰杰

1(桂林电子科技大学 机电工程学院, 桂林 541004)

2(桂林航天工业学院 科技处, 桂林 541004)

随着人工智能的发展, 激光雷达以其分辨率高、测距远、不受光照影响和抗干扰能力强等优点获得了广泛的关注.激光雷达可帮助AGV (自动导引运输车,Automated Guided Vehicle)小车在未知环境中实时定位与建立地图信息, 为后续轨迹跟踪、路径规划等提供良好基础.激光雷达即时定位与地图构建(Simultaneous Localization And Mapping, SLAM)可分为前端里程计、后端优化、闭环检测和地图构建4个模块, 其中最重要的为前端里程计模块[1].当前, 前端里程计主要有直接匹配方案、基于特征的匹配方案和多传感器融合方案[2].

直接匹配方案主要有ICP算法(迭代最近点, Iterative Closest Point)[3]和正态分布变换算法(Normal Distributions Transform, NDT), ICP算法是一种基于对应点的匹配算法, 定位精度高, 但依赖初值, 实时性较低;NDT算法是一种基于概率模型的匹配算法, 实时性较好, 但定位精度低于ICP算法.基于特征的匹配方案主要有提取曲率[4]、角点[5]和法失[6]等特征的ICP变种算法和LeGO-LOAM (激光雷达测距与地图构建算法,Lightweight and Ground-Optimized Lidar Odometry and Mapping)算法[7,8].其中, ICP变种算法将定位精度进一步提高, 但实时性依旧较低; LeGO-LOAM算法根据线、面特征进行匹配, 采用高频率定位, 低频率建图的方案, 定位精度和实时性均较好, 但在结构信息较少的环境中, 累积的误差使得地图变得模糊[9].多传感器融合方案有激光雷达与实时动态载波相位差分定位技术(Real-Time Kinematic, RTK)[10]、激光雷达与IMU等融合算法.其中, 激光雷达与RTK融合算法定位精度高,RTK定位无累计误差, 但RTK在室内或有严重遮挡环境下无法工作, 鲁棒性较差; 激光雷达与IMU融合算法有松耦合[11]和紧耦合[12]两种算法: 基于卡尔曼融合的松耦合算法扩展性强, 可接入其他传感器同时工作,但当长时间运行后IMU漂移过于严重, 从而导致定位精度降低; 紧耦合算法通过建立一个统一的误差函数,同时优化激光雷达和IMU的位姿, 定位精度高, 实时性较好.

AGV小车需要在室内仓库结构化信息丰富的场景中行驶, 同时需要将货物搬运至其他仓库过程中经过结构化信息较少的室外场景, 此时标准的LeGOLOAM算法能够提取的线、面特征较少, 定位精度下降.综上, 为实现AGV小车在室内室外实时高精度建图与定位, 本文采用IMU与激光雷达紧耦合的混合匹配算法, 当环境中结构信息较多时, 激光里程计采用LeGO-LOAM算法, 而当环境中结构化信息较少时采用ICP算法.

1 LeGO-LOAM算法特征提取

为建立统一坐标系, 本文将世界坐标系记为W, 激光雷达坐标系记为L, 假设惯性测量单元IMU与AGV小车重心重合, 可将AGV小车与IMU坐标系统一记为I.

1.1 三维点云线束分割

根据研究需求, 需将激光雷达无序点云转化为有序点云, 将获取的激光雷达原始点云坐标信息记为(x,y,z), 扫描点到激光雷达原点距离记为l, 则有:

将一帧无序的三维点云按垂直角度α和水平角度β大小排序, 使其划分为二维有序点云, 为下一步提取特征做准备.

1.2 基于余弦值提取特征

LeGO-LAOM算法是通过曲率值来表达扫描点的平滑程度, 进而提取一帧点云中特定数量的角点和平面点.由于曲率值没有上下限, 无法通过设定阈值对每一帧中点云特征点的数量进行定量统计, 即无法获知环境结构化信息的丰富程度.因此, 本文选取扫描点的夹角余弦值来提取角点、平面点的特征同时对两者数量进行分别统计.扫描点夹角的余弦值的变化区间为[-1,1], 而在实际环境中平面上扫描点夹角的余弦值理论上为-1, 角点夹角的余弦值理论上为0, 这为寻找特征点提供了量化的参考值, 同时可设定余弦值区间提取并统计所需要的特征信息.

每一线束上第i扫描点与第i-1和i+1扫描点组成夹角余弦值:

其中,d(i-1)i、di(i+1)和d(i-1)(i+1)分别表示第i-1第i扫描点的距离、第i第i+1扫描点的距离和第i-1第i+1扫描点的距离.

在实际环境中平面上扫描点夹角的余弦值理论上为-1, 由于噪音的存在, 设定平面点余弦值区间为[-1,-0.984], 即夹角在 [170°, 190°]内.同理, 设定角点余弦值区间为 [-0.174, 0.174], 即夹角在 [80°, 100°]内, 并且统计角点和平面点余弦值区间内的特征点个数.为了保证所选取特征的均匀性, 将每一帧点云在水平平面上平分为6等份, 在这6个子图中等量选取以下4类特征点: (1)余弦值最接近-1的, 每个子图选取4个共计24个扫描点为平面点; (2)选取余弦值较接近-1的, 每个子图选取4个共计24个扫描点为待选平面点; (3)余弦值最接近0的, 每个子图选取2个共计12个扫描点为角点; (4)余弦值较接近0的, 每个子图选取2个扫描点, 共计12个扫描点为待选角点.

1.3 特征关联与误差函数构建

在第k+1帧扫描点云pk+1中, 选取角点和平面点组成角点集和平面点集.将第k+1帧点云根据IMU提供的位姿作为初值投影到第k帧激光雷达坐标系下.对于k+1帧中某一角点i, 首先在第k帧点云中寻找欧式距离最近的角点j, 然后在第k帧中寻找与j角点不同线束上欧式距离最近的角点l, 将jl连成直线,将i,j,l的坐标记为根据文献[13],角点i到直线的距离即误差计算公式:

同理, 对于k+1帧中的某一平面点i, 首先在第k帧中寻找距离平面点i最近的平面点j, 其次在第k帧中寻找距离平面点j最近的同一线束上的平面点l和不同线束上的平面点m, 平面点jlm组成一个平面,将i,j,l,m的坐标记为:点i到平面的距离即为误差记为:

LeGO-LOAM算法的误差函数为:

2 IMU状态估计与误差函数

IMU是利用加速度计、陀螺仪测量出线加速度、角加速度, 再通过积分推算出AGV小车的航向、速度、位姿等导航参数的自主式导航系统.根据文献[14]AGV小车的状态C可以表示为:

其中,R为3行3列的旋转矩阵,P为1行3列的位置矩阵,v为速度, IMU的偏置b可以写成:

其中,bg、ba分别表示陀螺仪和加速度计的偏置.

根据IMU原始数据角速度w和加速度a, 测量值和受白斯噪声η和偏置b的影响有:

根据IMU的测量值, 推导出无人车的旋转、位置和速度等运动信息, 在时间t内按式(13)-式(15)进行:在式(13)、式(14)和式(15)中, 假设在时间t+Δt内角速度和加速度保持不变.

然后根据IMU预积分方法从而得到从时刻i到时刻j相对运动的旋转角度ΔRij, 位置ΔPij和速度Δvij的偏差.

由于IMU的漂移主要在旋转角度和空间位置上比较明显, 因此本文将IMU的误差函数EB构建为:

构建IMU的误差函数是为了让IMU与激光雷达的总误差最小化, 进一步约束激光雷达点云匹配, 进而求解出AGV小车的位姿信息.

3 混合匹配算法

3.1 ICP匹配算法

标准ICP算法的原理是: 在源点云和目标点云间,按照欧式距离最小为对应点的规则, 找到对应点集, 然后迭代计算出最优匹配参数, 即旋转矩阵R和平移矩阵T, 使得误差函数EL2最小或者达到设定迭代次数.

其中,Pi、Qi、R、T和n分别表示源点云、目标点云、旋转矩阵、平移矩阵和源点云中的点云个数.

ICP算法流程如算法1所示.

算法1.ICP算法1) 找寻对应点集: 在目标点云数据中找到源点云的对应点集, 对应点间的欧式距离作为误差, 构建误差函数;2) 计算变换矩阵: 计算旋转矩阵R和平移矩阵T;3) 更新目标点云: 使用步骤2中参数R、T对目标点云进行旋转和平移得到新的目标点云;4) 判断迭代停止: 如果误差函数小于设定阈值或者迭代次数达到设定上限, 则停止迭代计算并输出最终转换矩阵参数R、T, 否则返回步骤2.

3.2 混合匹配算法

在结构化信息较少时, 基于线、面特征的LeGOLOAM算法定位精度降低, 而基于点到点的ICP算法仍可以准确、可靠的工作; 在结构化信息较多时, LeGOLOAM算法定位精度高于ICP算法.同时, 点到线、面的LeGO-LOAM算法收敛速度为二阶收敛, 比点到点的ICP算法一阶收敛速度快, 实时性更好.由此可知,LeGO-LOAM算法和ICP算法具有互补性, 因此提出基于IMU与激光雷达紧耦合的混合匹配算法来进一步提高AGV小车定位精度和系统鲁棒性.

将LeGO-LOAM算法中的曲率提取特征点的方式改进为以余弦值提取特征点的方式, 可准确判别出环境中结构化信息的丰富程度.系统运行框架如图1所示, 当环境中平面特征点数量大于或等于48个时, 激光里程计采用LeGO-LOAM算法, 当环境中平面特征点数量小于48个时, 激光里程计采用ICP算法.将IMU位姿信息作为混合匹配算法的初始值, 构建IMU与ICP算法的联合误差函数, 实现位姿共同优化.此时系统的误差函数为:

图1 系统运行框架图

AGV小车的位姿可表示为:

其中,I表示AGV小车的坐标系,tx、ty、tz表示激光雷达第k帧时刻AGV小车的x、y、z轴方向位移, θz、θx、 θy表示AGV小车绕z、x、y轴方向的旋转角度,pk、qk表示激光雷达第k帧时刻AGV小车的位置信息和角度信息.构建系统误差函数就需要对AGV小车的位姿进行求解, 使得误差函数最小.迭代的位姿初始值为IMU状态估计的位姿, 这样能够有效减少迭代次数, 提高运行效率, 同时消除了零初值或者匀速运动假设初值带来的错误特征匹配, 更加符合真实的运功.此外在实时输出位姿的同时, 将输出位姿对IMU的位姿解算进行实时修正, 可降低IMU零漂、温漂等误差影响, 使得位姿解算更加准确.

4 实验分析

本实验使用的激光雷达为16线束, 水平角度分辨率为0.2°, 垂直角度分辨率为2°, 更新频率为10 Hz.IMU的角度偏移量2°/h, 加速度偏移量0.003 mg, 更新频率为300 Hz.RTK的水平定位精度10 mm, 垂直定位精度20 mm, 更新频率100 Hz.由于室内RTK无法工作, 故选用结构化信息较多的室外停车场作为实验场景一(图2中卫星图教学楼内环), 教学大楼外环道路上结构化信息较少, 结合内部停车场可作为实验场景二(图2中卫星图教学楼内环加外环).

图2 卫星图

4.1 定位精度分析

为了验证3种算法的有效性, 所有算法不做闭环检测, 利用RTK测得的位姿作为参考值, 3种算法估计位姿为测量值.运用LeGO-LOAM算法、IMU与LeGO-LOAM紧耦合算法(简称IMU/LeGO-LOAM)和基于IMU与激光雷达紧耦合的混合匹配算法(简称混合匹配算法)分别对场景一和场景二进行5次试验,每隔10米计算一次相对位姿误差PRE.以PRE平均值作为水平定位精度标准, 统计5次实验结果取平均值绘制成表1.

表1 相对位姿误差PRE对比 (m)

从表1中可以看出, 在结构信息较为丰富的场景一中, 混合匹配算法以IMU/LeGO-LOAM算法模式运行, 混合匹配算法相比LeGO-LOAM算法定位精度提高了18.5%.图3为混合匹配算法建立的场景一三维点云地图.对比图1的卫星图可知, 图3中无重影较好的建立了三维点云地图.在场景二中, 混合匹配算法相比LeGO-LOAM算法和IMU/LeGO-LOAM算法定位精度分别提高了33.0%和10.9%.由此可以得出混合扫描匹配算法定位精度均高于IMU/LeGO-LOAM算法和LeGO-LOAM算法.图4为3种算法在场景二中RPE与AGV小车行驶里程之间的变化曲线, 其中LeGO-LOAM算法用黑色正方形表示, IMU/LeGOLOAM算法用红色圆表示, 混合匹配算法用蓝色三角形表示.由于未做闭环检测, 当AGV小车行驶到起始点附近时, 位姿漂移严重, 故图4中只选总轨迹长度836 m的前800 m的RPE误差对比, 在后续中会给出起始点附近的地图和累积误差.

图3 场景一三维点云地图

图4 RPE误差与里程的变化曲线对比

4.2 建图效果与累积误差分析

为验证3种算法的建图效果, 本文将通过3种算法得到的相邻帧点云之间的位姿转换矩阵将所有点云拼接成点云地图.如图5所示, 图5(a)、图5(b)和图5(c)分别为LeGO-LOAM算法、IMU/LeGOLOAM算法和混合匹配算法建立的场景二俯视图,图5中白色曲线、黑色曲线、黄色曲线和红色曲线分别为RTK估计的轨迹、LeGO-LOAM算法估计的轨迹、IMU/LeGO-LOAM算法估计的轨迹和混合匹配算法估计的轨迹.从图5中可看出3种算法在前期结构化信息丰富的场景中漂移并不明显, 后期AGV小车行驶至教学楼外圈结构信息较少时, LeGO-LOAM算法开始出现较大漂移, IMU/LeGO-LOAM算法由于IMU提供了一定的位姿信息进行优化, 故漂移小于LeGO-LOAM算法, 混合匹配算法在此阶段转换到IMU与ICP紧耦合的匹配算法, 故漂移低于IMU/LeGO-LOAM.本文为检测3种算法性能, 未做闭环检测, 所以当AGV小车行驶至起始点附近时, 地图出现较大重影.起始点的局部放大图如图6所示, 对比可以得出LeGO-LOAM算法建立的三维点云地图重影较大, 起点终点位姿相差较大, 且路径没有闭合; IMU/LeGO-LOAM算法建立的三维点云地图重影较小, 起点终点位姿相差较小, 且路径基本闭合; 混合匹配算法建立的三维点云地图重影最小, 起点终点位姿相差很小, 且路径闭合.

图5 轨迹点云地图对比

图6 起始点放大图对比

首先将3种算法估计的起点位姿和RTK估计的起点位姿进行对齐, 然后将3种算法估计的终点坐标与RTK给出的终点坐标进行对比, 计算出累积误差.从表2中可以看出混合匹配算法在场景一中累积误差与IMU/LeGO-LOAM算法一致, 混合匹配算法相较于LeGO-LOAM算法累计误差降低33.8%.在场景二中结构化信息较少的外环环境及大转弯、减速带区域,混合匹配算法累积误差相较于IMU/LeGO-LOAM算法和LeGO-LOAM算法累计误差分别降低了44.3%和89.8%

表2 累积误差对比 (m)

5 结论与展望

为实现AGV小车在室内、室外不同场景实时定位及高精度建图, 提出了基于IMU与激光雷达紧耦合的混合匹配算法.实验结果表明, 在结构化信息丰富的场景一中, 混合匹配算法与IMU/LeGO-LOAM算法相较LeGO-LOAM算法相对位姿误差均降低18.5%, 累计误差均降低33.8%; 在结构化信息较少的场景二中,混合匹配算法相较于LeGO-LOAM算法相对位姿误差降低33.0%, 累计误差降低89.8%; 混合匹配算法相较于IMU/LeGO-LOAM相对位姿误差降低10.9%, 累计误差降低44.3%; 混合扫描匹配算法定位精度均高于IMU/LeGO-LOAM算法和LeGO-LOAM算法,同时对比分析3种算法建立的地图重影程度, 得出混合扫描匹配算法比IMU/LeGO-LOAM算法和LeGO-LOAM算法建立的地图重影程度更小, 地图精度更高.下一步将会增加系统闭环检测, 进一步提高复杂环境中AGV小车激光雷达定位与建图精度.