基于视觉SLAM和人工标记码的定位与导航方法研究

王永力，胡旭晓，兰国清，承永宏

(浙江理工大学 机械与自动控制学院，浙江 杭州 310018)

近年来信息处理、人工智能等领域的突破性进展，为自主移动机器人的发展提供了理论基础.在室内移动机器人研究中，自主定位与导航算法属于核心技术.为实现机器人的智能行走，必须解决其实时准确定位的问题，而视觉传感器能获取丰富的图像纹理信息，已逐渐成为移动机器人定位与导航的首选方案[1].同时定位与地图构建SLAM(Simultaneous Localization and Mapping)是自主移动机器人定位与导航的重要组成部分.SLAM是指搭载特定传感器的主体，在没有环境先验信息的情况下，在运动过程中建立环境模型，同时估计自身的运动.其中传感器扮演着重要的角色.它被粗略地分为内部传感器和外部传感器.其中，内部传感器有光电编码器、陀螺仪等；外部传感器比较丰富，有摄像头、GPS(Global Positioning System)、激光测距仪、声纳等.自主移动机器人的位姿估计通常依赖编码里程计.然而编码里程计在实际应用中有很大的不确定性，随着里程数增加，编码里程计往往会积累较大的误差.这对自主移动机器人来说是巨大的隐患.移动机器人在导航过程中需要不断感知周围动态环境，以引导自身安全到达目标位置.GPS导航、电磁导航、无线电导航、视觉导航等是移动机器人导航的主要形式[2-4].GPS导航定位精度不高，且较难在室内场景使用.电磁导航可用于室内场景的导航，但需要提前铺设大量的专用磁条，因而灵活性较差.采用无线电导航时，移动机器人接收的无线电信息易受其他电磁波的干扰.而视觉传感器能获取丰富的环境信息，同时成本相对较低，随着计算机处理技术的发展，视觉导航已不再是难事.

鉴于此，本文将视觉里程计和编码里程计进行信息融合，以提高定位精度，并借助人工标记码来实现机器人航向角的在线矫正和精确停靠.

1 基于扩展卡尔曼滤波算法的信息融合定位

1.1 视觉里程计和编码里程计

编码里程计最显著的问题就是无边界的误差累积带来的不确定性.通常编码里程计的误差很大，即使在10 m的移动距离内，机器人的位置估计也可能是错误的.移动机器人的定位误差有两种本质不同的来源，即系统性误差和非系统性误差.系统性误差主要来源于轮子直径不等、轮子未对准、有效轮距的不确定性等.非系统性误差来源于外部环境，如不平整的地面、轮子打滑、与外界物体碰撞等[5].

视觉里程计能够通过相邻帧间的图像来估计相机的运动.视觉里程计主要分为特征点法和不使用特征点的直接法.特征点法主要通过FAST(Features from Accelerated Segment Test)、ORB(Oriented FAST and Rotated Brief)等具体方法提取图像上的特征点，并通过匹配描述子来估计相机运动.特征点法对于特征丰富、纹理明显的场景有非常显著的应用效果.直接法则建立在灰度不变的强假设上，通过计算相邻帧间图像像素的光度误差来追踪关键点.直接法可快速估计移动机器人位姿，适用于实时性较高且计算资源有限的场合[6].但直接法因建立在灰度不变的强假设上，所以不适用于光照明显变化的场合.

本文选用ORB-SLAM2软件来搭建视觉里程计.ORB-SLAM2软件采用ORB特征点提取方法，在定位精度、快速运动鲁棒性等方面具有优异的性能.ORB-SLAM2还具有后端优化和回环检测功能，在一定程度上可消除视觉里程计的累积误差[7].但是，当周围环境特征点极少的时候，比如相机面对一堵白墙或一面玻璃，使用ORB-SLAM2软件则很难提取足够多的特征点，会极大地影响机器人的位姿估计.鉴于此，本文将视觉里程计和编码里程计的信息融合起来，通过取长补短来提高移动机器人的定位精度.

1.2 信息融合定位方法

