CN102087530B - 基于手绘地图和路径的移动机器人视觉导航方法 - Google Patents

Abstract

一种基于手绘地图和路径的移动机器人视觉导航方法，首先根据较小偏差的原则提取运行路径中的关键引导点，将原始路径分成多段；然后，在各段运行过程中，对预先绘制环境地图中对应的参考图像以及机器人摄像头实时采集到的信息进行匹配，估计当前视野中最可能存在的图像，并利用SURF算法检测图像的特征，依靠KD-TREE方法快速求得匹配点，采用RANSAC算法求解参考图像与实时图像的投影变换矩阵，进而得到参考图像在实时图像中的位置，并融合里程计数据，得到机器人的参考位置。再后，根据获得的机器人参考位置，计算下一段的运行方向，直至运动到最后一个参考点。该机器人不需要精确的环境地图及精确运行路径就能运行到指定区域，并能避开动态障碍物。

Description

基于手绘地图和路径的移动机器人视觉导航方法

技术领域

本发明涉及的智能机器人导航领域，通过手绘地图的人机交互方式指导机器人利用视觉等传感器在动态未知环境中自主导航，对促使服务机器人早日走进人类家庭具有重要意义。

背景技术

《环球》杂志报道了(http://tekbots.eefocus.com/article/10-01/1688061264400769.html)比尔·盖茨曾经在《科学美国人》上发表文章，谈到机器人产业的未来。在他看来，机器人产业只要能发展到临界点，就可能会彻底改变这个世界，就是未来的电脑产业。印裔英国学者古普塔拉在他的文章《2020年，日本机器人统治世界》曾指出，到2020年，日本将是全世界机器人领域当之无愧的霸主；另外韩国三星经济研究所也曾经预测，到2020年，世界机器人市场规模将达到1.4万亿美元，韩国信息与通信部甚至曾订出2020年每家都有一个机器人的惊人目标。随着家庭机器人走进人类家庭越来越迫切，当机器人面对新环境，即具有个性化、差异化室内装修的家居环境，“脑海”是一片空白，也就是说，机器人面向一个未知家居环境，由于室内的物品的摆放也不是固定不变的，或者室内人员的任意走动，导致该环境也是动态的。对于非机器人专家，或者一般的家庭成员(机器人使用新手)来说，想轻松地操作机器人学习周围的环境，传统的依靠精确地图的绘制指导机器人导航，由于机器人定位精度的固有局限性，随着环境的复杂程度的提高，越来越显得力不从心。在这种情况下，如何探索一种直接简单的交互导航方式非常关键。

自从上个世纪60年代以来，移动机器人的导航研究一直是各国学者研究的重要领域。随着计算机处理水平的不断提高，视觉导航方法由于其自主性、廉价性和可靠性成为导航策略领域的研究热点，逐步成为机器人导航过程中的一种重要的传感器。对于移动机器人的视觉导航研究，DeSouza“Vision for mobile robot navigation：a survey”(DeSouza G.N.，Kak，A.C.IEEE Trans.Pattern Anal.Mach.Intell.，2002，24(2)：237-267)和Bonin-Font“Visual Navigation for Mobile Robots：A Survey”(Bonin-Font F.，Ortiz A.，and OliverG.J.Intell.Robotics Syst.2008，53，(3)：263-296.)等人对过去几十年来的发展状况进行了总结：导航的目的是使机器人到达指定的地点，因而大多数导航过程都需要相应的环境地图来引导机器人。目前，米制地图、拓扑地图以及混合地图是应用最为广泛的地图表示方式。对于米制地图，它表示环境的精确坐标信息，因而当环境比较大时对于计算机的存储能力以及环境的测量过程都是一个考验；对于拓扑地图，它表示环境中关键点的连通信息，对于环境的实际大小没有明确的表示，因而对于环境的描述并不是很完善；而将米制地图和拓扑地图综合起来而得到的混合地图则能够将环境在整体上用拓扑地图表示，在感兴趣的局部地区构建米制地图，从而丰富了环境信息，然而真正在导航过程中，机器人在拓扑节点之间的运行却很容易受到外界干扰的影响。

假想这样一种情形，当别人向我们问路时，我们通常会这样说：沿着某条路往前走，走大概多远或到达某个目标前，转向某个方向，继续往某个地方走，绕过某个目标，再走多远就到达了目的地，从开始到目的地的距离大概有多少等等。这就是人类在陌生环境下问路导航方式。Tversky等人“How Space Structures Language，in SpatialCognition：An Interdisciplinary Approach to Representing and Processing SpatialKnowledge”(B.Tversky，and P.Lee，in Proceedings of the Spatial Cognition：AnInterdisciplinary Approach to Representing and Processing Spatial Knowledge，Berlin，1998：157-176.)，“What do Sketches Say about Thinking？”(in Proceedings of the AAAISpring Symposium，Sketch Understanding Workshop，Stanford University，2002：148-151.)从人类认知学的角度出发，分析了手绘地图在人类导航过程中的作用，并指出路线图和路线方向对人类导航的关键作用，以及手绘地图与实际地图的偏差程度对导航结果带来的影响。在考虑前面介绍的其它地图表示方式不足的基础上，借鉴这种仿人导航方式，决定采用手工绘制地图的方式来指导机器人导航，该手绘地图的基本元素包括环境中的各个实体(用实际图像或实体符号表示)、机器人、运行路径和起始点至目标点的距离。人通过分析机器人所要运行的环境，将环境中的关键目标、机器人的位置和方向在地图上标示出，接着绘制一条机器人由起点到达目标终点区域的路径，并标注概略距离，最终完成地图的绘制。

基于手绘地图的移动机器人导航目前研究的还很少。Kawamura等人在文献“TowardPerception-Based Navigation Using EgoSphere[”(Kawamura K.，Peters II R.A.I，andWilkes D.M.，etc.in Proceedings for SPIE Conference，Boston，MA，October，2001.)，“Toward Egocentric Navigation”(Kawamura K.，Koku A.B.and Wilkes D.M.etc.International Journal of Robotics and Automation，2002，17(4)：135-145.)中提出了一种基于Egosphere的导航方法，这种方法所依靠的地图也是一种不精确的地图，可以通过手工在地图中的大致位置上绘制相应的路标，而后给机器人提供一个目标位置以及由几个关键点组成的路径，这样机器人便可以通过传感器感知到的实时状态SES(Sensory EgoSphere)与关键点处所应感知到的状态LES(Landmark EgoSphere)进行比较，从而指导机器人依次通过各个关键点。然而，这种方法需要机器人具有很强的感知能力，若机器人不能在任意时刻感知到两个以上的路标，则导航很容易失去控制。Chronics和Skubic等人在基于手绘地图的导航方面做了大量的工作“Extracting navigation states from a hand-drawn map”(Skubic M.，Matsakis P.，Forrester B.，and Chronis G.in Proceedings of the IEEEInternational Conference on Robotics and Automation(ICRA)，Seoul，Korea，，vol.1，2001：259-264.)，“Generating Multi-Level Linguistic Spatial Descriptions from RangeSensor Readings Using the Histogram of Forces”(Skubic M.，Matsakis P.，Chronis G.and J.Keller.Autonomous Robots，2003，14(1)：51-69)，“Qualitative Analysis ofSketched Route Maps：Translating a Sketch into Linguistic Descriptions”(Skubic M.，Blisard S.，Bailey C.，etc.IEEE Transactions on Systems，Man and Cybernetics，2004，34(2)：1275-1282.)，此种导航方法仅仅局限于声纳作为唯一的传感器，其实现过程主要分为三个步骤：手工绘制地图和路径，从所绘地图中提取关键点(QLS)，实时检测是否匹配到相应的QLS；仿真和实际实验表明，在简单环境下该方法能够达到理想的效果；然而，受困于其传感器的检测能力，在复杂的环境下，此方法在实时匹配过程中很容易出现误匹配的情形，也就可能导致导航的失败。

本发明提出的基于手绘地图的导航方法，该手绘地图(路标的概略位置和路标的图像信息)、路径(包括起点、终点及路线图)和机器人的起始点(包括粗略位置和方向)是三个必要的先验人机交互信息，利用机器人自带单目摄像机作为主要的传感器来引导机器人的运行，利用声纳来辅助机器人进行避障，并融合里程计信息来粗略定位，最终在它们的相互协作下完成导航任务。

发明内容

技术问题：本发明主要提出了一种基于手绘地图和路径的移动机器人视觉导航方法。该方法首先根据较小偏差的原则提取运行路径中的关键引导点，将原始路径分成多段；然后，在各段运行过程中，对预先绘制环境地图中对应的参考图像以及机器人摄像头实时采集到的信息进行匹配，这里提出预测估计的方法估计当前视野中最可能存在的图像，以加速图像的匹配过程，并利用SURF(Speed Up Robust Features)算法检测图像的特征，依靠KD-TREE方法快速求得匹配点，采用RANSAC(RANdom SAmple Consensus)算法求解参考图像与实时图像的投影变换矩阵H，进而得到参考图像在实时图像中的位置，并融合里程计数据，得到机器人的参考位置。再后，根据获得的机器人参考位置，计算下一段的运行方向，直至运动到最后一个参考点。最后，通过一系列的实验，充分验证了本发明在机器人不需要精确的环境地图及精确运行路径就能运行到指定区域，并能有效地实时避开动态障碍物。

技术方案：本发明的基于手绘地图和路径的移动机器人视觉导航方法包括：手绘地图的绘制与表示、基于预测估计的机器人导航，

一、手绘地图的绘制与表示

将真实地图M与不精确的手绘地图建立有效的映射关系，

其手绘地图表示为m{L^％，P^％，R^％}，其中

表示路标(尺寸，位置)在手绘地图中的概略位置，即存在映射关系路标(尺寸，位置)a L^％；

表示机器人的初始概略位姿，这里包含路径的起始点S，终点D，以及路径线路图，主要从所绘路径的延伸趋势出发，将原始路径分成几段，每段都有一个关键引导点，将机器人引导到关键引导点附近，然后向着下一个关键引导点运行；为了便于机器人的控制，关键引导点间的运动采用直线运行的方式，在原始路径上提取关键引导点的较小偏差原则是：既能反映出机器人原有的运动趋势，又能减少机器人整体的旋转频率，即选取的关键引导点的数目尽量少；

二、基于预测估计的机器人导航

1)预测估计方法：两个黑色节点表示此时的关键引导点和下一个关键引导点，设机器人Robot已经处在关键引导点N_this并且朝向两关键引导点N_this和N_next之间连线

的方向，关键引导点N_this和N_next线段之间的两个灰色节点N_0.5和N_0.75分别表示在矢量

方向上与关键引导点N_this相距在0.5Dist(N_this，N_next)和0.75Dist(N_this，N_next)的位置，其中Dist(N_this，N_next)表示两点N_this和N_next之间的距离；目标1至目标4是点N_next周围与其相距在一定摄像机视野范围内的环境中的目标，d₁至d₄以及α₁至α₄分别表示各个目标与点N_next的距离以及各个目标与机器人运行方向

的夹角，提出了两个约束函数

和

它们分别表示目标与N_next的距离，以及与矢量

方向偏离程度对其作为参考目标的影响，D表示原始图像的平均拍摄距离，α表示参考目标与矢量

的偏离角，d表示参考目标与关键引导点N_next的距离，作为每个参考目标i的综合可能性程度F(i)，可通过下式计算：F(i)＝f₁(d_i)gf₂(α_i)，根据经验，若综合可能性程度的最大值

则认为点N_next附近不存在参考目标，否则，使F(i)取最大值的目标i可以作为参考目标；若存在多个目标都能使F(i)取得最大值，则选择这些目标中α最小的做为参考目标；

2)比例尺更新：设更新前地图的比例尺是Ruler_old，该段运行开始的关键引导点位置为L₁，结束点位置为L₂，机器人根据图像信息定位结束点在地图上的位置为L′₂，则更新后的比例尺Ruler_new利用下面的函数关系式进行计算：

