CN108919810A - 基于视觉示教的移动机器人定位与导航*** - Google Patents

Abstract

本发明提供一种基于视觉示教的移动机器人定位与导航***，涉及移动机器人示教技术领域。该***包括工控机、相机、供电电源、车载电池、升压模块、底层运动控制单元、手柄，底层运动控制单元包括差分轮小车、嵌入式开发板、光电编解码器、直流减速电机、TTL转USB模块，通过教学和重复两个阶段，在教学阶段，车辆由手柄驱动，存储由任意序列的动作组成的路径，在重复阶段，使用里程计信息作为距离索引教学阶段所村图像信息，来匹配当前相机所采集的中图像信息，计算出车头的偏向角速度，将其叠加到所存速度中，从而补偿车辆转向，以确保它跟踪预定的路径。本发明无需相机反复标定，可自主学习和遍历任意形状的路径，重复地沿着预期路径自动行驶。

Description

基于视觉示教的移动机器人定位与导航***

技术领域

本发明涉及移动机器人示教技术领域，尤其涉及一种基于视觉示教的移动机器人定位与导航***。

背景技术

随着人力成本的提高和科学技术的发展，机器人，尤其是工业机器人，在工业自动化领域越来越广泛的应用。自动导航搬运车(Automated Guided Vehicle，以下简称AGV，)也被称作搬运机器人，是现代智能物流***中的重要环节，然而目前主流的AGV导引技术并不能使其在工作中从真正意义上实现完全自主。

导航作为核心技术之一，是AGV技术发展的最重要标志。AGV根据导航方式不同可以分为固定路径法和自由路径法两大类。

固定路径法以电磁导引为代表，主要特点是预先给AGV设定路径，AGV根据埋在路面下的电缆线产生电磁场调整自身的行驶方向。这种导引方式使用物理路径，环境表示简单，无需自动地图创建技术，定位时只需要通过传感器检测车体与物理路径之间的偏差，但固定轨迹的方式灵活性太低，不易维护，如果路径改变的话，需要开凿重新布线，成本太高。

自由路径法主要包括激光导航、毫米波雷达导航、惯性导航***、GPS***等。惯性导航***存在***误差；GPS定位***由于定位误差较大，难以适合定位精度要求很高的AGV。随着科技的进步，激光传感器、图像传感器逐步应用到AGV导航技术中。相较于摄像头等车载传感器，激光雷达具有高精度、高分辨率的优势，并已在很多自动驾驶试验车上广泛搭载。但这种技术也有其无法忽略的缺点——成本高。近年来，由于图像处理技术和计算机技术的发展，加上摄像头成本低的优点，基于视觉的导航技术受到广泛关注。相比较而言，目前以摄像头+软件”的视觉导航技术，工业上容易达到且成本更低。

不过与传统导航方式相比，视觉导航涉及面多，技术复杂。首先，视觉导航不存在物理路径，因此无法像传统导航方式那样直接检测车体与物理路径之间的偏差。因此在视觉导航中，AGV通过车载相机实时采集环境数据，通过将环境数据的处理结果与事先建立的环境地图进行对比获得自身在地图中的位姿估计，这种位姿估计即定位比传统导航方式中的直接偏差计算要复杂。其次，视觉导航中使用的地图具有明显的测度信息，需要进行精确绘制，所创建的环境地图精度影响AGV定位精度。

