基于视觉里程计和自然信标融合的移动机器人定位算法

王 志，朱世强，李月华，张学群

（浙江大学流体动力与机电系统国家重点实验室，杭州 310027）

基于视觉里程计和自然信标融合的移动机器人定位算法

王 志，朱世强※，李月华，张学群

（浙江大学流体动力与机电系统国家重点实验室，杭州 310027）

针对未知室内环境下移动机器人的定位问题，提出了一种基于单目视觉和自然信标融合的移动机器人定位算法。该方法采用普通摄像头和全景摄像头结合的方式进行定位。采用普通摄像头提取天花板特征，进行视觉里程计定位。全景普通摄像头提取周边环境及天花板信息，采用Harris-SIFT构建自然信标库，并且进行自然信标检测与识别。在两者的基础上，移动机器人进行实时定位。利用自然信标定位时，提出方向滤波器结合视觉里程计和惯导系统估计旋转角度，利用自然信标求取定位特征点，两者结合进行定位。该方法有效地结合了相对定位的快速性和绝对定位的准确性。在室内环境进行试验，其中自然信标定位在x，y方向的平均误差分别为38.66和31.54 mm。相比视觉里程计而言，结合自然信标定位的平均误差分别减小了32.53%和68.68%。融合视觉里程计的自然信标定位运算速度相比仅靠自然信标定位而言提高了约6倍左右，同时与视觉里程计定位处于同一数量级。试验结果表明相对于视觉里程计而言，所提算法大大提高了定位精度，同时不影响算法的实时性。

机器视觉；机器人；图像处理；视觉定位；自然信标

王 志，朱世强，李月华，张学群. 基于视觉里程计和自然信标融合的移动机器人定位算法[J]. 农业工程学报，2017，33(10)：70－77. doi：10.11975/j.issn.1002-6819.2017.10.009 http://www.tcsae.org

