基于单目视觉的仓储物流机器人定位方法

张 涛，马 磊，梅玲玉

(西南交通大学 系统科学与技术研究所，成都 610031)(*通信作者电子邮箱zhangtao1945@gmail.com)

基于单目视觉的仓储物流机器人定位方法

张 涛*，马 磊，梅玲玉

(西南交通大学 系统科学与技术研究所，成都 610031)(*通信作者电子邮箱zhangtao1945@gmail.com)

针对轮式仓储物流机器人的自主定位问题，提出了一种基于视觉信标和里程计数据融合的室内定位方法。首先，通过建立相机模型巧妙地解算信标与相机之间的旋转和平移关系，获取定位信息；然后，针对信标定位方式更新频率低、定位信息不连续等问题，在分析陀螺仪和里程计角度误差特点的基础上，提出一种基于方差加权角度融合的方法实现角度融合；最后，设计里程计误差模型，使用Kalman滤波器融合里程计和视觉定位信息弥补单个传感器定位缺陷。在差分轮式移动机器人上实现算法并进行实验，实验结果表明上述方法在提高位姿更新率的同时降低了角度误差和位置误差，有效地提高了定位精度，其重复位置误差小于4 cm，航向角误差小于2°。同时该方法实现简单，具有很强的可操作性和实用价值。

室内定位；多传感器信息融合；Kalman滤波(KF)；移动机器人；人工路标

近年来随着电子商务的发展，对物品的入库、储存、管理、出库监管提出了新的要求，如何高效地运送仓库中的物品是仓储管理系统能够快速运作的关键。 而机器人的普遍应用为其提供了新的机遇。 本文研究的主要内容是解决移动机器人“在哪儿[1]”的问题，实现移动机器人在一定范围内的自主定位。高云峰等[2]根据所使用的传感器不同将移动机器人定位分为航迹推算定位、地图匹配定位和基于信标定位三类。航迹推算定位通过惯性测量单元或者编码器推算出位置信息，从而不需要外界信息，但是其定位误差会逐渐累积，所以其定位精度受传感器精度影响较大。Cho等[3]通过使用Kalman滤波算法融合了惯性导航系统(Inertial Navigation System, INS)和编码器用于移动机器人的位置估计。地图匹配定位是机器人通过搭载的传感器测量周围环境后与地图进行匹配定位，目前较为热门的研究方向是在未知环境中进行同时定位与地图构建(Simultaneous Localization And Mapping, SLAM)。在文献[4]中使用RBPF(Rao-Blackwellized Particle Filters)方法结合激光测距传感器和里程计实现了一种SLAM算法并得到了广泛的应用。Wang等[5]针对低成本激光雷达在文献[4]的基础上提出一种改进闭环校正性能的方法。基于信标定位通过外置的信标给机器人提供绝对位置信息，常用的方式有视觉信标[6-7]、射频识别(Radio Frequency IDentification, RFID)[8]等，但是视觉信标需要前期的安装工作，所以在信标维护、费用等方面具有一定局限性。

本文首先介绍了基于二维码信标的定位原理，使用粗分割方法进行初步分割以达到减小运算量、提高信标识别速度的目的。其次建立相机模型，并在此基础上分析信标和相机间的位置关系，进而实现相机定位信息的提取。由于信标定位存在更新频率低、定位信息离散等问题，本文分析了里程计和陀螺仪角度误差特性，使用方差加权的方式融合角度，以得到更加稳定可靠的航向角；同时建立里程计误差模型，分析信标定位和里程计定位的噪声特性，使用Kalman滤波器对相机和里程计数据进行融合。在文章最后，将该系统搭载在移动机器人平台上进行实验以验证该系统的实际定位效果。

1 原理简介

