基于二维码视觉与激光雷达融合的高精度定位算法

栾佳宁，张 伟*，孙 伟，张 奥，韩 冬

（1.同济大学电子与信息工程学院，上海 201804；2.西安电子科技大学空间科学与技术学院，西安 710126）

（*通信作者电子邮箱zhang_wi@tongji.edu.cn）

0 引言

在一个已知环境中，移动机器人为在环境中进行移动，需要借助自身的环境感知能力来计算出自己在环境中的位姿，这个问题被称为机器人的定位问题。对于以仓储机器人为代表的室内移动机器人而言，其对于定位算法的要求集中在高精度、低成本、高可靠性。由于其工作环境较为固定，可以提前构建高精度地图且通过人为构建特征的方式提升机器人的定位精度。

最近几年以ORB-SLAM［1］为代表的一系列视觉即时定位与建图（Simultaneous Localization And Mapping，SLAM）算法以及包括SVO（Semi-direct monocular Visual Odometry）［2］在内的视觉里程计算法都有了很大的发展，但是由于仅使用一个相机无法有效对物体的深度进行估计［3］，所以视觉SLAM算法往往需要使用双目相机系统或者是RGB-D 相机，在仅拥有单目相机的机器人系统上无法成功应用。同时，也有学者通过学习的方法来解决定位问题，文献［4］中作者在学习阶段提取环境特征点并进行降维存储，并在后续利用匹配的方式计算自身的位姿；但是，相对于较为成熟的激光SLAM 算法，纯视觉SLAM 算法容易因为外界光线变化或是特征稀疏等问题导致跟踪失败。

在已有高精度地图的情况下，目前使用较多的是基于蒙特卡罗的定位算法。该算法最早由Dellaert 等［5］提出，其利用自身的运动模型进行采样，并利用激光传感器的数据评估每个粒子，并将得分最高粒子的位姿作为自身的最终位姿。大量研究人员在该算法基础上进行了改进，例如ROS（Robot Operation System）中搭载了自适应蒙特卡罗定位（Adaptive Monte Carlo Localization，AMCL）算法［6］，使用自适应KL 散度（Kullback-Leibler Divergence，KLD）［7］来更新粒子，可有效降低总体采样粒子数，也让机器人面对绑架问题具有更好的鲁棒性，并在业界得到了广泛使用。厉茂海等［8］通过减少采样粒子数量来优化采样过程，提出了以混合高斯模型为基础的蒙特卡罗算法，进一步降低了计算量。在这些算法中，由于其先验传感器信息只有里程计和激光雷达，且无有效算法对机器人的打滑或漂移等现象进行预测或限制，因此机器人在出现上述情况时定位会产生较大偏移，甚至可能导致算法开始全局重定位，另一方面由于缺乏全局的环境信息，所以全局定位时有可能收敛至全局最优点且很难自恢复。

此外，在AMCL 算法的自适应调整机制执行中，自适应机制通过计算粒子的平均分数来判断机器人是否遇到绑架问题，并利用全局随机播撒粒子的方式试图解决此问题。但在实际执行时，算法对于机器人是否被绑架的判断常常失准，很容易因为一些外界干扰或是机器人打滑等问题导致整体分数突然降低；同时该算法收敛较慢，且容易收敛至局部最优点，导致定位出现差错，例如在完全对称的环境中可能出现定位翻转。

除基于粒子滤波的算法之外，以Cartographer［9］为代表的基于优化的激光SLAM 算法也可以在拥有高精度地图的情况下完成定位任务，Cartographer 引入子图这一概念，在前端完成当前帧与子图的匹配，并在后端完成回环检测，以进行位姿的优化。但是在已经完成建图的情况下，利用Cartographer 定位仍存在着某些问题，其中最主要的便是随着地图的增大，Cartographer 在进行回环检测时消耗的算力会大幅上涨，一些研究者将回环检测的搜索任务分配给GPU（Graphics Processing Unit）进行计算以减少时间，但是对于没有搭载GPU 的机器人而言，利用Cartographer 进行高帧率的定位仍存在着地图大小等环境因素的限制。