在未知环境中，由于环境感知传感器的探测范围和测量精度限制、环境中墙壁等物体对传感器探测的遮挡等问题，机器人无法通过一次测量创建全局环境地图。因此，机器人只能在不断地环境探索过程中获取足够的环境感知数据，才能完成全局环境地图的创建工作。对于机器人在环境中各个位置创建的局部地图而言，只有确切地知道机器人各位置的位姿才能将该局部地图转换为全局地图，即地图的创建依赖于机器人的位姿。然而现实情况是，机器人的位姿往往是通过环境地图得到的，即机器人的位姿依赖于环境地图。位姿和地图估计之间的这种相互依赖的关系给机器人在未知环境下的导航提出了新的课题，称之为“同时定位与地图构建”(Simultaneous Localization and Mapping，以下简称SLAM)问题。SLAM的最终目的是创建一致的环境地图，是否能够实现正确闭环是方法自身性能的一种体现；而基于闭环消除定位误差的地图创建方法，在通过航迹推测所估计的位姿误差较大的情况下，只能通过辅助方法建立约束条件，即在建立正确闭环的前提下，再通过优化方法校正机器人位姿和创建环境地图。因此slam***这样的实现需要构建全局一致的地图因此带来更高的计算成本。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于视觉示教的移动机器人定位与导航***，计算效率高，无需相机反复标定，可以快速地进行室内AGV的导航，非专业人员易上手、操作简单、使用方便，而且可以自主学习和遍历任意形状的路径，可以重复地沿着预期的路径自动行驶而无需人工参与。

为解决上述技术问题，本发明所采取的技术方案是：

一种基于视觉示教的移动机器人定位与导航***，包括：工控机、相机、供电电源、车载电池、升压模块、底层运动控制单元、手柄；

所述底层运动控制单元包括差分轮小车、嵌入式开发板、光电编解码器、直流减速电机、TTL转USB模块，用于实现小车平台的运动控制、显示小车状态信息以及编码器数据的读取与发送；所述差分轮小车和嵌入式开发板的电源端连接供电电源；所述相机通过螺丝固定在差分轮小车的上层平台，且视角朝向差分轮小车前进方向；所述光电编码器安装在差分轮小车的每个车轮上；直流减速电机安装在差分轮小车的每个车轮上，用来控制差分轮小车电机转速；

所述工控机通过USB接口与相机进行通讯，获取相机生成的图像信息；工控机还与升压模块连接，所述升压模块的电源端连接车载电池，用于将车载电池的12V输出电压升高到工控机所需的19V电压；工控机通过TTL转USB模块与嵌入式开发板连接，用于给嵌入式开发板发送控制指令，同时读取车轮光电编码器的数据，得出小车的里程计信息；

所述手柄与工控机连接，用于提供差分轮小车运动控制使能作用；

所述嵌入式开发板中存储有底层运动控制程序，包括数据收发程序和电机运动控制程序；数据收发程序基于SCIP2.0协议编写，用于与工控机传输速度信息和编码器数据；电机运动控制程序用于将速度信息解算成PWM波以控制各直流减速电机转速；

所述工控机中存储有基于linux操作***下的ROS(即机器人操作***)平台、采用C++语言编写的示教软件上层导航程序和PID控制程序，用于实现整个***的完整运行，所述示教软件上层导航程序包括教学阶段和重复阶段；在教学阶段，将机器人置于起始位置，由操作员操作手柄驱动机器人在前、后和自转运动的基础上移动任意路径，并记录路径点附近的图像信息、车辆速度信息及由里程计信息得出的路径点位置坐标，路径点附近的图像信息由相机实时拍摄，完成后，按工控机终端中的Ctrl-c键，停止录制，记录信息存储在工控机中；在重复阶段，所存的速度指令将再次回放，按住手柄使能键，机器人开始沿着教学阶段的路径自动驾驶，机器人将实时读取的相机图像信息与它在教学阶段保存的图像信息采用ORB算法进行匹配，并计算从读取的图像到记录的图像的转换，然后将此转换发送到PID控制程序进行误差补偿，计算出车头的偏向角速度和速度大小，将其叠加到所存速度中，从而补偿车辆转向和速度大小，以确保跟踪预定的路径，最终实现自动重复在教学阶段所教的路线；

所述底层运动控制程序用于接收示教软件上层导航程序发送的速度控制指令，借助差分轮小车的特性，通过手柄的操作实现机器人的前、后和自转运动。