在仓储物流系统中，通常情况下物品都会整齐地存放在货架之上，货架与货架之间的距离基本相同，且均匀分布在仓库之中，此时可以将整个仓储空间地图构建为栅格地图，单个货架占用一个或者多个栅格点。物流机器人的单次任务就可以等效为从一个特定的坐标点移动到另外一个特定的坐标点。本文中将储货仓库构建为一个长方形栅格地图，每个视觉信标代表一个栅格。机器人的定位过程就是需要在这个局部空间中实时得知自身的位置和航向角，位姿信息用于控制、导航等功能。建立如图1所示的世界坐标系Ow。

图1 世界坐标系

图1中大正方形是设计中使用的视觉信标。将多个信标整齐的、间距的、同朝向的排列在水平地面上，机器人通过搭载垂直于地面的相机对信标进行识别。每一个信标都包含唯一的编号ID和在世界坐标系中对应的位置信息(xw,yw)，相邻信标间的间距为d。

移动机器人在世界坐标系中的观测模型如图2所示。

图2 移动机器人观测模型

2 信标定位信息的提取

本文中使用的信标是 快速响应二维码(Quick Response code, QRcode)，作为常用的视觉标签具有识别简单、信息冗余量大等优点。

2.1 二维码初分割

二维码的识别过程需要对图像进行处理，此过程计算量大。为减小对设备硬件的要求，通过边缘投影法[9]对图像进行初步切割。QRcode本身由一系列黑白的像素构成，通过边缘检测处理可获得丰富的边缘信息，可考虑利用边缘信息对其进行初步切割，而边缘投影法的基本思想正是源于此。首先使用sobel算子对图像进行边缘检测，然后分别向其垂直和水平方向上进行投影，计算边缘图像在每一列、行上的值并作归一化处理，通过选取合适的阈值Tc分割边缘较为丰富的图像区域将其认定为QRcode区域。其数学表达式(1)如下：

(1)

其中：G(x,y)表示单幅图像中的像素点(x,y)灰度值。fW(x,y)用来表示第x列处灰度总和占总体灰度的比值，fH(x,y)用来表示第y行处灰度总和占总体灰度的比值，k为常数。通过式(1)直接求取的边缘投影一般毛刺较多，可以考虑对其先进行平滑滤波。本文中通过求解边缘投影的平均值，选取平均值的k倍作为阈值。图3是sobel边缘图像和经过边缘投影法切割的图像。图4为边缘图像垂直边缘投影，从图中可以看出选取合理的分割阈值可以实现垂直方向切割。在运行环境光线稳定时，初切割可以提高信标识别速度。

图3 边缘投影法QRcode初分割结果

图4 边缘垂直投影图

2.2 视觉定位信息提取

通过初切割后的区域并不精确，不能够获取精确的航向角，需对其进一步切割。本文使用QRcode经典算法[10]识别码值，通过确定码上包含的三个正方形定位模块，对其进行精确切割。

每个二维码的坐标都通过前期设定，如果能够获取到Oca系相对于Oc系的旋转和平移，就可以推算出机器人在Ow系中的位姿。相机成像过程需要经过刚体变换和透视变换两个部分，相机模型[11-12]可以等效为：

(2)

其中：

不失一般性，将二维码选定在zw=0的平面内，则式(2)可以等效为式(3):

(3)

在本设计中相机垂直于地面安放，所以近似认为翻滚角和俯仰角为零。通过式(4)可以获取航向角度θrc，其中r21和r22为R中的元素。在Ow系中相机位置(xca,yca)以及通过视觉测量得到的机器人位置(xrc,yrc)可以用式(5)表示。

θrc=arctan(r21/r22)

(4)

(5)

3 多传感器定位信息融合

由于二维码信标贴放有一定间隔，所以基于视觉的定位系统是信息离散的定位方式，定位时间间隔较长且更新频率较低，不符合移动机器人实时控制的需求。在本文中引入里程计作为位置传感器以及陀螺仪作为角度传感器进行辅助定位。里程计的定位原理本质上是基于航迹推算，其位置误差会随时间累积，长时间运行里程计信息并不可靠。陀螺仪角度通过角速度积分方式获取，由于噪声、零偏、角度随机游走等因素影响，其积分角度也会存在长时间漂移。考虑对多个传感器进行建模，使用Kalman滤波器对传感器的信息进行融合。