$\mathrm{Rule}{r}_{\mathrm{new}}=\left\{\begin{array}{cc}\frac{\mathrm{Dist}(L_1,L_2)}{\mathrm{Dist}(L_1,L_2^{\′})}\mathrm{gRule}{r}_{\mathrm{old}}& ,\mathrm{RC}\\ \mathrm{Rule}{r}_{\mathrm{old}}& ,\mathrm{other}\end{array}\right.$

其中，Dist(.)表示两点间的距离，RC表示比例更新条件，根据经验这里设为

0.33＜Dist(L₁，L₂)/Dist(L₁，L′₂)＜3；

3)无约束导航，可以归纳以下3个步骤：

a)按照地图比例尺计算本关键引导点与下一个关键引导点间的距离，并据此确定在这两个关键点之间的运行模式。

b)按照a)中的模式进行运行，并按照预测估计的方法在必要的时候旋转摄像头寻找或跟踪参考目标。

c)机器人运行到下一个关键引导点附近后，根据图像信息或里程计信息进行定位，而后更新此关键引导点的位置以及地图比例尺，最后返回到a)中继续下一阶段的运行，直到运行到最后一个关键引导点。

4)避障导航

当机器人在行进的过程中遇到静态或者动态障碍物时，为了有效地避开环境中的静态或动态障碍物，并能在避障的过程中同时进行基于视觉的导航，机器人能够对避障前的状态进行记忆，避障之后，机器人返回到避障前的状态继续运行或者是进入一个新的状态。