进一步地，在教学阶段中，存储相机图像信息和路径点的时间戳应保持一致，且根据场景范围大小和目标点定位精度要求自定义间隔距离，每隔一段间隔距离记录一次图像信息。

进一步地，在教学阶段中所存路径点以文件形式存在工控机中，可重复使用，并且重复教学阶段可获得新的路径。

进一步地，在教学阶段中加载文件中的路径点和相机图像信息至内存中使用，判断所有的点是否都已经示教完，如果是，则示教结束；否则获取下一个参考路径点和相机图像信息，并执行后续操作。

进一步地，在重复阶段，若差分轮小车速度接近于零且时间超过五秒或偏离参考路径1m以上，停止继续示教并报错或报警。

进一步地，重复阶段的起点须在教学阶段起点的1m的距离范围内，且位姿朝向一致。

采用上述技术方案所产生的有益效果在于：本发明提供的一种基于视觉示教的移动机器人定位与导航***，计算效率高，无需相机反复标定，可以快速地进行室内AGV的导航，非专业人员易上手、操作简单、使用方便，而且可以自主学习和遍历任意形状的路径，在教学阶段，机器人是由一名人类操作者驾驶，机器人会存储它的速度、图像信息和里程计信息；在自主导航中，在二维甚至是三维空间中无需显式机器人定位，只需回放在教学阶段学习的速度，同时还能根据图像信息和由里程计信息生成的位置信息对速度大小和方向进行修正，因此，机器人可以重复地沿着预期的路径自动行驶而无需人工参与。

附图说明

图1为本发明实施例提供的基于视觉示教的移动机器人定位与导航***结构框图；

图2为本发明实施例提供的***软件框架示意图；

图3为本发明实施例提供的匹配算法流程图；

图4为本发明实施例提供的特征图像生成示意图；

图5为本发明实施例提供的FAST算法中的待处理图像示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

如图l所示，本实施例提供一种基于视觉示教的移动机器人定位与导航***，包括：工控机、相机、供电电源、车载电池、底层运动控制单元、手柄。

底层运动控制单元包括差分轮小车、嵌入式开发板、直流减速电机(带光电编码器)、TTL转USB模块，用于实现小车平台的运动控制、显示小车状态信息以及编码器数据的读取与发送。

本实施例中，嵌入式开发板的型号为STM32，工控机的型号为占美gk400，直流减速电机的型号为MD36行星减速电机(带500线光电编码器)，TTL转USB模块的型号是CH340G，相机的型号是华硕Xtion pro live，升压模块的型号为MKX-DC12V～19V，车载电池的型号为凯美威12V100A，供电电源的型号为SKYRC IMAX B6，手柄的型号为Sony PS3。

差分轮小车和嵌入式开发板STM32的电源端连接供电电源，由SKYRC IMAX B6供电。相机通过螺丝固定在差分轮小车的上层平台，且视角朝向差分轮小车前进方向。光电编码器安装在差分轮小车的直流减速电机上；直流减速电机安装在差分轮小车的每个车轮上，用来控制差分轮小车电机转速。

工控机占美gk400通过USB3.0接口与相机Xtion pro live进行通讯，获取相机生成的图像信息，确保了相机与工控机之间数据传输的稳定性、安全性和实时性，为工控机上运行的示教软件实时处理由相机传来的大量图像数据提供了有效支持。工控机占美gk400还与升压模块连接，然后由车载电池凯美威12V100A供电，具体通过车载电池12V端口进行供电，升压模块用于将车载电池的12V电压输出升高到工控机所需的19V电压。工控机占美gk400通过TTL转USB模块CH340G与嵌入式开发板STM32连接，用于给嵌入式开发板发送控制指令，同时读取车轮光电编码器的数据，得出小车的里程计信息。搭载Linux***的占美gk400工控机是整个***的大脑，它一方面通过TTL转USB模块CH340G给STM32发送控制指令，使其能够前、后和自转运动，另一方面读取车轮光电编码器的数据和相机的数据，实现两种传感器数据的融合。