3.1 里程计误差模型

(6)

则进一步可得：

里程计主要的误差来源于三个方面：不等的轮直径、轮距不确定以及地面的随机不确定因素。考虑在一个采样周期中左右轮行进距离为Δsl和Δsr，其产生的误差为δsl和δsr。根据航迹推算模型可得里程计误差模型如式(7)所示。

(7)

3.2 基于方差加权的角度初步融合

在获取最后的角度之前，先初步融合里程计和陀螺仪的角度，有助于获取更加准确的角度信息，结合陀螺仪和里程计在角度方面的优势。里程计角度获取已经通过式(6)获得，显然单独的里程计角度误差会在机器人的行进过程中不断累积，考虑角度每次采样周期内变化Δθb，造成的角度误差为零均值高斯噪声，其带来的角度误差方差σθb为下式，kb为常数。

陀螺仪通过角速度积分获取角度，由于零偏、噪声、角度随机游走等因素存在，其角度误差会随着积分时间而不断累积，使得角度发生漂移。考虑采样周期Δt内，造成的角度误差方差σθg表示为下式，其中kg为常数，wo表示静态时陀螺仪输出角速度标准差。

根据上述分析可以得知里程计角度在低频段具有更好的特性，在角度变化剧烈时误差较大，而陀螺仪在高频段表现更好，对于长时间低动态表现不佳。可以考虑通过方差权重的方式将角度融合起来，融合角度θgb表示为下式：

k=σθg/(σθb+σθg)

使用上述方法对陀螺仪和里程计角度进行融合实验。实验结果如图5所示，子图(a)是多种传感器获取的角度，其中真实角度使用运动捕捉系统进行测量，子图(b)是各传感器角度和真实角度的误差。可以明显看到在静止时陀螺仪角度误差随时间累积，里程计角度误差则主要在动态转向角度大范围变化时变化较大，而通过方差加权融合后的角度在一定程度上弥补了单一传感器存在的缺陷，兼顾了动态和静态优点，降低了角度误差。

图5 方差权重融合角度与角度误差图

3.3 Kalman滤波融合定位信息

Kalman滤波是一种最优滤波器，通过建立系统的状态方程和量程方程并利用五个方程进行状态预测和更新，最终完成状态的最优估计。本文需要对机器人的位姿进行测量，里程计可以提供连续的定位信息，使用其和视觉信息进行融合能够得到准确的、连续的定位信息。

考虑选取里程计相关误差变量作为状态向量X，其数学表达式如下：

则根据式(7)可以得到状态转移矩阵F，但是考虑到里程计角度本身不够精确，将误差模型中θb用融合角度θgb进行替代。状态转移过程中的噪声主要考虑融合角度的角度噪声，以及里程计左右轮行进过程中的距离不确定噪声δsl,δsr。不考虑位置误差δxb,δyb噪声不确定性，原因在于位置误差主要由上一时刻状态和δsl,δsr,δθb决定，转移噪声可以认定为零。δsl,δsr分别与移动的距离有关，考虑在单次采样周期内移动距离为Δsl、Δsr，其噪声方差和移动距离的平方成比例关系[1]。kθ为融合后角度误差方差可通过实验确定。根据上述分析可以选取过程噪声协方差矩阵Q如下式：

其中kl,kr为常数也可以通过实验确定。

选取二维码定位信息和里程计定位信息的差值作为观测向量Z，其数学表达式如式(8)：

(8)

3.4 实验结果

本文整个实验在一个3.6 m×2.4 m的矩形区域内进行，最终结果是使机器人在此区域内进行自主定位。本文使用装备有机载PC的Pioneer3差分机器人作为运载体，将相机垂直于地面安放在机器人的前方，地面上等间隔铺设二维码，每个二维码间的距离为0.6 m，间距主要考虑货架间距和机器人本体移动时存在的误差，应当确保机器人在两个二维码间做直线运行时不会偏离出相机能够检测的范围。同时机器人上使用一块嵌入式板卡用于采集惯性测量单元(Inertial Measurement Unit, IMU)传感器ADIS16405和里程计数据用于数据融合，采样周期为81.45 Hz。整个实验过程是先将机器人放置在原点附近，通过自身定位自主按照矩形区域移动，机器人控制算法使用PID控制。实际运动轨迹通过OptiTrack运动捕捉系统采集。

