学术交流 | LiDAR地图匹配与二维码融合的AGV室内定位与导航
以下文章来源于测绘学报 ，作者测绘通报
测绘学报
关注《测绘学报》公众平台，及时掌握测绘地理信息领域前沿信息，随时随地浏览优秀学术论文，为您提供一个高水准的学术交流平台。
↑ 点击上方「中国测绘学会」
可快速关注我们
摘要：
自动导引机器人（AGV）室内定位的方法多样，激光雷达匹配定位是其主要手段之一，但在环境特征少的区域，因缺乏约束会导致匹配错误。针对这一问题，本文提出了一种LiDAR和二维码地标组合导航方法，并利用turtlebot3在室内走廊环境下进行了模拟试验。结果表明，纯激光匹配导航平均误差为11.5 cm，采用与二维码结合的导航方法能将平均误差控制在2 cm内。该方法能够实现AGV在环境特征单一场景下精准定位与导航，提高了其对工作环境的适应能力和可靠性。
关键词：LiDAR；二维码；室内定位；自动导引机器人；导航
阅读全文：
http://tb.sinomaps.com/article/2021/0494-0911/20210102.htm
全文概述
现今自动导引机器人(automated guided vehicle, AGV)的高度智能化和简单便捷的操控性，为仓库等室内场景提供了迅速高效便捷的运输方式[1-12]。定位导航精度一直是室内AGV研究的重点与难点，其中激光和相机传感器在室内定位中有着广泛的应用[3-7, 10-12]，与此同时，惯性导航、超带宽技术、磁导航以及各种组合导航技术也在室内定位中发挥着重要作用[8-9]。
对于搭载激光雷达的机器人，自适应蒙特卡洛定位(adaptive Mount Carlo localization, AMCL)是常用定位方法，其基于粒子滤波匹配，能适应大多数室内环境。但在特征点不足、地图场景相似因素多的环境中，其定位可靠性较差[10-12]，导致机器人偏移预设路径。二维码中可储存如坐标等大量有用信息，其3个定位角点也能为机器人当前姿态提供参照，其布设简单，成本低廉，灵活性强，是室内辅助定位的良好手段[13-14]。
综合上述情况，本文针对室内定位中特征点不足的环境，采用激光雷达匹配定位的同时，通过相机识别二维码地标校正定位坐标，并在走廊中实地测试证明该方法的精度与可靠性。
算法原理
在激光雷达匹配定位下辅以二维码坐标校正算法应先建立环境栅格地图，如图 1所示。导航开始时，首先确定机器人在栅格地图中的初始位姿，复制N个初始位姿作为AMCL的初始粒子群，运行时粒子群根据机器人的运动变换自身位置，通过激光雷达的当前帧，计算出激光点云与占据栅格的吻合程度，每一个激光点与栅格地图的不同匹配情况有相应的评估分值，当激光点与占据栅格重合时拥有最高得分。最终用高斯分布描述机器人的位姿信息，将基于定位与目标规划的运动路径发送给下位机控制舵机运动，运动过程中相机扫描到二维码后，将解算的二维码坐标反馈给定位算法，以该坐标作为新的初始坐标散布粒子群，形成循环控制。
图1 算法流程
1
地图建立
SLAM(simultaneous localization and mapping)，即在未知环境中获取机器人实时位姿并创建环境地图。本文建图阶段采用激光SLAM中常用的Hector SLAM方法[5-7]。
2
机器人的定位与导航
自适应蒙特卡洛定位(AMCL)是一种基于粒子滤波的概率定位方法。在初始位置周围对粒子进行均匀散布，结合机器人的运动模型对粒子位置进行估计。同时把激光传回的点与地图做匹配，对每一个粒子位置的可信度进行评分，保留高分粒子作为机器人位姿，舍弃低分粒子，并增加新的粒子维持整个循环[12]。
为了对粒子运动进行模拟，需要了解机器人的运动模型。差速轮主要通过两轮差速进行转向控制和曲线运动控制，转动的方向和曲线运动的半径可通过两轮旋转方向及两轮的速度差ΔV实现。机器人的运动模型可表示为
(1)
点击浏览公式
式中，(xk, yk, θk)为机器人当前时刻的状态；(xk-1, yk-1, θk-1)为上一个时刻机器人的状态；V为速度；W为角速度。
在ROS的导航节点中，将机器人的预设轨迹分段成等距离散点。给机器人指定初始坐标位置，根据机器人当前的位姿、目标点位置和全局栅格地图，A*算法能逐路径点规划出到达下一个目标点的最佳全局路径。动态窗口算法(dynamic window approach，DWA)在此基础上构建出局部路径，实现机器人的动态避障，将路径转化为控制信息发送给机器人Open CR控制板驱动舵机，使机器人沿轨迹运动。同时，舵机运行数据、全局地图和激光雷达数据将反馈给AMCL定位算法，对运动中机器人的位姿进行实时估计，从而实现机器人的闭环控制[15]。
3
二维码识别与坐标校正
二维码中存放着该点的精准坐标，通过计算画面中心与当前帧中二维码的相对位置，即可得到机器人的实际坐标信息。如图 2所示，二维码上的3个方框即为二维码自带的3个定位标识，P0、P1、P2、P3为逆时针标注的4个角点，θ为二维码正向与相机正向夹角。
图 2 二维码识别
当二维码出现在画面中时，相机坐标如图 2所示，将二维码中心坐标位置作为校正精确点，其中心像素计算公式为
(2)
点击浏览公式
式中，(xP, yP)为角点像素坐标；(xcenter, ycenter)为画幅中心点像素坐标；(Δx, Δy)为实际校正坐标；k为比例系数。
二维码与相机画幅的夹角为
(3)
点击浏览公式
由于该夹角形式为欧拉角，还要将其转换成机器人控制通用的四元数形式。本文的定位导航只研究二维平面，故只用考虑航向角yaw，俯仰角pitch与翻滚角roll均为0°，则简化后的欧拉角转四元数公式为
(4)
点击浏览公式
将得到的校正位姿反馈给AMCL，重新散布粒子云，即可实现二维码的定位校正。
试验与结果分析
1
试验设计
本次试验采用turtlebot3 burger型号机器人，搭载树莓派4开发板，采用主从机连接的方式进行控制与数据解算。试验相关的代码与运算均在ROS操作系统内运行。
在运动学上，机器人为双轮差速运行方式。在传感器方面，搭载Hokuyo的UTM-30LX-EW单线激光雷达，相机则采用树莓派原装V2摄像头，固定焦距800万像素。
将Hokuyo激光雷达、树莓派相机与turtlebot3机器人进行连接，相机垂直朝下扫描二维码地标。在ROS中将机器人的各个传感器坐标与Base footprint基准坐标系进行链接，明确各个坐标系之间的转换参数(如图 3所示)。
图3 试验机器人
通过Hector SLAM方法建立试验环境的二维栅格地图，地图栅格分辨率为1 cm。由于本文研究低特征环境下的导航方法，通过Photoshop图像处理软件将二维栅格地图中间部分的特征进行处理，去除部分特征物，用于更好地模拟空旷狭长的走廊环境。最终得到长10 m、宽1.92 m的导航特征地图。
导航过程中采用激光雷达的自适应蒙特卡洛定位，通过A*全局路径规划算法实现点与点间沿固定路径运行，利用动态窗口算法进行局域路径规划用以避障；用Open CV对实时影像逐帧抓取并通过Zbar库对二维码信息进行提取，利用二维码携带的坐标对激光雷达的匹配误差和机器人舵机的运行误差进行统一校正。试验中将雷达的扫描半径限制为2 m，在运动过程中为了在相机实时影像帧间顺利识别二维码，将相机实时影像设为60帧，画幅大小为320×240像素，在地面的覆盖范围约为20 cm×15 cm，机器人以0.11 m/s的速度运动，到达目标点的容许误差设为3 cm(如图 4所示)。
图4 试验场景与导航地图
试验选取定点导航的模式，设定一条折线轨迹，并建立目标点，让机器人在走廊中沿着轨迹前进到达预定目标点。二维码标识贴在机器人运行轨迹上，其间隔为1m，每张二维码都精确测定其在栅格地图中的相对坐标，机器人沿着预定轨迹运行，运行到地标上方时二维码被相机捕获，即可通过二维码相对于相机画幅的位置与偏移角度，得出机器人当前精确的位置与航向角。
试验结果记录3组轨迹数据，无外部校正的运行轨迹、半径2m模拟低特征点环境轨迹，以及低特征条件下辅以二维码校正的轨迹。同时沿着机器人运行方向选取9个特定点，量取机器人到达该点时与设定坐标的误差绝对值，每个特定点间隔1 m。最终得出航向与横向各自在激光定位与二维码校正下的精度数据，分析得出结论。
2
结果分析
本次试验得到4组轨迹数据，如图 5所示。
图5 机器人运动轨迹对比
由于两侧墙面对机器人的位置均有约束，激光扫描墙面特征明显，因此运行过程中并不会出现如同航向上的大幅度位置误差的问题，3条轨迹在地图y轴上基本重合。在走廊环境下，机器人的定位偏差主要表现在走廊纵向。其中二维码辅助导航效果良好，基本与真实轨迹一致。单纯的激光导航在中后期开始误差明显，而无约束条件时误差随着时间与位移的累计越来越大，基本不能按照设定轨迹运行。
地图x轴向的精度与y轴向的精度如图 6所示。
图6 特定点导航误差对比
在航向上，机器人的航向误差呈现先增加后减小的趋势，这是由于初始位置位于房门位置，地图特征明显，匹配定位的精度高；机器人运行到走廊中部时，机器人运行的误差逐渐增大，中间部分只有走廊两侧的墙面特征，机器人运行过程中出现误匹配的可能性增大，粒子滤波表现也没有完全收敛，呈现出线状而非精确定位后的聚集状态。运行6 m后最大误差可达20.0 cm。而在结合二维码的定位校正后，机器人的航向导航精度得到显著提高，其最大误差为2.1 cm。试验结果也显示两种方法在地图y轴上精度差异基本不大，激光匹配定位最大误差为2.5 cm，二维码校正后的最大误差为1.5 cm。
将各点误差计算均方根误差，得到表 1。可以看出，在结合二维码导航时，均方根误差明显减小，导航精度得到有效提高。
表1 均方根误差对比
结语
本文描述了现有激光匹配定位在低特征点环境下的不足，提出了一种基于二维码识别和激光雷达匹配的融合定位方法，最后通过试验对比证明了新方法的可行性。二维码识别有其局限性，如光照要求与表面干净整洁的要求，但其优势也是十分明显，低廉的成本与便捷的操作让其应用场景非常广泛。在特定场合结合二维码的导航精度十分可观，符合AGV作业需求。
原标题：《学术交流 | LiDAR地图匹配与二维码融合的AGV室内定位与导航》