手柄Sony PS3与工控机连接，用于提供差分轮小车运动控制使能作用。

嵌入式开发板STM32中存储有底层运动控制程序，包括数据收发程序和电机运动控制程序，其中数据收发程序基于SCIP2.0协议编写，负责与工控机的数据通信，电机运动控制程序负责解算工控机下发的速度指令，用来控制电机转速；

所述工控机中存储有基于linux操作***下的ROS(即机器人操作***)平台、采用C++语言编写的示教软件上层导航程序，用于实现整个***的完整运行，包括教学阶段和重复阶段。所述底层运动控制程序用于接收示教软件上层导航程序发送的速度控制指令，借助差分轮小车的特性，通过手柄的操作实现机器人的前、后和自转运动。本实施例中***软件框架如图2所示。

在教学阶段，将机器人置于起始位置，由操作员操作手柄驱动机器人在前、后和自转运动的基础上移动任意路径，并记录路径点附近的图像信息、车辆速度信息及由里程计信息得出的路径点位置坐标，路径点附近的图像信息由相机实时拍摄，完成后，在工控机Ubuntu***终端中按下Ctrl+c键，停止录制，所存路径点以文件形式存储在工控机中，可重复使用，并且重复教学阶段可获得新的路径。存储相机图像信息和路径点的时间戳应保持一致，且根据场景范围大小和目标点定位精度要求自定义间隔距离，每隔一段间隔距离记录一次图像信息。加载文件中的路径点和相机图像信息至内存中使用，判断所有的点是否都已经示教完，如果是，则示教结束；否则获取下一个参考路径点和相机图像信息，并执行后续操作。

在重复阶段，机器人起点须在教学阶段起点的1m的距离范围内，且位姿朝向一致，所存的速度指令将再次回放，按住手柄使能键(可自定义)，机器人开始沿着教学阶段的路径自动驾驶，机器人将实时读取的相机图像信息与它在教学阶段保存的图像信息采用ORB算法进行匹配，并计算从读取的图像到记录的图像的转换，然后将此转换发送到PID控制程序进行误差补偿，计算出车头的偏向角速度和速度大小，将其叠加到所存速度中，从而补偿车辆转向和速度大小，以确保跟踪预定的路径，最终实现自动重复在教学阶段所教的路线。若差分轮小车速度接近于零且时间超过五秒或偏离参考路径1m以上，停止继续示教并报错或报警。

ORB(全称是ORiented Brief)算法是一种新的角点检测与特征描述算法，其流程如图3所示，包括以下步骤：首先基于FAST算法实现关键点定位，基于Harris实现选择最好的关键点，进行尺度金字塔变换、中心与角度方向计算，提取二进制BRIEF描述子，进行低相关性像素块过滤，最后接受最终256描述子。

图像特征匹配是一个非常重要的中间步骤，它在表面重建、三维物体识别、相机定位等问题中有着极其重要的应用。生成的特征图像如图4所示。该算法对比与其他算法具有相似甚至更好的可重复性、鲁棒性，并且计算的更快。ORB算法是将FAST角点检测与BRIEF特征描述结合并进行了改进的图像特征匹配算法，提升了算法的执行效率，为算法在实时计算机视觉***中应用提供了可能。

其中FAST的提出者Rosten等将FAST角点定义为：若某像素与其周围邻域内足够多的像素点相差较大，则该像素可能是角点。FAST算法的步骤：

步骤1、如图5所示，在一幅待处理图像中取一个以像素p为中心、半径为3的圆，该圆上有16个像素点(p1、p2、...、p16)；

步骤2、定义一个阈值，计算p1、p9、p5、p13与中心p的像素差，若它们的绝对值有至少3个超过阈值，则当做候选角点，再进行下一步考察；否则，不可能是角点；