实验轨迹结果如图6所示，图中可以看出机器人行进轨迹为一个矩形，经过滤波融合的轨迹相比单里程计轨迹有了很大的改进，基本不存在误差累积问题，可以用于机器人控制中。

图6 机器人运行轨迹

Y轴坐标、X与Y轴误差随时间的变化如图7所示，图中子图(a)为Y轴坐标，可以看出当机器人检测到二维码时就会使用二维码的信息对里程计位置信息进行校正，而当没有检测到时则只会对里程计位置进行估计，以使得定位误差降低。子图(b)和(c)分别是X轴和Y轴方向上位置与真实位置的误差，由于在实验过程中相机安放于车体前方，未安装在旋转中心，通过后续变换将其变换到旋转中心，因此角度上存在的误差会一定程度耦合到位置误差中，所以在定位精度上会受到一定的影响。但是图中可以看出通过Kalman滤波融合的方法明显降低了定位误差，最大定位误差小于4 cm，进而可以验证算法的正确性。

图7 Y方向位置与定位误差

4 结语

本文中从信标识别开始到定位信息融合，介绍了一种适用于差分移动机器人自主定位的方法。在视觉定位信息提取中把握住QR码特征，巧妙地利用图像与世界坐标的单应性矩阵解算相机旋转和平移并计算出其在世界坐标系中的位姿。 同时根据里程计与陀螺仪角度误差特点给出了一种基于角度误差方差加权的方法，此方法有效地结合了里程计低频特性和陀螺仪的高频特性，在有效抑制陀螺仪零偏和噪声带来的角度漂移的同时解决了里程计大角度范围变化下不准确的问题。 最后推导里程计误差模型，结合Kalman滤波器将信标定位信息用于修正里程计定位信息，在提高定位更新频率的同时提高了定位精度。最后实验结果表明此方法简单易行，具有非常好的可靠性与可实践性，其定位精度满足仓储物流机器人系统的需求。

References)

[1] SIEGWART R, NOURBAKHSH I R.自主移动机器人导论[M].2版.西安:西安交通大学出版社,2013:185-200.(SIEGWART R, NOURBAKHSH I R. Introduction to Autonomous Mobile Robots [M]. 2nd ed. Xi’an: Xi’an Jiaotong University Press, 2013: 185-200.)

[2] 高云峰,周伦,吕明睿,等.自主移动机器人室内定位方法研究综述[J].传感器与微系统,2013,32(12):1-5.(GAO Y F, ZHOU L,LYU M R, et al. Research overview of indoor localization methods for autonomous mobile robots [J]. Transducer and Microsystem Technologies, 2013, 32(12): 1-5.)

[3] CHO B S, MOON W S, SEO W J, et al. A dead reckoning localization system for mobile robots using inertial sensors and wheel revolution encoding [J]. Journal of Mechanical Science and Technology, 2011, 25(11): 2907-2917.

[4] GRISETTIYZ G, STACHNISS C, BURGARD W. Improving grid-based SLAM with Rao-Blackwellized particle filters by adaptive proposals and selective resampling [C]// Proceedings of the 2005 IEEE International Conference on Robotics and Atomation. Piscataway, NJ: IEEE, 2005: 2432-2437.

[5] WANG P, CHEN Z, ZHANG Q, et al. A loop closure improvement method of Gmapping for low cost and resolution laser scanner [J]. IFAC-PapersOnLine, 2016, 49(12): 168-173.

[6] ZHANG H, ZHANG C, YANG W, et al. Localization and navigation using QR code for mobile robot in indoor environment [C]// Proceedings of the 2015 IEEE International Conference on Robotics and Biomimetics. Piscataway, NJ: IEEE, 2015: 2501-2506.