为实现低计算量的高帧率定位，研究人员也尝试通过人工构建特征的方式（如利用二维码或其他路标）来完成定位任务［10-13］。Nazemzadeh 等［14］使 用QR 码（Quick Response Code）和里程计做融合，提出了一种基于扩展H∞滤波器的定位算法，Zhang 等［15］提出了一种利用天花板上的二维码进行定位，利用激光雷达进行避障的定位算法。这些算法利用二维码信息进行定位，有效消除了里程计的累积误差，但是为保证定位的准确度，这些算法都需在环境中布置大量的二维码，同时也需要机器人按照一定的路径运动。

在此基础上，胡章芳等［16］提出了融入二维码信息的自适应蒙特卡罗定位算法，利用二维码信息提供的绝对位置信息对AMCL 算法进行初始化和矫正，使得机器人能够在一定程度上脱离二维码进行定位，但是仍对二维码的布置和机器人的运动路径有着一定的限制，且定位精度不高。

上述算法引入了二维码信息辅助定位，有效消除了里程计的累积误差，但是由于二维码均布置在地面或天花板上，易磨损玷污、部署复杂，需要经常维护，同时需要极大数量的二维码才能保证整体环境内的定位精度，且由于该算法直接将二维码的识别结果送入算法进行位姿解算，对二维码识别错误等有可能出现的问题缺乏处理机制，算法整体鲁棒性不高。

针对以上问题，本文提出了一种融合二维码视觉和激光雷达的定位算法，可将二维码部署于障碍物表面或墙上，相较于之前的纯单目二维码定位或是二维码-激光融合算法，本算法对机器人的运行轨迹无任何要求，可实现环境中的任意运动且在此同时环境中布置二维码的数量大大减少；由于引入了二维码的绝对位姿信息，所以相对于AMCL 等雷达定位算法，算法无需初始位姿先验，也可有效应对机器人绑架问题。系统首先利用二维码携带的绝对位姿信息进行粗定位，并结合激光雷达数据对该位姿进行优化提升位姿精度，同时引入里程计信息对二维码的位姿进行监督，防止二维码识别出现错误，同时保证在未观测到二维码的时候仍能够较准确地定位，适用于解决仓储机器人为代表的室内机器人的定位问题。

1 视觉传感器信息处理

1.1 二维码相对位姿解算

为获取机器人的准确位姿，需要解算出机器人与二维码之间的相对位姿，首先利用张正友标定法［17］对相机进行标定得到相机内参。然后，当视觉传感器获取一帧图像后，识别算法会首先在图像上寻找二维码所携带的特征点，将其解析并利用其作为索引去检索数据库以得到当前二维码坐标系与地图坐标系之间的变换Rw2t、Tw2t，并可通过求解PnP（Perspective n Points）问题计算出机器人基坐标系（在此假设机器人相机坐标系与基坐标系重合）与二维码坐标系间的变换Rt2b、Tt2b。本文采用AprilTag［18］开源算法进行二维码的识别和特征提取后利用文献［19］提出的线性PnP解算算法进行PnP解算，AprilTag 算法具有实时性好、准确率高等特点，最后利用式（1）、（2）获取机器人基坐标系相对于世界系的变换Rw2b、Tw2b：

1.2 多二维码位姿融合

相较于传统二维码定位算法使用二维码作为地标，本文算法将其贴在障碍物和墙上，克服了作为地标易磨损玷污的缺点且减少了二维码在环境中需求的数量，同时在机器人的运行过程中可能会有多个二维码出现在机器人的视野中。但与此同时，由于相机的标定存在一定误差、二维码布置存在一定误差且因成本考虑仅使用单目视觉进行识别，相较于将二维码布置到地面或是天花板抑或是利用Kinect进行辅助的算法［20］，得到的变换关系的准确度相对较差。虽然系统并不利用二维码解算到的位姿直接作为结果输出，但仍希望视觉定位的位姿能尽量准确，因此本文提出以下算法对多个二维码返回的位姿进行融合，提升二维码返回位姿的准确度。