为了实现多传感器之间的信息融合，本文采用扩展卡尔曼滤波器(EKF).EKF是一种高效的递归滤波器，在高斯线性情况下可依据最小均方误差准则获得系统状态的动态估计.而实际工程中许多情况下测量数据与系统动态参数间的关系是非线性的.对于这一非线性问题，通常可利用一阶泰勒展开式将非线性问题转化为一个近似的线性滤波问题，借助线性滤波理论进行求解[8].本文采用的融合策略如下：初始化移动机器人(以下简称机器人)的运动模型，将视觉里程计和编码里程计的读数作为观测值，当有观测值输入EKF时更新、迭代机器人运动模型的参数，从而让机器人的状态不断逼近真实值.

根据机器人的运动模型可得到EKF的状态转移方程，即：

Xk=f(Xk-1,Uk)+wk

(1)

式中：Xk、Xk-1分别为k时刻和k-1时刻机器人的状态空间；Uk为输入控制量；f表示状态转移过程.假设wk是协方差为Rk、均值为0的高斯白噪声，即：

wk～(0,Rk)

(2)

这样，已知k时刻的输入量和k-1时刻的状态，即可预测k时刻的状态.状态预测值的协方差矩阵为：

(3)

式中：Pk-1为Xk-1的协方差矩阵；Fk为f相对于状态空间的雅可比矩阵.

对于观测方程，有如下观测值：

zk=h(Xk)+vk

(4)

式中：h表示观测状态转移过程；vk为观测误差.假设vk是协方差为Qk且均值为0的高斯白噪声，即：

vk～(0,Qk)

(5)

记h相对于状态空间的雅可比矩阵为：

(6)

在得到预测和观测信息后，可更新k时刻机器人状态的后验概率分布.其中，后验均值为：

X(k|k)=X(k|k-1)+Kk(zk-h(Xk|k-1))

(7)

后验协方差矩阵为：

P(k|k)=(1-KkH)P(k|k-1)

(8)

k时刻扩展卡尔曼滤波的增益系数为：

Kk=P(k|k-1)HT(HP(k|k-1)HT+Qk)-1

(9)

通过式(7)～式(9)可了解机器人运动模型的更新迭代过程.根据前一时刻的状态Xk-1可得到机器人的先验状态X(k|k-1)及其协方差矩阵P(k|k-1).在算出k时刻扩展卡尔曼滤波的增益系数后，可更新k时刻机器人的后验状态X(k|k)及其协防差矩阵P(k|k).因此，在信息融合定位中，随着观测值的不断输入，机器人运动模型的参数会不断迭代更新，以达到实时定位的目的.

2 基于人工路标的导航算法

在货物搬取运输、拣选抓取等物流工作中，移动机器人需要按照既定的路径行驶，并完成停靠装卸货物等动作[9].在工程中，机器人周围导航环境不可能一成不变，基于视觉里程计和编码里程计的融合定位方法仍无法完全满足精确的导航要求.而标记码特征稳定，识别速度快，且识别效率高，对光照变化的鲁棒性强.因此，本文以标记码作为人工路标来辅助机器人导航.所选用的AR tag(Augment Reality tag)标记码(图1)采用黑底，能够与周围环境形成强烈对比，以便从其背景中进行提取.根据工程要求，可在不同位置制作不同编号、不同尺度、不同分辨率的标记码.

图1 AR tag二维码

标记码可分为纠正码和导航码.纠正码用于纠正机器人的航向角和位置.室内机器人由于机械结构等系统性误差或者轮胎打滑等非系统性误差的干扰，会不可避免地偏离原有运动方向，而难以保证其按照规划路径的顺利移动.因此，按一定距离放置纠正码，以便有效地改善这一问题.导航码用于机器人定位和发布控制指令.不同的控制指令与各导航码编号进行绑定，可建立各目标位置与控制指令的对应关系.机器人向某一目标位置移动过程中扫描导航码，并计算导航码与机器人的实际距离，判断其是否在有效距离内，若在有效距离内则认为机器人已到达目标位置，并执行该导航码的控制指令.