[7] 温丰,柴晓杰,朱智平,等. 基于单目视觉的SLAM算法研究[J].系统科学与数学,2010,30(6):827-839.(WEN F, CHAI X J, ZHU Z P, et al. A visual SLAM algorithm based on monocular vision [J]. Journal of Systems Science and Mathematical Sciences, 2010, 30(6): 827-839.)

[8] MI J, TAKAHASHI Y. Design of an HF-band RFID system with multiple readers and passive tags for indoor mobile robot self-localization [J]. Sensors, 2016, 16(8): 1200.

[9] 刘家昕,刘家朋,赵宇明.基于边缘增强的二维码区域快速检测定位算法[J].计算机工程,2012,38(12):136-138.(LIU J X, LIU J P, ZHAO Y M. Fast 2D-code region detection and location algorithm based on edge enhancement [J]. Computer Engineering, 2012, 38(12): 136-138.)

[10] 国家质量技术监督局. GB/T 18284—2000 快速响应矩阵码[EB/OL]. [2016- 12- 07]. http://down.bzwxw.com/05/GB_T%2018284-2000.pdf.(The State Bureau of Quality and Technical Supervision. GB/T 18284—2000 QR Code [EB/OL]. [2016- 12- 07]. http://down.bzwxw.com/05/GB_T%2018284-2000.pdf.)

[11] ZHANG Z. A flexible new technique for camera calibration [J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2000, 22(11): 1330-1334.

[12] 吴福朝.计算机视觉中的数学方法[M]. 北京：科学出版社, 2008:46-49.(WU F C. Mathematical Methods in Computer Vision [M]. Beijing: Science Press, 2008: 46-49.)

Indoorpositioningmethodofwarehousemobilerobotbasedonmonocularvision

ZHANG Tao*, MA Lei, MEI Lingyu

(InstituteofSystemsScienceandTechnology,SouthwestJiaotongUniversity,ChengduSichuan610031,China)

Aiming at autonomous positioning of wheeled warehous robots, an indoor positioning method based on visual landmark and odometer data fusion was proposed. Firstly, by establishing a camera model, the rotation and translation relationship between the beacon and the camera was cleverly solved to obtain the positioning information. Then, based on the analysis of the characteristics of the angle difference between the gyroscope and the odometer, a method of angle fusion based on variance weight was proposed to deal with low update frequency and discontinuous positioning information problems. Finally, to compensate for a single sensor positioning defect, the odometer error model was designed to use a Kalman filter to integrate odometer and visual positioning information. The experiment was carried out on differential wheeled mobile robot. The results show that by using the proposed method the angle error and positioning error can be reduced obviously, and the positioning accuracy can be improved effectively. The repeat positioning error is less than 4 cm and the angle error is less than 2 degrees. This method is easy to operate and has strong practicability.

indoor positioning; multi-sensor information fusion; Kalman Filter (KF); mobile robot; landmark

2017- 03- 06;

2017- 04- 08。

国家自然科学基金重点项目(61433011)；国家自然科学基金青年项目(61603316)。

张涛 (1993—)，男，重庆人，硕士研究生，主要研究方向：机器人定位、机器人路径规划； 马磊(1972—)，男，贵州贵阳人，教授，博士，主要研究方向：机器人控制、多机器人系统、新能源控制； 梅玲玉(1991—)，女，安徽芜湖人，硕士研究生，主要研究方向：组合导航。

1001- 9081(2017)09- 2491- 05

10.11772/j.issn.1001- 9081.2017.09.2491

TP242

A

This work is partially supported by the National Natural Science Foundation of China (61433011, 61603316).

ZHANGTao, born in 1993, M. S. candidate. His research interests include robot indoor positioning, robot path planning.

MALei, born in 1972, Ph. D., professor. His research interests include robot control and multi-robot systems, control of new energy systems and electro-machenical systems.

MEILingyu, born in 1991, M. S. candidate. Her research interests include integrated navigation.