有益效果：本发明方法使机器人不需要精确的环境地图及精确运行路径，以“仿人问路”的方式就能运行到指定区域，并能实时避开动态障碍物，当改变参考物的位置和数量、手绘路径等，导航依然能够顺利进行，充分体现了该方法具有简单、灵活、高效和鲁棒性强等优点，甚至便于扩展到人与机器之间的人机自然语言交互方式，对机器人走进家庭、和谐友好地服务人类具有重要的意义。

附图说明

图1绘制的路径及提取的关键引导点，

图2候选点选取流程图，

图3关键引导点选取流程图，

图4参考目标的预测估计，

图5与距离有关的约束函数，

图6与偏离方向有关的约束函数，

图7.机器人无约束导航流程图，

图8.机器人动态避障流程图。

具体实施方式

1手绘地图的绘制与表示

假设实际环境地图为M，

这里的路标(尺寸，位置)表示导航设置的关键路标；静态障碍物(尺寸，位置)表示在较长的时间段内静止不动的物体，由于其特征不是很明显，因此不能用作导航的参考物，但机器人在行进过程中，考虑到避障，必须避开这些静态障碍；动态障碍物(尺寸，位置)表示在机器人行进的过程中，环境中物体的位置在不停地变动；任务区(目标物，位置，范围)表示目标或者任务作业区域。移动机器人(尺寸，位置)的初始位姿。

手绘地图的绘制比较简单，打开交互绘制界面，由于预先在系统中保存了环境中关键路标的图像信息，可以用鼠标浏览图像库找到它们，并根据它们在实际环境中所处的大体位置，随意拖至绘图面板中对应的大致位置；根据机器人在实际地图中的概略位置和方向，在手绘地图中对应位置绘制它，并同时确定路径的起点，然后绘制路径和目标点。其手绘地图表示为m{L^％，P^％，R^％}，其中

表示路标(尺寸，位置)在手绘地图中的概略位置，即存在映射关系路标(尺寸，位置)a L^％；这里包含路径的起始点S，终点D，以及路径线路图，该路径图不是机器人走的实际或者真实路径，且该路径具有随意性、灵活性、不精确性，而只是指引移动机器人沿该路径的大致趋势行走。

表示机器人的初始概略位姿。手绘地图m{L^％，P^％，R^％}与实际环境地图之间存在“松散”映射关系。这是因为环境的精确大小无法获知，地图也没有精确的比例尺，所以手工绘制的路径无法表示机器人要走的精确路线。另一方面，绘制的路径对机器人起到的只是指引作用，机器人导航的最终目的是到达目标区域，因此机器人没有必要完全按照指定路径运行。于是本发明从所绘路径的延伸趋势出发，将原始路径分成几段，每段都有一个关键引导点，将机器人引导到关键引导点附近，然后向着下一个关键引导点运行。为了便于机器人的控制，关键引导点间的运动采用直线运行的方式，这样可以有效地避免机器人频繁旋转所带来的累积误差。在原始路径上提取关键引导点的较小偏差原则是：既能反映出机器人原有的运动趋势，又能减少机器人整体的旋转频率，即选取的关键引导点的数目尽量少。

关键引导点的提取过程如图1所示，图中的曲线表示所绘制的原始路径，最小的圆圈表示曲线的数字化表示，次小的圆圈表示关键引导点的候选点，最大的圆圈表示所提取的关键引导点，虚线表示机器人的大致引导方向，它可以与原始的路径进行比较。无论是候选点还是关键引导点，都是从原始的用于表示曲线的离散数字点中提取出来的。具体的提取过程如下：