在部分纠正码被遮挡或损坏的情况下，要使机器人仍严格按照既定路径移动，可采用开环纠正的方法，来提高导航算法的鲁棒性，以便根据融合定位中航向角的变化情况对机器人移动方向进行自动纠偏.在直线移动过程中，一旦检测到航向角变化超过阈值则会进行一定的补偿修正，使其重回既定路径.

导航算法的流程如图2所示.机器人以初始速度运动，并根据视觉传感器传入的图片信息判断是否扫描到标记码.若未扫描到标记码，则根据融合里程计信息判断机器人航向角是否偏离，并进行位姿调整，即开环纠偏；若扫描到纠正码，则根据纠正码的绝对坐标推算机器人的位姿并调整到正确位姿，即闭环纠偏.若扫描到导航码，则根据导航码的编号ID(Identification)发布相应的运动指令，从而按照既定轨迹完成机器人导航任务.

图2 导航算法的流程

3 定位与导航实验

3.1 实验平台搭建

本文实验围绕Turtlebot2移动机器人搭建实验平台.Turtlebot2移动机器人机身搭载的Kinect1.0深度相机能够采集分辨率为640×480(像素)的RGB图像和环境深度图.实验选用的室内移动机器人如图3所示.

图3 室内移动机器人

3.2 定位实验

为检测视觉里程计和编码里程计的信息融合效果，在实验室内清除不必要的杂物，摆放了桌椅，形成一圈回环路径.在400 cm×300 cm的矩形区域内，控制机器人沿矩形轨迹运动.机器人运动速度为0.2 m/s,转动速度为0.5 rad/s.定位程序持续运行，共采集100组实际坐标数据，并拟合出实际运动轨迹.移动机器人定位实验结果如图4所示.对比视觉里程计和融合里程计发现，视觉里程计和编码里程计融合后，里程计数据更接近机器人实际运动轨迹.实测运动坐标与融合里程计坐标的误差如图5所示.

图4 移动机器人定位实验结果

图5 实测运动坐标与融合里程计坐标的误差

根据图5可求得，定位误差的算数平均值为0.015 cm，最大误差为0.034 cm.分析可知，定位误差和定位实时性能够满足室内自主移动机器人的定位需求.

3.3 导航实验

本文采用的移动机器人室内导航实验路标布局如图6所示.其中，AB段和FE段长度为2.5 m，BC段和DE段长度均为1.3 m，AF段、BE段和CD段长度均为2.5 m.打印出的标记码大小为10 cm×10 cm.在6个目标点布置对应于ID的导航码，当机器人运动到目标点附近时，判断其与导航码之间的距离是否在触发范围内，以决定机器人是否执行对应控制指令；各路段中间均放置一张纠偏码，移动机器人运动过程中得到纠偏码的反馈，若发现其偏离既定路径5 cm以上，则自动调整机器人的航向角，以完成纠偏.

图6 移动机器人室内导航实验路标布局

导航实验包括如下两个分实验：分实验一，使机器人路径沿着室内导航实验路标的ABCDEFA顺序运动，起点为A，绕完一圈回到起点A；分实验二的起点为A，终点为E，使机器人沿着ABCDEBAFE顺序运动.两组分实验均订阅了融合里程计的消息，将数据导入Matlab软件，并绘制图7所示的移动机器人室内导航实验运动轨迹.

图7 移动机器人室内导航实验运动轨迹

从图7可以看出，纠偏码和导航码的结合，使机器人沿着既定路径运动，完成了导航任务.由此验证了基于人工路标的导航方法可以顺利地帮助机器人完成导航任务.

4 结束语

本文采用扩展卡尔曼滤波器，将ORB-SLAM视觉里程计与编码里程计进行信息融合.实验表明，融合后运动轨迹更接近实际运动轨迹，融合后里程计具有更好的定位效果.以两种二维码作为人工路标，将室内各目标位置的导航码与控制指令建立对应关系，直接控制机器人按规划路径运动，实现导航功能.将纠偏码用于纠正机器人在运动中航向角的偏离.实验证明，该导航方法能有效地完成室内自主导航任务.下一步将对导航方法进一步优化，使机器人满足具有大面积车间作业、承担复杂导航任务的工程需求.