1.2.1 异常识别二维码去除

设将位姿映射到世界坐标系后，返回位姿集P={P1，P2，…，Pn}，定义位姿Pi与真值之间具有误差ei，记由于噪声或者标定不完全出现的自然误差构成的集合为N，由于二维码污损或其他问题导致的识别错误引入的误差集合为M。异常位姿去除阶段不考虑自然误差，故其目标可定义为寻找集合Φ∈[1，n]使：

在绝大多数情况下噪声较小，且标定误差不大，所以ei一般较小，即|ei-ej|较小，本文利用这一特点来切分位姿集，由式（4），实际计算时可利用|pi-pj|来切分位姿集。设当前位姿真值为Ptrue，则：

利用式（4）的结论，以位姿Pi为中心点，δdis为半径，δyaw为高，在x-y-yaw空间内构建置信域Ωi。则若Pj∈Ωi，则可大概率认为ei、ej∈N。将该结论推广，对位姿集中所有点按上述方式构建置信域，并将所有在置信域中的位姿放入Pi的内点集ψi，并定义num(ψi)代表ψi中的元素数目，则：

1.2.2 多二维码位姿融合

由于二维码布置在障碍物的表面，其相对于机器人的距离和角度都会随着机器人的运动发生巨大的变化，又因为机器人有时候会在很远的距离上对二维码进行观测，所以会使机器人观测多二维码时存在着一定的重投影误差。如图1 所示的情况，机器人对4 个红点所示位置的二维码进行观测时，生成了4 个相互不重叠的位姿（4 个红色小箭头）。本文在对误差进行研究时，假设误差的主要来源为相机标定不完全以及存在着观测噪声。

图1 重投影误差示意图Fig.1 Schematic diagram of re-projection error

为了能够获得标定误差和实际投影误差的关系，本文利用一种基于几何特征的PnP 解算算法来进行PnP 计算［18］，该算法针对呈矩形分布的P4P 问题提出了一种线性的求解方式，由于该算法避免了非线性迭代的过程，所以本文基于该算法进行了相机内参误差和解算位姿误差间的关系推导。

如图2所示，设q0、q1、q2、q3分别为图像中二维码的4个角点，其在像素坐标系下的坐标分别为(x0，y0)、(x1，y1)、(x2，y2)、(x3，y3)，f为相机的焦距，P0、P1、P2、P3分别为世界坐标系中二维码的角点。

图2 三维位姿测量示意图Fig.2 Schematic diagram of 3D pose measurement

相机内参矩阵和焦距与内参矩阵的关系分别表示为：

点q0的相机坐标系下的坐标为：

其他3个点同理，记它们的坐标分别为：(X0，Y0，f)、(X1，Y1，f)、(X2，Y2，f)、(X3，Y3，f)。

对于由光心Oc和q2、q3组成的平面π1，其法向量N1可表示为：

由于P0P1‖π1，所以可得：

继而可得：

整理式（13），并定义：

将N1的表达式代入，可得：

k1同理，所以可得h与fx、fy、cx、cy无关，即标定结果不对h的值产生影响。

考虑到世界坐标系中|P0P1|=d1，可知：

变换式（16）得到：

求k对各变量的导数：

由多元函数的泰勒展开，在仅保留一阶小量时，可得：

同时可得到位置误差与标定误差的关系：

而后，利用式（28）计算R并将上文解出的误差关系代入后可解出ΔR：

在获得标定误差和位姿误差的对应关系后，假设相机的标定误差服从随机的高斯分布，则利用式（25）～（28）将可得到观测结果的均值和方差。

设对第k个二维码的观测的结果为：

利用式（32）最大高斯概率的密度函数归一化积对各变量进行多高斯分布融合：