(1)候选点的提取。从起点开始，沿着路径的方向依次检测各个离散数字点，设置角度变化阈值为M，候选点最小和最大距离阈值分别为D_min和D_max。具体的流程图如图5所示。

根据经验选取M为20度，D_min为1DP，D_max为8DP。根据以上参数，按照图2流程图计算，就可以得到如图1所示的次小圆圈所示的候选点。

(2)关键引导点的提取。如图3所示为从候选点中得到关键引导点的流程图。其中，T_i表示第i个候选点，T表示候选点的总数，i_b和i_e表示临时的开始的检测点和结束检测点，H表示两个关键引导点之间的最大候选点个数，

表示第i_b和第i_e个候选点之间的直线，表示第i_b和第i_e个候选点之间的所有原始离散点p至

的距离的最大值，DP表示原始曲线上任意两个相邻离散点之间的像素距离，α表示确定关键引导点的阈值。

根据试验经验，选取H为4DP，α为1.0。

通过(1)(2)两个步骤，就可以从原始路径上获得关键引导点。

2基于视觉的机器人粗定位

为了使机器人在导航行进的过程中，根据视觉获取的信息，粗略的定位机器人。这里给出两个必要的假设：1)原始图像的大致拍摄距离已知，且环境特征较丰富；2)原始图像里的环境信息在自身垂直平面内变化很小(即环境中具体物体在垂直方向上的高度变化不大)。

2.1近似距离的获取

根据针孔模型如图5所示，空间中的任一点由摄像机坐标系向图像坐标系的转换过程都符合中心射影或透视投影理论。其中O_cX_cY_cZ_c和O_iX_iY_i分别表示摄像机坐标系和图像坐标系，O_cX_cY_c平面和O_iX_iY_i平面是相互平行的，设直线O_cX_c和O_cY_c分别表示摄像机的垂直轴线和水平轴线，P_c、Q_c为摄像机坐标系下的空间点并且P_cQ_c平行于摄像机的垂直轴线，P′_cQ′_c为P_cQ_c沿着Z_cO_c方向平移一段距离得到的空间线段，P_i和P′_i分别为P_c和P′_c在图像坐标系下的投影，Q_i和Q′_i分别为Q_c和Q′_c在图像坐标系下的投影，f表示摄像机的焦距，为图像坐标系中心与光心(摄像机坐标系中心)之间的距离。

由图5分析可知：

x_i/x_c＝m_i/m_c＝f/z_c                   (1)

x′_i/x′_c＝m′_i/m′_c＝f/z′_c         (2)

在摄像机焦距f保持不变的前提下，由(1)和(2)可以得到：

z′_c/z_c＝[(x′_c-m′_c)/(x_c-m_c)].[(x_i-m_i)/(x′_i-m′_i)]     (3)

根据平行关系，x′_c-m′_c＝x_c-m_c，因此可以得到z′_c/z_c＝(x_i-m_i)/(x′_i-m′_i)。

设P_cQ_c和P′_c′Q′_c为原始图像与导航过程中实时图像里对于某个环境的同一对应线段。根据上面的假设(2)可以得到，x′_c-m′_c≈x_c-m_c，因此同样可以得到

z′_c/z_c≈(x_i-m_i)/(x′_i-m′_i)                             (4)

将上面的同一对应线段推广到同一环境信息，则可以得到下面的结论：

$d_r\≈\frac{p_1}{p_2}{d}_{o---\left(5\right)}$

这里d_r表示实时获取的目标与摄像头之间的距离；d_o表示原始目标与摄像头之间的距离；p₁表示目标的原始图像在距摄像机d_o时的像素高度；p₂表示目标的原始图像在距离d_r时的像素高度，因此，只需要得到实时图像在原始图像中的大致位置，根据原始图像与摄像头的距离信息，就可以求出摄像机与实时图像的距离。

2.2图像匹配算法

由于Speed Up Robust Features(SURF)算法实际上是对SIFT算法的改进。它能够继承SIFT算法对于图像的亮度、视角、仿射变换、尺寸变化等所具有的鲁棒性特性，又能极大地提高图像的检测速度，解决了SIFT方法在图像匹配应用中所面临的实时性难题“Distinctive imagefeatures from scale-invariant keypoints”(Lowe，D.G..International Journal of Computer Vision，2004，60(2)：91-110.)，“SURF：speed up robust features”(Bay H.，Tuvtellars T.，Gool，L.V.inProceedings of the European Conference on Computer Vision，2006：404-417.)。利用SURF算子求取各自的特征点，每个SURF特征都是用128维的向量表示的，按照KD-TREE“基于特征点的景象匹配技术研究”(徐秀云，南京理工大学硕士学位论文，2009.)的方法搜索原始图像与实时图像中每个特征点间的欧氏距离的最小值和次小值，并按照最小值和次小值的比值小于阈值T(这里取0.6)的原则，即可求出两图像中的对应点。令点P(x，y)为原始图像中的任一特征点，P′(x′，y′)为其在实时图像中的对应点，P和P′存在一种投影变换关系，它描述了两幅图像之间的对应关系。我们可以用3×3的矩阵H描述该对应关系，H也称为投影变换矩阵(单应性矩阵)。H与P、P′的具体关系可以用下式描述：

$\left(\begin{array}{c}{x}^{\′}\\ y^{\′}\\ 1\end{array}\right)=H\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& h_{33}\end{array}\right]\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)---\left(6\right)$