步骤3、若p是候选点，则计算p1～p16这16个点与中心p的像素差，若它们有至少连续9个超过阈值，则是角点；否则，不可能是角点；

步骤4、对图像进行非极大值抑制：计算特征点出的FAST得分值(即score值，也即s值)，判断以特征点p为中心的一个邻域(如3x3或5x5)内，若计算有多个特征点，则判断每个特征点的s值(16个点与中心差值的绝对值总和)，若p是邻域所有特征点中响应值最大的，则保留，否则，抑制；若邻域内只有一个特征点(即角点)，则保留；得分计算公式如下：

其中，V表示得分，t表示阈值。

上面的方法是FAST-9，当然FAST-10、FAST-11、FAST-12也是一样的，只是步骤3中，超过阈值的个数不一样。FAST算法实现起来简单，尤其是以速度快著称。

但该算法仅仅确定了特征点的位置，没有得到其他任何信息。在ORB算法中，依然采用FAST来检测特征点的位置，但算法进行了如下改动：(以FAST-9为例)

(1)假设在图像中要提取N个特征点，则降低FAST的阈值，使FAST算法检测到的特征点大于N；

(2)在特征点位置处，计算特征点的Harris响应值R，取前N个响应值大的点作为FAST特征点(Harris角点响应计算：Harris角点检测中的数学推导)；

(3)由于要解决BRIEF算法的旋转不变性，则需要计算特征点的主方向。

ORB算法中利用重心来计算，如下：

θ＝atan2(m₀₁，m₁₀)

其中，(x，y)是特征邻域内的点，atan2表示反正切，得到的θ值就是FAST特征点的主方向。

BRIEF算法是对已检测到的特征点进行描述，它是一种二进制编码的描述子，摈弃了利用区域灰度直方图描述特征点的传统方法，大大的加快了特征描述符建立的速度，同时也极大的降低了特征匹配的时间，是一种非常快速，很有潜力的算法。

由于BRIEF仅仅是特征描述子，所以事先要得到特征点的位置，可以利用FAST特征点检测算法或Harris角点检测算法或SIFT、SURF等算法检测特征点的位置。接下来在特征点邻域利用BRIEF算法建立特征描述符。

BRIEF算法具体步骤如下：

步骤1、为减少噪声干扰，先对图像进行高斯滤波，方差为2，高斯窗口为9x9；

步骤2、以特征点为中心，取S×S的邻域窗口，在窗口内随机选取一对(两个)点，比较二者像素的大小，进行如下二进制赋值：

其中，p(x)、p(y)分别是随机点x＝(u1，v1)、y＝(u2，v2)的像素值；

步骤3、在窗口中随机选取N对随机点，重复步骤2的二进制赋值，形成一个二进制编码，这个编码就是对特征点的描述，即特征描述子。一般N＝256。

以上便是BRIEF特征描述算法的步骤。

经过上面的特征提取算法，对于一幅图中的每一个特征点，都得到了一个256bit的二进制编码。接下来对有相似或重叠部分的两幅图像进行配准。

特征配对是利用汉明距离进行判决：

①两个特征编码对应bit位上相同元素的个数小于128的，一定不是配对的；

②一幅图上特征点与另一幅图上特征编码对应bit位上相同元素的个数最多的特征点配成一对。

该算法速度优势相当明显，但存在三个致命的缺点。针对尺度不变性，可以像SIFT算法一样，子尺度空间构造图像金字塔解决。

由此得到两幅图像中匹配的点对，并计算点对相应的X、Y坐标信息。因为相机面向小车前进方向且垂直于小车平台，将匹配的点对X做差并成一定系数后传给控制器以矫正小车航向，使小车前进方向相机看到的图像信息与教学阶段记录的图像信息吻合。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (6)

1.一种基于视觉示教的移动机器人定位与导航***，其特征在于：包括：工控机、相机、供电电源、车载电池、升压模块、底层运动控制单元、手柄；