式（32）的结果为多高斯分布融合后的均值，对各变量分别求均值后利用结果作为视觉定位的最终输出。

2 基于多传感器融合的位姿增强

2.1 激光点云对准的位姿优化

在获得融合二维码信息得到的位姿后，继续利用点云对准的方式对位姿进一步优化，本文选择一种基于旋转矩阵求解的点云对准算法［21］，定义每一个激光点的误差为：

其中：pj为激光点云在机器人基坐标系上的坐标，yj为pj在地图上欧氏距离最近的障碍物点坐标，C和r分别为激光雷达坐标系和地图坐标系的旋转矩阵和平移矩阵。

同时定义点云在雷达坐标系中的质心和其在地图上欧氏距离最近的障碍物点的质心分别为：

利用上述定义构建优化函数：

则求解目标为寻找雷达与地图之间的旋转变换关系C及平移变换关系r使得J最小。

定义平移量d的表达式为：

将d代入误差函数，得到：

通过这种表达方式，可以分离C和d并分别对两项进行优化。

对于含有C的各项：

式中：

定义如下的代价函数对C进行优化：

对变量依次求导得到：

式（43）、（44）分别为旋转矩阵C的约束条件，对于式（42），令=0，可得：

由于旋转矩阵是一个正交矩阵，所以可得：

求解出机器人的最优旋转矩阵后，将C代入式（37）中，并令平移量d=0，得到：

利用式（48）、（49）的结果对机器人基坐标系的位姿进行不断修正，便可使式（36）的误差不断减小，直至达到最大迭代次数或是误差小于一定值后停止迭代。

2.2 基于里程计的位姿估计与搜索

在实际系统运行过程中，为保证系统的鲁棒性，需要保证在机器人视野中没有二维码时仍能够顺利完成定位，故引入了基于里程计的位姿计算。首先里程计数据代入式（50）计算出当前时刻的先验位姿，并利用2.1 节所述的点云对准方式进行对准，相较于AMCL的KLD采样，该算法在高帧率的情况下具有更高的准确度以及更少的算力消耗。

2.3 里程计-二维码定位的交叉位姿验证

实际应用中，由于二维码污损或是张贴不平整等外部原因会导致视觉传感器返回位姿错误，继而可能会导致定位算法无法搜索到正确的位姿，因此为了保证系统的鲁棒性，本文在这里提出了里程计-二维码定位矫正算法。设：Pl(xl，yl，θl)、Pv(xv，yv，θv)分别为上一时刻机器人的位姿和优化的得到的最优位姿。Δx、Δy、Δθ分别为利用机器人里程计计算的位姿差，则机器人利用运动模型计算得到的位姿为：

得到该位姿后，基于连续性假设，由式（51）、（52）对机器人的位姿进行检测，并利用式（53）输出矫正后的位姿，Podomcal为3.2节提出的基于里程计计算得到的位姿：

在此同时，考虑到机器人绑架问题的存在，若在短时间内多帧图像均被系统判定为识别错误，则有可能是因为机器人被人为挪动，此时算法选取与环境匹配度更高的位姿作为结果输出，定义maxpoint 函数的输出为输入中具有最高分数的位姿，则输出位姿表示为：

2.4 基于二维码视觉与激光雷达融合定位算法

基于以上的各算法，在此给出本文提出的融合算法的算法流程，该算法有效解决了视觉传感器和雷达数据的融合问题，且具有较好的鲁棒性，能够应对二维码识别错误、没有找到二维码等多种问题。

记ξvisal为利用每个识别到的二维码信息计算的机器人基坐标系在世界坐标系下的坐标，其内部可能包含有多个元素，也有可能不包含任何元素，即视野中可以没有任何二维码，Plast为上一时刻的位姿，Δx、Δy、Δθ分别为利用机器人里程计计算两时刻位姿差，则整体的算法流程如下：

算法1 Pose Enhancement Algorithm。

输入Plast，ζ，Δx，Δy，Δθ；

输出Pcurrent。