根据原始图像和实时图像的对应点，按照RANSAC算法“Random sample consensus：Aparadigm for model fitting with applications to image analysis and automated cartography”(FISCHLER M.，BOLLES R..CACM，1981，24(6)：381-395.)，则可能得到投影变换矩阵H，若得不到H，则说明两图像不具有匹配关系，原来的对应点大都是不可靠点，反之，则可根据求得的矩阵H进一步判别对应关系的有效性。为了验证有效性，假设将原始图像的四个顶点A、B、C、D按照式(5)投影到实时图像中，就可以得到对应点A′、B′、C′、D′。根据本发明给出的假设2)，可以容易得出原始图像在实时图像中的投影位置应该处在一个大致的矩形框内，并且该矩形框与原始图像的大小以及位置不存在巨大的差异，因此可以通过投影得到的矩形框的信息来进一步判定H的正确性。设原始图像的宽和高分别是W_S和H_S，判别原则如下：

其中，A′_x、A′_y分别表示A′的横坐标和纵坐标，其他点依此类推，Angle(.)表示两向量的夹角，|.|表示向量的长度。对于按照H所求得的点A′、B′、C′、D′，若满足式(7)，则认为这四点所描述的轮廓就是原始图像在实时图像中的位置，然后，通过式(8)计算原始图像在实时图像中的高度为p₂：

$p_2=\frac{1}{2}|D_y^{\′}+C_y^{\′}-A_y^{\′}-B_y^{\′}|---\left(8\right)$

已知p₁和d_o，由式(5)，很容易计算出实时图像与摄像机的大致距离d_r。

3基于预测估计的机器人导航

本发明所提出的导航方法，对于机器人的传感器要求是：带有里程计设备、声纳设备及车载可水平旋转摄像头。在导航过程中，图像信息用于粗略地描述机器人与参考目标的相对位置，而里程计信息是在图像信息不够充分或不需要图像处理的阶段，用于辅助机器人运行的。

3.1预测估计方法的提出

本发明提出了一种预测估计的方法，该方法具体表现在两个方面：

(1)预测摄像头视野内图像是否需要处理；若当前视野内的图像不需要处理，则可以省略(a)图像的特征提取，(b)实时图像与原始图像的匹配。

(2)当视野内出现需要处理的图像时，预测它最可能是哪类图像。若预测到实时图像属于某种图像，则可以缩小待匹配图像的范围，减小待匹配图像的数目，因而可以缩短图像处理过程图像匹配的运算时间。因此，这种预测估计的方法可以有效地提高机器人的实时控制性能，在探测目标时具有主动性，而不是被动的，所以这种预测估计的方法也可以允许地图中出现两个或两个以上的相同目标，这是因为它能够事先确定要处理的是哪一个目标。下面详细介绍这种方法：

图4中，两个黑色节点表示此时的关键引导点和下一个关键引导点，假设机器人Robot已经处在关键引导点N_this并且朝向矢量

的方向，两个灰色节点N_0.5和N_0.75分别表示向量

上与N_this相距在0.5Dist(N_this，N_next)和0.75Dist(N_this，N_next)的位置。目标1至目标4是点N_next周围与其相距在一定摄像机视野范围内的环境中的目标，d₁至d₄以及α₁至α₄分别表示各个目标与点N_next的距离(通过像素距离以及地图比例尺可以计算得出)以及各个目标与机器人运行方向

的夹角。经过分析，作为参考目标与该目标和关键引导点的距离以及该目标偏离机器人的运动方向的程度有关：距离太近或太远，受困于图像的识别能力，均不宜识别图像；方向偏离太多，也不便于机器人控制摄像头来识别图像。基于这种考虑，我们提出了两个约束函数

和

它们分别表示目标的距离与方向偏离对其作为参考目标的影响，建模如图5和6所示。图5中D表示原始图像的平均拍摄距离，α表示参考目标与矢量的偏离角，d表示参考目标与关键引导点N_next的距离。对于图4中的每个目标i，其可以作为参考目标的综合可能性程度F，可通过下式计算：

F(i)＝f₁(d_i).f₂(α_i)                                 (9)

根据经验，若综合可能性程度最大值

则认为点N_next附近不存在参考目标。否则，使F(i)取最大值的目标i可以作为参考目标，若存在多个目标都能使F(i)取得最大值，则选择这些目标中α最小的做为参考目标。例如，在图4中，通过计算可知目标2可以作为参考目标。

3.2更新比例尺

在手绘地图中，已经给出了各个目标的像素位置，以及起点至终点的大致直线距离，再根据起点至终点的像素距离，就可以得到手绘地图与实际环境的初始比例尺；机器人在关键引导点附近基于图像的定位与里程计信息进行比较，则可以更新地图的比例尺。

若更新后机器人在地图上的位置有了变化，则可以通过该变化更新地图的比例尺。

设更新前地图的比例尺是Ruler_old，该段运行开始的关键引导点位置为L₁，结束点位置为L₂，机器人根据图像信息定位结束点在地图上的位置为L′₂，则更新后的比例尺Ruler_new利用下面的函数关系式进行计算：

$\mathrm{Rule}{r}_{\mathrm{new}}=\left\{\begin{array}{cc}\frac{\mathrm{Dist}(L_1,L_2)}{\mathrm{Dist}(L_1,L_2)}\mathrm{gRule}{r}_{\mathrm{old}}& ,\mathrm{RC}\\ \mathrm{Rule}{r}_{\mathrm{old}}& ,\mathrm{other}\end{array}\right.$

其中，Dist(.)表示两点间的距离，RC表示比例更新条件，根据经验这里设为

0.33＜Dist(L₁，L₂)/Dist(L₁，L′₂)＜3；

3.3无约束导航

这里的无约束导航即在导航中无需考虑避障。机器人导航的过程可以归纳以下3个步骤：

a)按照地图比例尺计算本关键引导点与下一个关键引导点间的距离，并据此确定在这两个关键点之间的运行模式。