所述底层运动控制单元包括差分轮小车、嵌入式开发板、光电编解码器、直流减速电机、TTL转USB模块，用于实现小车平台的运动控制、显示小车状态信息以及编码器数据的读取与发送；所述差分轮小车和嵌入式开发板的电源端连接供电电源；所述相机通过螺丝固定在差分轮小车的上层平台，且视角朝向差分轮小车前进方向；所述光电编码器安装在差分轮小车的每个车轮上；直流减速电机安装在差分轮小车的每个车轮上，用来控制差分轮小车电机转速；

所述工控机通过USB接口与相机进行通讯，获取相机生成的图像信息；工控机还与升压模块连接，所述升压模块的电源端连接车载电池，用于将车载电池的12V输出电压升高到工控机所需的19V电压；工控机通过TTL转USB模块与嵌入式开发板连接，用于给嵌入式开发板发送控制指令，同时读取车轮光电编码器的数据，得出小车的里程计信息；

所述手柄与工控机连接，用于提供差分轮小车运动控制使能作用；

所述嵌入式开发板中存储有底层运动控制程序，包括数据收发程序和电机运动控制程序；数据收发程序基于SCIP2.0协议编写，用于与工控机传输速度信息和编码器数据；电机运动控制程序用于将速度信息解算成PWM波以控制各直流减速电机转速；

所述工控机中存储有基于linux操作***下的ROS(即机器人操作***)平台、采用C++语言编写的示教软件上层导航程序和PID控制程序，用于实现整个***的完整运行，所述示教软件上层导航程序包括教学阶段和重复阶段；在教学阶段，将机器人置于起始位置，由操作员操作手柄驱动机器人在前、后和自转运动的基础上移动任意路径，并记录路径点附近的图像信息、车辆速度信息及由里程计信息得出的路径点位置坐标，路径点附近的图像信息由相机实时拍摄，完成后，按工控机终端中的Ctrl-c键，停止录制，记录信息存储在工控机中；在重复阶段，所存的速度指令将再次回放，按住手柄使能键，机器人开始沿着教学阶段的路径自动驾驶，机器人将实时读取的相机图像信息与它在教学阶段保存的图像信息采用ORB算法进行匹配，并计算从读取的图像到记录的图像的转换，然后将此转换发送到PID控制程序进行误差补偿，计算出车头的偏向角速度和速度大小，将其叠加到所存速度中，从而补偿车辆转向和速度大小，以确保跟踪预定的路径，最终实现自动重复在教学阶段所教的路线；

所述底层运动控制程序用于接收示教软件上层导航程序发送的速度控制指令，借助差分轮小车的特性，通过手柄的操作实现机器人的前、后和自转运动。

2.根据权利要求1所述的基于视觉示教的移动机器人定位与导航***，其特征在于：在所述教学阶段中，存储相机图像信息和路径点的时间戳应保持一致，且根据场景范围大小和目标点定位精度要求自定义间隔距离，每隔一段间隔距离记录一次图像信息。

3.根据权利要求1或2所述的基于视觉示教的移动机器人定位与导航***，其特征在于：在所述教学阶段中所存路径点以文件形式存在工控机中，可重复使用，并且重复教学阶段可获得新的路径。

4.根据权利要求3所述的基于视觉示教的移动机器人定位与导航***，其特征在于：在所述教学阶段中加载文件中的路径点和相机图像信息至内存中使用，判断所有的点是否都已经示教完，如果是，则示教结束；否则获取下一个参考路径点和相机图像信息，并执行后续操作。

5.根据权利要求1所述的基于视觉示教的移动机器人定位与导航***，其特征在于：在所述重复阶段，若差分轮小车速度接近于零且时间超过五秒或偏离参考路径1m以上，停止继续示教并报错或报警。

6.根据权利要求1或5所述的基于视觉示教的移动机器人定位与导航***，其特征在于：所述重复阶段的起点须在教学阶段起点的1m的距离范围内，且位姿朝向一致。