3 实验及结果分析

3.1 实验平台

使用如图3 所示的室内移动机器人系统进行实验，搭建了一个模拟仓库环境，场地尺寸为8 540 mm×5 100 mm，共布置32 个二维码，均位于障碍物表面，二维码使用DJI Robomaster S1 系列的二维码，3D 场地效果图如图4 所示。机器人采用IMU 结合编码器生成里程计数据；同时利用DAHENG MERS133-54 摄像头采集图像，摄像头固定在云台的yaw轴上。机器人自身携带一个TX2 处理器作为运算单元。

图3 室内移动机器人Fig.3 Indoor mobile robot

图4 测试场地示意图Fig.4 Schematic diagram of test field

为模拟机器人的真实运动场景同时保证评判的公平性，由实验人员手动遥控机器人在场地中进行随机运动，并在运行过程中包含无法观测到二维码、只观测到一个二维码和观测到多个二维码几种情况，运行轨迹如图5 所示，将全过程记录并分别用视觉里程计定位、AMCL 算法和本文算法进行定位。

图5 机器人的运动轨迹及二维码布置Fig.5 Robot’s moving trajectory and two-dimensional code layout

3.2 实验评判指标

在本次实验中，因为场地缺乏运动捕捉系统，所以无法获取机器人的精确位姿信息，为了能够最大限度地给出算法的精确度信息，本次实验并没有用建图算法进行建图，而是使用了场地的高精度地图，以保证评价的准确性。同时本文利用以下指标来对位姿进行评判：

其中yj、ej的定义与式（33）中相同。需要特别说明的是，采样点距离障碍物的平均距离并不等同于机器人在世界坐标系上的误差，在机器人姿态角存在误差时，远处采样点误差较大。该指标与定位误差呈正相关关系，但不存在绝对的线性关系。

3.3 实验结果及分析

3.3.1 算法对比

为了提升算法的可比较性，机器人运行全程利用ROS 的录包功能录制机器人运行全程传感器返回的数据，并使用一台处理器为i5-6300HQ 的计算机分别用4 种算法对数据进行处理。

本文共对四种算法进行对比，参数与设置与说明如下：

1）二维码里程计叠加（TAG-ODOM）算法：将所有利用二维码计算得到的位姿做平均后直接使用，若没有识别到二维码，则在上一时刻位姿的基础上叠加利用里程计数据计算出的两时刻位姿差。

2）AMCL 算法：初始位姿已知，最大粒子数1 000，最小粒子数50，里程计的噪声分别设置为1.0，1.0，1.0，0.8，0.8。

3）基于二维码视觉与激光雷达融合定位算法（fusion method）：点云对准算法的最大迭代次数为50。

4）Cartographer 算法：初始位置已知，输入里程计和雷达的数据，建图分辨率为0.05 m。

图6 为机器人运行全程中点云与地图的匹配度对比，可以明显看出，二维码视觉与激光雷达融合算法在机器人运行的全程表现良好，而AMCL 算法则表现欠佳，总体准确度与本文算法差距较大，其主要原因在于在于运动过程中速度较快且速度变化比较剧烈，导致里程计误差较大。二维码里程计叠加算法因没有对二维码的观测进行校准，所以精度会随着机器人与二维码之间的距离变化波动显著。Cartographer 算法定位精度介于AMCL和本文算法之间，表现较好。

图6 四种算法定位精度对比Fig.6 Localization accuracy comparison of four algorithms

表1 给出了实验数据的综合对比。在实际测试过程中，AMCL 算法在粒子完全收敛后的处理速度与本文算法大致相同，但是变化剧烈的运动速度导致了AMCL 粒子不能很好地收敛，所以在进行高机动性的运动时，本文算法的处理时间要远短于AMCL算法。

表1 不同算法的采样点平均误差与单次位姿计算耗时对比Tab.1 Comparison of different algorithms inaverage sampling point error and time cost per pose calculation