b)按照a)中的模式进行运行，并按照预测估计的方法在必要的时候旋转摄像头寻找或跟踪参考目标。

c)机器人运行到下一个关键引导点附近后，根据图像信息或里程计信息进行定位，而后更新此关键引导点的位置以及地图比例尺，最后返回到a)中继续下一阶段的运行，直到运行到最后一个关键引导点。

根据上面的步骤，可以将机器人的控制方式看成是分段进行的，即每个阶段的运行都是从当前的关键引导点开始，到下一个关键引导点结束；在结束之前，对机器人信息与地图信息进行更新，为下一个阶段的运行做准备。机器人无约束导航的流程图如图7所示，其中N_this表示当前的关键引导点，N_next表示下一个关键引导点，N_last表示最后一个关键引导点，Dist(N_this，N_next)表示N_this和N_next的距离，D_T用于表示判别机器人在两个关键引导点间运行模式的距离阈值，Ruler表示关于手绘地图像素与实际距离的比例尺，ImageFlag用于保存当前需要检测的图像类别，D_R-O表示该参考关键引导点与其参考目标之间的距离。根据图中的标号所示，下面对其具体解释如下：

1)是每段运行的开始，明确机器人的位置(也就是N_this)、方向R_θ，以及N_next，这一步就是要使R_θ转向的方向，为下一步机器人直行做准备。

2)确定运行的模式，Dist(N_this，N_next)是根据两关键点的像素位置及比例尺计算出来的，DT可以根据运行环境的大小适当选取，这里将其设置为1米，即当这两个关键点的距离小于1米时，直接按照里程计信息控制机器人进行直行，而不需要依靠视觉信息进行控制，增强了机器人的实时控制性能。

3)按照里程计信息到达这个关键引导点，若是在前面的运行中一直是直行，则不需要更新，但是需要考虑环境中存在障碍物的情形(3.3小节讨论)，在这种情况下，若机器人没有一直直行，则需要更新N_this的相关位置信息。

4)是参考目标的预测估计，详见3.1节。

5)为了便于模块8)、9)中对参考目标进行搜索跟踪，首先应将摄像头旋转至适当的位置，然后以此位置为基准进行目标搜索。如图4中的角度β，即可以作为摄像头相对于机器人运行方向的基准角度。

6)基于预测估计的方法，当机器人运行的距离小于0.5Dist(N_this，N_next)时，也即机器人运行到图4中的N_0.5之前时，对环境进行图像识别处理意义不是很大，因此在这一段只对机器人进行直行控制，而不进行图像处理，当机器人运行到N_0.5之后时再进行图像搜索与跟踪。

7)这一模块在ImageFlag中保存模块4)中所得到的参考目标的图像类别，而环境中的其他图像种类则不包含在内。

8)是第2节中所介绍的图像匹配部分，它的功能在于求解实时图像与参考目标图像的投影变换矩阵H以及检测H是否合理。如果不合理，则说明实时图像没有可用信息，在第9)步应进行目标的搜寻工作；如果合理，则根据原始参考图像在实时图像中的位置信息，于下一步调整摄像头方向以跟踪参考目标。

9)寻找目标过程，就是以5)中所计算出的角度β为基准，在一定的角度范围η内搜索目标的过程，即不停地旋转机器人上的摄像头，使其方向与机器人相对方向保持在[β-0.5η，β+0.5η]内，直到找到目标为止。

10)对前两个模块的处理结果进行分析的过程。如果8)、9)给出的结果若实时图像中没有原始参考图像的匹配结果，则说明没有找到目标，则程序转到13)步进行处理；如果8)、9)计算出原始参考图像在实时图像中的位置，则也不一定说明找到了目标，这是由于环境的复杂性对图像的检测可能存在干扰，例如，假设某个环境与参考目标图像具有相似的SURF特征，则当摄像头面对这个环境时也有可能计算出合理的投影变换矩阵H，进而在实时图像中得到相应的位置，干扰了我们的决策。因此，为了避免这种情况的出现，我们决定对前n次合理的检测结果不作分析，而只是将结果(如原始参考目标在实时图像中的高度)保存在一个队列中，当第n+1次检测的匹配图像时，则认为找到了目标，并同样将结果保存在队列中，以便用于11)步的处理。

11)这个模块是判断机器人是否到达本阶段的后一个关键引导点。由于在前一步中已经找到了目标，并且将相应的结果保存在队列中，因此，可以根据队列中保存的结果计算机器人与目标的距离。

当机器人找到目标并靠近目标的过程中，也可以根据上面已经计算出的机器人与目标的距离预测还需直行多少距离才能达到目标，这种方法用于机器人将要到达目标，突然遇到强烈的干扰而无法识别环境时，临时利用里程计信息辅助运行到目标的情形。

12)此时，机器人已经到达参考目标附近。这里需要在参考目标方向附近旋转摄像头，多次多方位扫描参考目标信息，以得到更精确的目标距离和方向。

13)判别是否有必要继续直行来寻找该阶段的后一个关键引导点。在Dist(N_this，N_next)前面存在一个系数1.2，这是为了增加目标检测的时间，这是根据地图的不精确性而给予的补偿处理。

14)此时，机器人的视野范围内所存在的信息不足以确定机器人的位置，需要在机器人周围360°范围内搜索信息来完成机器人的定位。同12)中的过程相似，将扫描范围设定在[-180°，180°]，并在此范围内只扫描一次。在扫描过程中，记录所扫描到的相应参考目标的高度信息与角度信息。扫描完成后，根据所扫描到的每个参考目标，利用中值滤波计算机器人的位置，而后，将各个参考目标所计算出来的机器人位置求取均值，以得到最终的机器人位置。