Wang Zhi, Zhu Shiqiang, Li Yuehua, Zhang Xuequn. Mobile robot localization algorithm by combining visual odometry with natural landmarks[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(10):70－77. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.10.009 http://www.tcsae.org

0 引 言

定位技术是移动机器人领域研究的热点和难点之一。传统的定位方式主要有：电磁引导[1]、超声定位[2]、激光定位[3-6]、无线电定位[7]、惯导定位[8]以及 GPS定位等。这些定位方式大多只能用于简单固定的环境，扩展性较差，需对环境进行相应改造。近年来，由于视觉定位的便捷性，不需对环境进行改造，在移动机器人领域得到了快速发展[9]。视觉定位通过获取环境信息，检测环境中的特征如点[10]、线[11]、特定区域[12]等获取相机的位姿估计。视觉定位技术可分为绝对定位和相对定位。绝对定位主要是在环境中人为设置已知位置的信标，根据信标定位机器人[13]。由于信标位置已知，因此该定位方式简单可靠，但是适用面窄。一旦信标被遮挡或损坏，定位过程将无法进行。一般情况下，信标主要设置在机器人周边，但也有信标设置在天花板上[14]。相对定位是指机器人通过对比前一时刻与当前时刻环境中特征对应关系获得机器人当前变化量，然后结合上一时刻位姿得到当前位姿。该定位方式灵活，可用于机器人的实时定位与导航[15]。最常用的视觉相对定位技术主要是视觉里程计技术[16]。视觉里程计可分为单目视觉里程计、双目视觉里程计以及全方位视觉里程计[17]。但是视觉相对定位存在累计误差，不适合长距离定位。

基于视觉的定位需要快速、准确地提取环境特征。杨国国等[18]卷积神经网络来识别图像中目标特征进行害虫定位与识别。贺付亮等[19]利用视觉图像分割提取环境特征来识别桑葚。在室内环境下，可以很容易地从地面、周边环境以及天花板上提取。由于天花板具有不容易遮挡，变动小以及受光照影响小等特点，研究者多选用天花板作为视觉定位的参照物。韩立伟等[20]提出了一种基于直线和单特征点的视觉推算定位方法，主要提取天花板图像中 2条垂直直线交点作为特征点，以及上下帧图像间特征直线的夹角作为方位角，从而进行定位。但该方法在每一时刻间都有累计误差，造成定位结果不准确。潘良晨等[21]等提出了一种基于特征跟踪的定位方法。该方法仍然以天花板作为参照物，采集到特征点后根据卡尔曼滤波方法估算机器人当前位姿。但是该方法本质上仍是相对定位，无法得到准确的绝对位置。Chen等[22]提出了一种基于天花板的视觉 SLAM，该算法主要包括天花板特征提取，特征定位和视觉里程计三部分，同时考虑到几何约束。但是该算法运算复杂度高，不适用于实时定位。此外由于室内环境变动不大，因此也可作为定位参照物。马建光等[23]提出了一种基于全向摄像机的定位方法。该方法主要全景摄像头获取全景图像，然后进行图像压缩，最后根据投影图像的特征点和特征向量进行定位。由于全景图像畸变大，故定位精度不高[24]。

本文提出了一种基于视觉和自然信标融合的移动机器人定位方法，该方法结合环境特征与天花板特征进行定位。首先机器人在环境中选取特定点作为自然信标，存储其天花板特征以及环境特征。在定位过程中结合视觉里程计和自然信标进行定位。

1 机器人定位系统

本论文采用的机器人平台为TurtleBot平台，试验系统如图1所示。其视觉定位系统由PC机、普通彩色摄像机以及全景摄像机组成。2个摄像机光轴均垂直向上，垂直于天花板。普通摄像机采集天花板图像，检测得到天花板特征点与特征直线位置信息。全景摄像头采集周边环境图像，通过环境图像判断机器人方位，然后结合天花板特征信息定位。整个定位系统流程图如图2所示。

图1 试验系统Fig.1 Experimental system

图2 定位系统流程图Fig.2 Flow diagram of proposed localization method

2 基于天花板的视觉里程计定位

由于天花板被遮挡的可能性较小，因此可以提取稳定的特征。办公室内的天花板一般有矩形扣板和矩形日光灯，具有两组直线，组内直线平行，组间直线垂直，可以方便地提取特征直线。初始化时选定离图像中心最近的直线交点作为特征点，特征直线的方向作为初始方向。在假定机器人不会大距离打滑的情况下根据下一帧图像特征点位置和特征直线方向角进行定位。其工作流程图如图3所示。

图3 视觉里程计定位软件流程图Fig.3 Software flowchart of visual odometry

2.1 图像处理与特征检测

天花板之间有缝隙，形成了相邻的矩形。但是由于光线噪声等影响，会造成边缘模糊，造成特征提取不准确。因此需对图像进行滤波处理。本文选用改进的导向滤波器进行滤波[25-26]。导向滤波器具有较好的边缘保持特性[27]。由于其运算复杂度与支持窗口大小无关，因此具有较好的实时性。滤波函数可表示为：

式中G为参考图像；I为输入图像，则滤波器输出ˆZ可视为参考图像G在窗口Ωζ1(p)下的线性变换；“·”代表两矩阵对应元素相乘；μG,ζ1(p),μI,ζ1(p)和μG·I,ζ1(p)分别代表G,I和G·I的平均值。定义为

式中q是以点p为中心窗口的邻域点；Z(p)是点p处滤波器的输出值；Ni为邻域点数目； 为常数。

经图像滤波处理后对图像进行特征直线检测。本文采用hough算法检测特征直线，但是由于hough算子比较耗时，因此本文只对定位参考图像做全局检索，得到特征直线和特征点信息。其他图像均只在上一帧图像特征点附近做局部检索。加快了检测速度，提高了算法实时性。

2.2 基于天花板视觉里程计定位模型

假定地面平整，天花板无遮挡物，同时地面与天花板平行。相机经标定后得到的焦距内参数为kx,ky。假定天花板高度为h，则摄像机每一个像素所代表的世界坐标系中的距离的比例系数为kl=h/kx。则相机坐标系与世界坐标系的对应关系为式（3）。

式中udi,vdi为第i帧图像特征点相对于光轴中心的坐标；xwi,ywi为特征点在世界坐标系下的位置；pxi,pyi为世界坐标系相对于摄像机坐标系的位置；φi为第i帧图像摄像机的方位角。由式（3）可得到式（4）和式（5）。

式中ud,i+1,vd,i+1为第(i+1)帧图像特征点相对于光轴中心的坐标；px,i+1,py,i+1为第(i+1)帧图像世界坐标系相对于摄像机坐标系的位置；φi+1为第(i+1)帧图像摄像机的方位角。Δφi为第i帧与(i+1)帧图像间角度变化量；Δpxi与Δpyi为第i帧与(i+1)帧图像间位置变化量。

由对应元素相等可得到：

在之前的研究中[10-11]，都是直接计算上下帧图像之间的相对位置，然后逐步累积。而本文则采取当前时刻图像与初始时刻图像进行定位，只要特征点在视野范围内，参考图像即为初始图像，本文算法无需累计运算，这样可大大减小累计误差，提高定位精度。因此机器人位置可表示为

式中px0,py0和ud0,vd0分别为参考定位图像的特征点世界坐标和图像坐标。然后将世界坐标系相对于摄像机坐标系的位置转化为摄像机坐标系相对于世界坐标系的位置

由于摄像机始终垂直于天花板，所以各点纵坐标已知且固定，只需计算平面坐标，减小了计算难度。

3 基于自然信标视觉定位

由于视觉里程计的累计特性，在长时间运行时不可避免地将出现累计误差。特别是机器人在一个闭合的轨迹上运行时，误差并不会随机器人回到原点而清零，而是继续累计。因此每隔一段距离需用自然信标对机器人进行绝对位置校正，这样既不会影响定位的实时性，还能提高定位精度，去除累计误差。

3.1 自然信标库建立

为了融合自然信标进行定位，在定位开始前需要首先建立自然信标库，然后基于自然信标进行定位。建立自然信标的关键在于特征的提取与存储以及自然信标点的选取问题。目前常用的特征提取算法有：SIFT算子[28-29]、SURF算子[30]、FAST算子[31]以及Harris算子[32]等。其中SIFT算法是目前最成功的局部特征提取算法，其具有很好的尺度、旋转以及光照不变性[28]。但是由于SIFI算子计算复杂，难以应用到实时定位中。

在本文中，为了减小计算复杂度，选用 Harris算子来提取环境特征。Harris算子R具有计算简单，对图像旋转及灰度变化具有鲁棒性等特点。其值为：

式中det表示矩阵行列式，Tr为矩阵直迹，k为常数，C为相关矩阵，且

式中Iu(x),Iv(x),Iuv(x)分别为图像在点x的灰度值在u和v方向的偏导和二阶混合偏导。当某点的R值大于设定值时，该点为特征点。

本文中的自然信标融合天花板与周边环境，因此需要对天花板图像和环境全景图像分别进行特征提取和存储。在建立自然信标库的时候，自然信标点的选取也影响着机器人的定位精度。自然信标数量太多，会造成运算速度的下降，不利于实时定位；但是如果数量太少，则会积累较多的累计误差。在本文中，选取原点作为第一个自然信标，然后将环境图像与自然信标库中的图像进行特征检测与匹配，当匹配特征点数目小于一定阈值T时，表明该点与远离自然信标，可选取该点为新的自然信标点，添加到自然信标库中，且赋予特定的ID编号。本文中取T=15。

3.2 自然信标识别与定位

机器人借助自然信标进行定位，需准确识别到自然信标以及其ID。识别到非信标点或者错误的信标ID会造成很大的定位误差，且难以校正。建立自然信标库后，与自然信标卡中图像逐一比对。若采用传统的SIFT特征检测与匹配算法会耗费大量的时间。为了提高实时性，本文选用Harris-SIFT算法进行自然信标检测与识别。选用Harris角点检测方法替代SIFT算子检测特征点，然后为每个特征点选择主方向，生成特征向量描述子，最后根据特征向量间的欧氏距离作为匹配判据，找出匹配特征点。

机器人方位的判定对自身定位起着非常重要的作用。本文提出方向滤波器估计机器人旋转角度。本文中的方向滤波器选取方向误差作为状态矢量，采用间接卡尔曼滤波的方法对机器人旋转角度进行估计，由于数据融合仅仅发生在误差空间，近似线性空间，适合采用卡尔曼滤波器。假设当前图像I到自然信标图像N的旋转变换用四元数表示，的估计值用表示，两者的误差用δq表示，则有：

Ä表示四元数相乘。令Δq表示视觉里程计计算得到的上下帧之间的旋转变换，则有：

假设误差δq服从均值为 0 的正态分布，则根据式（16）可得到：

式中，ΔR表示Δq对应的旋转矩阵。采用改进的罗德里格参数变换可得到：

式中，w表示动态噪声，no表示观测噪声。由于Xk,Yk是方向误差空间的矢量，两者可以视为不相关，因此动态噪声w的协方差矩阵Q和观测噪声no的协方差矩阵W均为对角矩阵。为了更好地估计旋转角度，本文结合惯导系统的旋转角度进行计算。利用视觉里程计计算Q，惯导系统计算W。

方向滤波器最终的输出为：

利用Harris-SIFT算法检测对当前时刻天花板图像与自然信标天花板图像进行匹配，求得特征点位置。但是由于天花板特征非常相似，容易出现误匹配，因此在匹配完成之后，仍采用上述聚类分析的方法，得出最佳匹配点。具体过程如下：1）输入：包含n个匹配点的旋转角度和簇的数目ki；2）输出：ki个簇，使平方误差准则最小。具体步骤：1）任意选择ki个对象作为初始的簇中心；2）根据簇中对象的平均值，将每个对象(重新)赋予最类似的簇；3）更新簇的平均值，即计算每个簇中对象的平均值；4）直到平均值不再发生变化。

根据对象数量最多的簇选取定位特征点。本文中取ki=3。然后利用3.2节中的方法进行定位。即：

式中，px,refpy,ref和ud,refvd,ref和分别为自然信标图像的特征点世界坐标和图像坐标。Δφi,ref为机器人相对于自然信标的转动角度。

4 验结果与分析

4.1 自然信标检测试验

自然信标包含环境自然信标和天花板自然信标，在移动机器人行进过程中，需对自然信标进行检测，检测结果如图4和图5所示。图5中白色圆圈表示定位特征点。从图4和图5中可以看出，可根据匹配特征点的数量来判定自然信标是否匹配成功。若匹配成功，便可根据自然信标进行定位。

图4 不同时刻周围自然信标检测结果Fig.4 Detection results of natural landmarks in different times

图5 天花板自然信标检测结果图Fig.5 Detection results of ceiling natural landmarks

4.2 总体定位试验

在图 1 所示移动机器人上，对本文提出的基于视觉和自然信标融合的定位算法进行了验证，该平台是一个机器人开源平台，供用户进行二次开发。试验在实际室内环境中进行。控制机器人在一个长为6.3 m，宽为3.78 m的矩形轨迹上运动。分别采用视觉里程计以及视觉里程计结合自然信标 2种方式进行定位，以机器人起始位置作为坐标原点，以天花板缝隙方向分别作为x轴和y轴建立世界坐标系。机器人沿逆时针运动，最后回到原点。试验共记录了 264组试验数据。为便于显示，按一定间隔选取了77组数据进行显示，整体定位结果如图6所示。

从图 6中可以看出，视觉里程计定位轨迹与真实轨迹大致吻合，但是中间出现较大定位误差，且回到原点后，累计误差无法消除，会随着机器人的运动继续增大。但是，融合自然信标后的自然定位与真实位置基本吻合，在运动过程中不会出现较大的定位误差。而且由于自然信标的存在，当机器人回到原点后，可以根据自然信标判断已回到原点，这样可以消除累计误差，所以融合自然信标定位的运动轨迹可以实现闭环。视觉里程计定位和融合自然信标定位的定位误差曲线见图7。从图7中可以看出视觉里程计定位x轴的平均误差为57.3 mm，y轴的平均误差为100.69 mm，而融合自然信标定位的x轴的平均误差为38.66 mm，y轴的平均误差为31.54 mm。相比视觉里程计而言，结合视觉里程计和自然信标定位算法的x轴的平均误差减小了32.53%，y轴的平均误差减小了68.68%，因此融合自然信标的视觉定位技术可以大大提高定位精度，同时不会影响算法的实时性。

图6 机器人运动轨迹图Fig.6 Moving trajectory of robot

图7 定位误差图Fig.7 Error map of localization

4.3 任意位置定位试验

为了验证本文所提的融合自然信标定位的绝对定位特性以及其稳定性。本文还做了任意点起步定位试验。即机器人不是从原点出发，而是从任意位置出发。如果采用视觉里程计定位，机器人的位置又从原点开始定位，无法进行绝对定位。而采用本文算法，则可以对任意位置进行定位，且没有累积误差。本文随机选取了 4个点作为测试点，然后进行定位。4个点定位结果如表1所示。

表1 随机位置定位结果Table 1 Localization results in random points mm

然后分别从位置2顺时针运动，位置3逆时针运动，所得到的定位数据分别如表 2所示。为了方便显示，只选用了其中部分结果显示。其中位置2总共测试了64组测试点，其平均误差为：x轴为36.3 mm，y轴为39.1 mm。位置 3总共测试了43组测试点，其平均误差为：x轴为28.5 mm，y轴为42.2 mm。

表2 从任意位置起步定位结果Table 2 Localization results starting from random positions mm

从表 2中可以看出，本文所提的基于自然信标定位的方法打破了传统视觉里程计定位不能精确绝对定位的限制。当自然信标库建立完成后，机器人可以在任意位置进行绝对定位，提高了机器人的实用性。

4.4 算法实时性分析

在实时性方面，由于视觉里程计定位为相对定位，而且定位过程中采用局部搜索的方式选取特征点，实时性较好。在试验中，视觉里程计的平均运行时间是53 ms，自然信标定位是 405 ms，而本文提出的融算法时间为69 ms。因此，融合视觉里程计的自然信标定位算法的运算速度相比仅靠自然信标定位提高了约 6倍，同时其运行时间与视觉里程计相当，其精度却大幅提高。因此，本文算法可以用于实时定位。

5 结论与讨论

本文叙述了一种基于视觉和自然信标融合的移动机器人定位方法。该方法将相对定位和绝对定位进行了很好地结合。选用自然信标结合视觉里程计进行定位。在环境未知的办公室环境下进行多组试验，试验结果表明该方法在闭合路径下可以实现闭环，无累计误差。其平均误差均在 50 mm以内。其中x轴的平均误差仅为38.66 mm，y轴的平均误差仅为31.54 mm。该算法还能实现任意位置起步绝对定位，且平均误差也在50 mm以内。实时性方面，本文算法的运算速度相比仅靠自然信标定位提高了约 6倍，而且与视觉里程计相当，可以应用于机器人实时定位。

目前本文工作主要集中应用于有方形块的天花板的室内环境。下一步工作中，主要研究怎样快速建立自然信标库以及降低自然信标库误识别与误匹配，提高定位精度。同时扩展到普通环境，能够适用于其他类型的天花板及室内环境，增强算法通用性。

[1]吴功平，曹珩，皮渊，等. 高压多分裂输电线路自主巡检机器人及其应用[J]. 武汉大学学报：工学版，2012，45(1)：96－102.Wu Gongping, Cao Yan, Pi Yuan, et al. An autonomous inspection robot for high voltage bundled transmission line and its application[J]. Engineering Journal of Wuhan University, 2012, 45(1): 96－102. (in Chinese with English abstract)

[2]苑晶，黄亚楼，孙凤池. 面向移动机器人自定位的无线网络构造算法及实现[J]. 仪器仪表学报，2011，32(1)：99－106.Yuan Jing, Huang Yalou, Sun Fengchi. Construction algorithm and its implementation for wireless network in self-localization of mobile robot[J]. Chinese Journal of Scientific Instrument,2011, 32(1): 99－106. (in Chinese with English abstract)

[3]钱晓明，张浩，王晓勇，等. 基于激光扫描匹配的移动机器人相对定位技术研究[J]. 农业机械学报，2016，47(3)：14－21.Qian Xiaoming, Zhang Hao, Wang Xiaoyong, et al. Relative localization technology for mobile robot based on laser scan matching[J]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(3): 14－21. (in Chinese with English abstract)

[4]韩明瑞，周波，钱堃，等. 基于激光雷达的室外移动机器人三维定位和建图[J]. 华中科技大学学报：自然科学版，2015(增刊 1)：315－318.Han Mingrui, Zhou Bo, Qian Kun, et al. 3D localization and mapping of outdoor mobile robots using a LIDAR[J]. Journal of Huazhong University of Science and Technology: Nature Science Edition, 2015(Supp.1): 315－318. (in Chinese with English abstract)

[5]刘洞波，刘国荣，胡慧，等. 基于激光测距的温室移动机器人全局定位方法[J]. 农业机械学报，2010，41(5)：158－163.Liu Dongbo, Liu Guorong, Hu Hui, et al. Method of mobile robot global localization based on laser range finder in greenhouse[J]. Transactions of the Chinese Society for Agricultural Machinery, 2010, 41(5): 158－163. (in Chinese with English abstract)

[6]曾慧，吴福朝，胡占义. 基于平面激光测量的移动机器人自定位方法[J]. 自动化学报，2007，33(2)：138－144.Zeng Hui, Wu Fuchao, Hu Zhanyi. Robot self- localization based on planar laser measurement[J]. Acta Automatica Sinica, 2007, 33(2): 138－144. (in Chinese with English abstract)

[7]Mikhaylov K, Tikanmäki A, Petäjäjärvi J, et al, On the selection of protocol and parameters for UWB-based wireless indoors localization[C]//International Symposium on Medical Information and Communication Technology (ISMICT).Worcester, MA, IEEE, 2016:1－5.

[8]冯国虎，吴文启，曹聚亮，等. 单目视觉里程计/惯性组合导航算法[J]. 中国惯性技术学报，2011，19(3)：302－306.

[9]Ben-Afia A, Deambrogio L, Salos D, et al. Review and classification of vision-based localization techniques in unknown environments[J]. Iet Radar Sonar Navigation, 2014,8(9): 1059－1072.

[10]晁志超，伏思华，姜广文，等. 单目摄像机-激光测距传感器位姿测量系统[J]. 光学学报，2011，31(3)：0312001.Chao Zhichao, Fu Sihua, Jiang Guangwen, et al. Mono camera and laser range finding sensor position-pose measurement system[J]. Acta Optica Sinica, 2011, 31(3):0312001. (in Chinese with English abstract)

[11]赵振庆，叶东， 陈刚，等. 垂直直线特征的双目视觉位姿测量方法[J]. 光学学报，2014，34(10)：1015003.Zhao Zhenqing, Ye Dong, Chen Gang, et al. Binocular vision method of measuring pose based on perpendicular lines[J].Acta Optica Sinica, 2014, 34(10): 1015003. (in Chinese with English abstract)

[12]侯俊捷，魏新国，孙军华. 基于同心圆合成图像匹配的双目视觉标定[J]. 光学学报，2012，32(3)：0315003.Hou Junjie, Wei Xinguo, Sun Junhua. Calibration method for binocular vision based on matching synthetic images of concentric circles[J]. Acta Optica Sinica, 2012, 32(3):0315003. (in Chinese with English abstract)

[13]厉茂海，洪炳镕，罗荣华，等. 基于单目视觉的移动机器人全局定位[J]. 机器人，2007，29(20)：140－144.Li Maohai, Hong Bingrong, Luo Ronghua, et al. Monocularvision-based mobile robot global localization[J]. Robot, 2007,29(20): 140－144. (in Chinese with English abstract)

[14]Xu De, Han Liwei, Tan Min, et al. Ceiling-based visual positioning for an indoor mobile robot with monocular vision[J]. IEEE Transactions on Industrial Electronics, 2009, 56(5),1617－1625.

[15]Yu Jinxia，Cai Zixing，Duan Zhuohua, et a1. Design of dead reckoning system for mobile robot[J]. Journal of Central South University of Technology, 2006, 13(5): 542－547.

[16]Kitt B, Geiger A, Lategahn H. Visual odometry based on stereo image sequences with RANSAC-based outlier rejection scheme[J]. IEEE Intelligent Vehicles Symposium, 2010, 43(6):486－492.

[17]姜燕华，熊光明，姜岩，等. 智能车辆视觉里程计算法研究进展[J]. 兵工学报，2012，33(2)：215－220.Jiang Yanhua, Xiong Guangming, Jiang Yan, et al. A review of visual odometry for intelligent vehicles[J]. Acta Armamentarii,2012, 33(2): 215－220. (in Chinese with English abstract)

[18]杨国国，鲍一丹，刘子毅. 基于图像显著性分析与卷积神经网络的茶园害虫定位与识别[J]. 农业工程学报，2017，33(6)：156－162.Yang Guoguo, Bao Yidan, Liu Ziyi. Localization and recognition of pests in tea plantation based on image saliency analysis and convolutional neural network[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(6): 156－162. (in Chinese with English abstract)

[19]贺付亮，郭永彩，高潮，等. 基于视觉显著性和脉冲耦合神经网络的成熟桑葚图像分割[J]. 农业工程学报，2017，33(6)：148－155.He Fuliang, Guo Yongcai, Gao Chao, et al. Image segmentation of ripe mulberries based on visual saliency and pulse coupled neural network[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE),2017, 33(6): 148－155. (in Chinese with English abstract)

[20]韩立伟，徐德，基于直线和单特征点的移动机器人视觉推算定位法[J]. 机器人，2008，30(1)：79－90.Han Liwei, Xu De. A visual dead reckoning positioning method based on lines and single feature point for mobile robot[J]. Robot, 2008, 30(1): 79－90. (in Chinese with English abstract)

[21]潘良晨，陈卫东. 室内移动机器人的视觉定位方法研究[J].机器人，2006，28(5)：504－509.Pan Liangchen, Chen Weidong. Vision-based localization of indoor mobile robot[J]. Robot, 2006, 28(5): 504－509. (in Chinese with English abstract)

[22]Chen Pengjin, Gu Zhaopeng, Zhang Guodong, et al. Ceiling vision localization with feature pairs for home service robots[C]//2014 IEEE International Conference on Robotics and Biomimetics (ROBIO), Bali, IEEE, 2014: 2274－2279.

[23]马建光，贾云得. 一种基于全向摄像机的移动机器人定位方法[J]. 北京理工大学学报，2003，23(3)：317－321.Ma Jianguang, Jia Yunde. An omnidirectional camera based localization for a mobile robot[J]. Transactions of Beijing Institute of Technology, 2003, 23(3): 317－321. (in Chinese with English abstract)

[24]王景川，陈卫东，曹其新. 基于全景视觉与里程计的移动机器人自定位方法研究[J]. 机器人，2005，27(1)：41－45.Wang Jingchuan, Chen Weidong, Cao Qixin. Omni-vision and odometer based self-localization for mobile robot[J].Robot, 2005, 27(1): 41－45. (in Chinese with English abstract)

[25]Li Zhengguo, Zheng Jinghong, Zhu Zijian, et al. Weighted guided image filtering[J]. IEEE Transactions on Image Processing, 2015, 24(1): 120－129.

[26]Kou Fei, Chen Weihai, Wen Changyun, et al. Gradient domain guided image filtering[J]. IEEE Transactions on Image Processing. 2015, 24(11): 4528－4539.

[27]Zhu Shiqiang，Wang Zhi, Zhang Xuequn, et al. Edgepreserving guided filtering based cost aggregation for stereo matching[J]. Journal of Visual Communication and Image Represention, 2016, 39: 107－119.

[28]刘立，万亚平，刘朝晖，等. 基于 SIFT匹配算法的移动机器人单目视觉定位研究[J]. 系统仿真学报，2012，24(9)：1823－1830.Liu Li, Wan Yaping, Liu Zhaohui, et al. Object localization for mobile robots using single-eye based on SIFT[J]. Journal of System Simulation, 2012, 24(9): 1823－1830. (in Chinese with English abstract)

[29]An Xuecheng, Hong Wei ，Xia Hua, Research on binocular vision absolute localization method for indoor robots based on natural landmarks[C]//Chinese Automation Congress(CAC), Wuhan, IEEE, 2015: 604－609.

[30]朱奇光，王佳，张朋珍，等. 基于高斯矩改进SURF算法的移动机器人定位研究[J]. 仪器仪表学报，2015，36(11)：2451－2457.Zhu Qiguang, Wang Jia, Zhang Pengzhen, et al. Research on mobile robot localization based on gaussian moment improved SURF algorithm[J]. Chinese Journal of Scientific Instrument, 2015, 36(11): 2451－2457. (in Chinese with English abstract)

[31]王蒙，戴亚平. 基于FAST角点检测的局部鲁棒特征[J]. 北京理工大学学报，2013，33(10)：1045－1050.Wang Meng, Dai Yaping. Local robust feature based on FAST corner detection[J]. Transactions of Beijing Institute of Technology, 2013, 33(10): 1045－1050. (in Chinese with English abstract)

[32]赵钦君，赵东标，韦虎. Harris-SIFT算法及其在双目立体视觉中的应用[J]. 电子科技大学学报，2010，39(4)：546－550.Zhao Qinjun, Zhao Dongbiao, Wei Hu. Harris-SIFT algorithm and its application in binocular stereo vision [J]. Journal of University of Electronic Science and Technology of China,2010, 39(4): 546－550. (in Chinese with English abstract)

Mobile robot localization algorithm by combining visual odometry with natural landmarks

Wang Zhi, Zhu Shiqiang※, Li Yuehua, Zhang Xuequn
(State Key Laboratory of Fluid Power and Mechatronic Systems, Zhejiang University, Hangzhou,310027,China)

Localization is crucial for the mobile robot system. Various localization methods have been proposed, such as light detection and ranging (LIDAR), ultrasonic method, WIFI, ultra wideband (UWB), vision odometry, and so on. The vision-based localization method has been intensively researched recently. Many researchers focus on the visual odometry,such as the monocular odometry and the stereo odometry. But the most obvious weakness of the visual odometry is the cumulative error, which will increase as the increasing of the displacement. To solve this problem, the artificial landmarks are adopted for absolute localization. But it needs previous setting and calibration. According to these observations, a mobile robot localization algorithm based on monocular vision and natural landmarks was proposed. The ordinary camera and omnidirectional camera are adopted in this method. The ordinary camera is used to extract the features from the ceiling for visual odometry, and the omnidirectional camera is used to extract the features from the environment and ceiling; then a natural landmark base is built using the Harris-SIFT (Scale-invariant feature transform) method. If there are not natural landmarks, the localization is implemented according to the change of the feature point between 2 neighboring images. If there exist natural landmarks, they will be used to localize the robot. During the visual odometry process, the feature point is extracted from the ceiling image and then tracked the feature point through the whole trajectory. The rotation angle and translation matrix can be determined by the coordinates of the feature point in the image and the world coordinate system. If the feature point is far away from the image center, it should be reselected so that it is close to the image center. The local search method for feature point is adopted to accelerate the localization. The search of feature point is only implemented near the feature point in the last frame. If there exist natural landmarks, the orientation filter is used to estimate the orientation angles. In this filter, the indirect Kalman filter is adopted, which uses the orientation error space as the state of the filter.Moreover, the orientation filter fuses the data of visual odometry and the inertial measurement unit to calculate the orientation angles. Then, the Harris-SIFT algorithm is used to determine the feature point for localization. To fully remove the outliers, the cluster analysis is implemented to cluster the matching points. And then the optimal matching is used to determine the feature point. The localization is implemented considering the orientation angles and the feature point obtained above. This algorithm has both of the advantages of the real-time performance of relative localization and the accuracy of absolute localization. The indoor experiment was implemented. The average errors of the proposed method inxaxis andyaxis were 38.66 and 31.54 mm,respectively. Compared with the visual odometry, the errors were decreased by 32.53% and 68.68%, respectively. As to the computational efficiency, the runtime of the localization based on only natural landmarks was almost 6 times higher than that of the proposed method. Thus, the proposed method can achieve real-time performance. The indoor experimental results demonstrate that compared with the visual odometry, the proposed method is much more accurate, and has a better extendibility.

computer vision; robots; image processing; visual localization; natural landmarks

10.11975/j.issn.1002-6819.2017.10.009

TP391.4

A

1002-6819(2017)-10-0070-08

2016-11-24

2017-04-15

国家自然科学基金创新群体基金（51521064）；杭州市创新链产业链重大科技创新项目（20132111A04）；杭州市重大科技创新项目（20142013A56）

王 志，男，博士生，主要从事双目立体视觉方面的研究。杭州浙江大学流体动力与机电系统国家重点实验室，310027。

Email：11325067@zju.edu.cn

※通信作者：朱世强，男，浙江义乌人，教授，博士，浙江省机器人产业技术联盟理事长，中国自动化学会机器人专委会委员，主要从事机器人技术研究。杭州 浙江大学机械工程学院，310027。Email：sqzhu@zju.edu.cn。