基于以上实验，表2对上文提到的4种算法以及文献［1］、文献［16］、文献［22］算法进行对比。

表2 各算法性能对比Tab.2 Performance comparison of different algorithms

由上述对比可以看出，AMCL 和Cartographer 两种算法只基于雷达，对环境要求较低，但是Cartographer 算力消耗较大，AMCL 精度较为一般的同时抗机器人绑架性能较差。文献［22］算法通过大量的二维码结合里程计获得了较好的定位效果，但是需要严格按照二维码路径运行，因此部署难度较大。I-AMCL（AMCL Integrated with two-dimensional code information）算法［16］利用二维码提供的信息进行初始化并纠正机器人的运动，但是整体精度较差。ORB-SLAM 虽然不需对环境进行任何布置，但是只能基于双目或RGB-D 相机运行且对于外界环境较为敏感。本文算法虽然对于环境布置有一定要求，但是在定位精度上表现较好，同时对于机器人运行路径基本没有要求，具有较强的灵活性。

3.3.2 算法鲁棒性测试

本文算法更多地考虑了实际运行过程中可能出现的问题，图7 给出定位精度和识别到二维码数量的关系。可以看出，在运行过程中，即使在一段时间内找不到二维码，定位仍能在一段时间内保持稳定，可见本文算法能很好地应对由于二维码污损或是环境中二维码数量较少引起的信息缺失问题。

图7 定位精度和识别到二维码数量的关系Fig.7 Relationship between localization accuracy and the number of two-dimensional codes recognized

针对二维码在实际运行过程中可能出现的误识别问题，同样给出测试结果。在实验时实验人员故意将字母V对应二维码在地图上的位姿设置错误，以此来测试二维码识别错误对系统带来的影响，针对图8 的图像，图9 给出了定位系统结果的展示。图中左侧小坐标的位姿为视觉识别的输出结果，但是经过系统的矫正，给出了右侧小坐标的输出结果，而根据点云的匹配程度和图10 给出的高视角无人机照片可以看出系统成功地对视觉传感器的误识别进行了矫正。

图8 误识别图像Fig.8 Wrongly recognized image

图9 误识别的矫正Fig.9 Correctness of wrong recognition

图10 机器人在场地中的位置Fig.10 Robot’s position in field

由以上两个测试可以看出，本文算法能有效应对因为二维码布置量较少或是污损导致的信息缺失问题，以及由于二维码污损导致携带信息错误或是场地布置时出现人为失误引起的识别错误问题。

4 结语

本文提出了一种基于二维码信息的视觉-激光定位融合算法，通过解算并融合自身相对于二维码的位姿获取机器人在地图上的大致位姿，并将其作为激光定位的初始位姿，然后利用点云对准进行优化，有效地解决了传统二维码算法以及二维码-激光融合算法存在的对机器人运动轨迹有限制的问题。相较于传统雷达定位算法，由于系统拥有了更多的环境信息，所以能不依赖初始位姿的设定进行定位的同时提高了系统的整体精度。经过测试，本文算法相较AMCL 算法采样点平均误差减小了92%，单次位姿耗时缩短了88%；同时引入里程计-视觉传感器监督机制，提升了系统的鲁棒性，在环境中可能出现二维码信息缺失以及二维码识别错误的情况下仍能成功地获得机器人在地图上的正确位姿。

得益于进行在观测到多个二维码时的位姿融合以及使用激光雷达的二次定位，在环境的布置上，本文利用32 个二维码实现了43 m2环境内的准确定位，相较传统将二维码作为地标的算法［22］在布置二维码数目有了较大的减少的同时并没有损失定位精度，并且将二维码布置在墙上的布置方式使得二维码更不易磨损，维护难度降低。同时，本文算法在地图上的任意位置均可进行定位，对于机器人的移动轨迹无任何要求。

下一步工作希望在已有算法的基础上，融入更多的传感器信息，例如引入能够观测场地的摄像头，给予机器人更多的先验信息，进一步提升定位精度。