15)是利用预测估计的方法为14)中的扫描过程提供参考目标。当机器人开始360°扫描时，假设周围与其相距在一定摄像机视野范围内的环境中存在k个目标，第i个目标中心相对于机器人当前位置的角度为α_i。考虑到地图的不精确性，我们将第i个目标可能出现的方向设定在集合Ψ_i内，其中Ψ_i＝{x|α_i-60°＜x＜α_i+60°，x∈Z}。则在扫描过程中，对于每个扫描周期，首先清空ImageFlag，然后检测当前摄像头的方向φ与每个Ψ_i的关系，当φ∈Ψ_i时，将第i个目标所表示的图像类别保存入ImageFlag中，最后根据ImageFlag中的信息进行扫描过程。

16)更新ImageFlag的过程，即是清空ImageFlag中保存的信息的过程，为下一个阶段的运行提前做好初始化。

17)是每段运行的终点。根据12)或者14)中所定位出的机器人位置信息，根据3.2节比例映射关系，更新机器人在地图上的位置，并将此点更新为下一阶段的初始关键引导点。

18)此时，机器人已经到达最后一个关键引导点附近。由于在17)中可能更新了最后一个关键引导点的位置，因此，为了到达原始的最后一个关键引导点，需要在这一步根据更新前后的位置做补偿运行，使机器人到达原始终点。

3.4避障导航

为了有效地避开环境中的静态或动态障碍物，并能在避障的过程中同时进行基于视觉的导航，避障之后，机器人会返回到避障前的状态继续运行或者是进入一个新的状态。图8为避障导航的流程图，其避障步骤如下：

1.在机器人的直行过程中利用声纳实时检测前方是否存在障碍物，若不存在，则继续直行，否则，记录当前机器人的方向R_θ并转向步骤2。

2.根据声纳数据判断机器人旋转方向以避开障碍物，转向步骤3。

3.机器人旋转一定角度，根据声纳数据判断此时其前方是否存在障碍物，若不存在，则机器人停止旋转并前行一小段距离，转向步骤4，否则，继续转向步骤3。

4.机器人按照3.3节避障前无约束导航中的运行模式继续运行(例如，前行并调整摄像头检测跟踪参考目标等)，转向步骤5。

5.根据里程计信息与图像信息判断机器人是否到达下一个关键引导点，若是，则转向步骤8，否则，转向步骤6。

6.根据机器人上的声纳信息检测R_θ方向处是否存在障碍物，若是，则转向步骤7，否则，转向步骤9。

7.根据机器人上的声纳信息检测机器人前方是否存在障碍物，若是，则转向步骤2，否则，转向步骤4。

8.退出避障状态，转入无约束导航状态，并进行下一阶段的运行分析。

9.机器人转向R_θ方向，退出避障状态，转入无约束导航状态，并按照避障前的模式继续运行。

将避障导航模块结合无约束导航导航模块，机器人就可以在复杂的环境中有效地完成导航任务。

4.实验结果

本实验采用美国ActivMedia Robotics公司的Pioneer3-DX机器人作为实验平台来实现上面介绍的导航方法。采用C/S模式对机器人进行控制，其中将机器人作为服务器端，负责图像、声纳和自身里程计信息数据的采集与传输以及控制命令的接收及执行；将高性能PC机作为客户机端，负责人机交互和各种不同数据的计算分析，并发送控制指令，从而可以完成机器人的自主导航。

该实验所需的硬件设备包括：内置PC的机器人一台、PTZ摄像头、声纳传感器、图像采集卡、无线网卡、无线路由器、高性能PC机一台等。

客户机端的软件设计包括地图的绘制模块、机器人状态显示模块、通信模块、导航算法设计模块等。考虑到Visual C#环境对于界面开发的易用性以及Visual C++环境开发项目的通用性，这里考虑利用C#和C++语言在Visual Studio 2008开发环境下的混合编程。即基于Windows Forms窗体应用程序，利用C#语言开发机器人的运行环境地图，用于用户地图的绘制、参数的设置以及机器人运行状态的显示；基于Win32 Console应用程序，利用C++语言导航系统中其他模块的设计，例如通信、图像检测匹配、导航算法设计等部分。

在服务器端，软件设计部分主要包括通信模块、传感器信息获取模块、底层驱动模块等，由于ActivMedia Robotics公司提供了一套关于Pioneer3-DX上的传感器及电机的API接口软件ARIA，结合此软件利用C#/C++语言可以很容易地编写出导航系统的各个模块。

为了验证本发明提出的导航算法，本发明从五个不同角度进行了如下实验。

实验一：在手绘路径不变的情况下，改变参考物在地图上的位置，观察机器人的实际运行情况。实验结果分析如下：

当参考物在小圆圈范围内位置发生变动时，机器人总能正确的运行到目标位置附近。即使参考物位置发生较大变动，但只要保证各个相对参考物相对位置不变，机器人仍然能够正确快速的运行到目的地。

实验二：在手绘路径不变的情况下，改变参考物的数量，观察机器人的实际运行情况。每次改变参考物数量，实验结果分析如下：

当环境较小时，改变参考物的数量时，对机器人的导航影响不是很大，但环境足够大，且参考物的数量极少时，其获取图像的信息就会受到限制，只能依靠里程计定位与导航，因此其导航结果会受到影响。

实验三：在参考物不变的情况下，每次实验通过改变手绘路径，观察机器人的实际运行情况。实验结果分析如下：

当改变手绘路径时，机器人实际运行的路径并不是保持不动，而是发生相应改变，但它仍然能够有效的导航到目标区域。从而可以看出，手绘路径的不精确性，并不影响其导航的结果。

实验四：在八个参考物中，有三个参考物完全相同，通过它们之间位置变化，检测导航算法在不同方位对相同参考物的识别能力，观察机器人的实际运行情况。实验结果分析如下：

当环境中有多个相同的参考物，而手绘路径保持不变时，由于本发明提出了基于预测估计的图像处理方法，能够有效预测到相同参考物中的哪一个，因此其导航结果不受任何影响。实验五：在机器人运行过程中添加动态障碍物。实验结果分析如下：

当机器人在沿着手绘路径导航的过程中，任意动态的障碍物靠近它或者在其附近活动，阻碍其前行方向，机器人能够实时的避开它们，并依靠手绘路径的指导，有效地导航到目标区域。

实验结果总结如下：

1)无论在何种情况，机器人运行的真实路径的大体趋势是一直的，与手绘路径的走向相似，除了在动态避障的过程中，实际路径有点波动，但避开障碍物后，马上能够恢复手绘路径的趋势，因此说明手绘地图在导航中的关键作用。

2)通过改变参考物的位置和数量、手绘路径等，导航依然能够顺利进行，可以看出本发明提出的导航方法具有较强的鲁棒性。

3)由于考虑不精确信息，其应用可以扩展到自然语言，具有较强的灵活性和简单易操作性，便于人机交互。

4)可以看出本发明提出的导航方法以“仿人问路”的方式，在手绘路径的指导下能够有效的指导机器人导航。

Claims (2)

1.一种基于手绘地图和路径的移动机器人视觉导航方法，其特征在于该方法包括：手绘地图的绘制与表示、基于视觉的机器人粗定位、基于预测估计的机器人导航，

一.手绘地图的绘制与表示

将真实地图M与不精确的手绘地图建立有效的映射关系，

其手绘地图表示为 

其中 

表示路标(尺寸，位置)在手绘地图中的概略位置，即存在映射关系路标(尺寸，位置)a 

表示机器人的初始概略位姿， 

这里包含路径的起始点S，终点D，以及路径线路图，主要从所绘路径的延伸趋势出发，将原始路径分成几段，每段都有一个关键引导点，将机器人引导到关键引导点附近，然后向着下一个关键引导点运行；为了便于机器人的控制，关键引导点间的运动采用直线运行的方式，在原始路径上提取关键引导点的较小偏差原则是：既能反映出机器人原有的运动趋势，又能减少机器人整体的旋转频率，即选取的关键引导点的数目尽量少；

二.基于视觉的机器人粗定位

为了使机器人在导航行进的过程中，根据视觉获取的信息，粗略的定位机器人；这里给出两个必要的假设：1)原始图像的大致拍摄距离已知，且环境特征较丰富；2)原始图像里的环境信息在自身垂直平面内变化很小，即环境中具体物体在垂直方向上的高度变化不大；

2.1近似距离的获取

这里d_r表示实时获取的目标与摄像头之间的距离；d_o表示原始目标与摄像头之间的距离；p₁表示目标的原始图像在距摄像机d_o时的像素高度；p₂表示目标的原始图像在距离d_r时的像素高度；

三.基于预测估计的机器人导航

1)预测估计方法：两个黑色节点表示此时的关键引导点和下一个关键引导点，设机器人Robot已经处在关键引导点N_this并且朝向两关键引导点N_this和N_next之间连线 

的方向，关键引导点N_this和N_next线段之间的两个灰色节点N_0.5 和N_0.75分别表示在矢量 

方向上与关键引导点N_this相距在0.5Dist(N_this，N_next)和0.75Dist(N_this，N_next)的位置，其中Dist(N_this，N_next)表示两点N_this和N_next之间的距离；目标1至目标4是点N_next周围与其相距在一定摄像机视野范围内的环境中的目标，d₁至d₄以及α₁至α₄分别表示各个目标与点N_next的距离以及各个目标与机器人运行方向 

的夹角，提出了两个约束函数 

和

它们分别表示目标与N_next的距离，以及与矢量 

方向偏离程度对其作为参考目标的影响，D表示原始图像的平均拍摄距离，α表示参考目标与矢量 

的偏离角，d表示参考目标与关键引导点N_next的距离，作为每个参考目标i的综合可能性程度F(i)，可通过下式计算：F(i)＝f₁(d_i)gf₂(α_i)，根据经验，若综合可能性程度的最大值 

则认为点N_next附近不存在参考目标，否则，使F(i)取最大值的目标i可以作为参考目标；若存在多个目标都能使F(i)取得最大值，则选择这些目标中α最小的做为参考目标；

2)比例尺更新：设更新前地图的比例尺是Ruler_old，该段运行开始的关键引导点位置为L₁，结束点位置为L₂，机器人根据图像信息定位结束点在地图上的位置为L′₂，则更新后的比例尺Ruler_new利用下面的函数关系式进行计算：

其中，Dist(.)表示两点间的距离，RC表示比例更新条件，根据经验这里设为

0.33＜Dist(L₁，L₂)/Dist(L₁，L′₂)＜3； 

3)无约束导航，可以归纳以下3个步骤：

a)按照地图比例尺计算本关键引导点与下一个关键引导点间的距离，并据此确定在这两个关键点之间的运行模式，

b)按照a)中的模式进行运行，并按照预测估计的方法在必要的时候旋转摄像头寻找或跟踪参考目标，

c)机器人运行到下一个关键引导点附近后，根据图像信息或里程计信息进行定位，而后更新此关键引导点的位置以及地图比例尺，最后返回到a)中继续下一阶段的运行，直到运行到最后一个关键引导点；

4)避障导航

当机器人在行进的过程中遇到静态或者动态障碍物时，为了有效地避开环境中的静态或动态障碍物，并能在避障的过程中同时进行基于视觉的导航，机器人能够对避障前的状态进行记忆，避障之后，机器人返回到避障前的状态继续运行或者是进入一个新的状态。 

Images