CN109483507A - 一种多个轮式机器人行走的室内视觉定位方法 - Google Patents

Abstract

本发明公开了一种多个轮式机器人行走的室内视觉定位方法，包括在室内周边设置与监控服务器连接的可见光摄像头，可见光摄像头视角覆盖机器人行走全部区域，监控服务器通过可见光摄像头获取机器人的行走轨迹，向机器人发送导航坐标，调整可见光的摄像头的焦距、视角参数并在后续的操作中维持不变，所述方法包括区域坐标标定、对多个机器人握手并分割多个机器人跟踪视频和对机器人行走跟踪；本发明的“GMM+KCF”视频跟踪算法，在继承传统KCF算法运行高效特点的同时，利用GMM算法进行在线模板更新，有效地克服了传统KCF算法无法适应的WMR成像大小随距离变化而变化所导致的跟踪丢失问题，在兼顾快速性的同时保证了视频跟踪的准确度。

Description

一种多个轮式机器人行走的室内视觉定位方法

技术领域

本发明涉及室内导航，具体涉及一种多个轮式机器人行走的室内视觉定位方法。

背景技术

室内轮式移动机器人作为移动服务机器人的一种，由于其结构简单、运动平稳、技术成熟等众多优点，显示出了极其广泛的应用前景和发展优势，并且随着科技的进步和人类需求的日益增长，移动机器人的应用范围大为扩展，不仅在工业、农业、医疗、服务等行业中得到广泛的应用，而且在城市安全、国防等有害与危险场合得到很好的应用。因此，移动机器人技术已经得到世界各国的普遍关注。

实现室内轮式移动机器人智能化的条件之一就是实现机器人自主运动或者称之为自主导航。一般而言，室内机器人导航/导引有如下几种方法：1.电磁/色带导航；2.激光导航；3.RFID(Radio Frequency Identification，射频识别)导航；4.惯性导航；5.地磁导航；6.视觉导航。其中电磁/色带导航方法由于需要预先铺设信标(例如电磁导航手段需要在地下埋设金属线，并在金属线加载导引频率)，这种方式安装复杂，难以扩展，且无法满足复杂路径的要求。激光导航的缺点是制造成本高，对环境要求较为苛刻。RFID导航(如WIFI定位，蓝牙定位，GPS等)也需要预先铺设发射基站，且导航可靠性较差，精度较低。惯性导航仅可以满足短时间内的导航精度要求，无法完成长时间准确的导航定位。地磁导航在室外具有较高精度，但受限于室内复杂/多变的电磁环境，地磁导航无法在室内环境实现准确导航。

与上述方法相比，视觉导航方法因为成本可控，不需要预先铺设辅助设备，受环境影响有限等特点，而成为目前的主流WMR(Wheeled MobileRobot，轮式移动机器人)导航算法。现有的视觉导航算法一般都是将视觉量测设备安装在WMR上，利用SLAM(simultaneouslocalization and mapping，即时定位与地图构建)技术实现导航定位。这种方法在环境特征丰富且相对稳定、仅有一辆WMR的情况下可以取得满意的效果，但是当同时存在多个WMR的情况下，这种方法的导航效果就会大打折扣。而随着多智能体协同技术的蓬勃发展，多WMR协同完成任务已经成为了时代发展的必然选择，而多WMR协同完成任务的基础便是实现多WMR的准确定位。

发明内容

本发明提出一种多个轮式机器人行走的室内视觉定位方法，可以对室内场景下的多轮式机器人行走进行导航定位，通过在环境中架设已经标定好的可见光摄像头，利用运动物体检测+视频跟踪的方法实现多轮式机器人行走进行的轨迹追踪，将捕捉到轮式机器人行走进行的图像信息利用摄像头标定参数转换至导航坐标系从而实现多轮式机器人行走进行的定位。

为了实现上述目的，本发明的技术方案是：

一种多个轮式机器人行走的室内视觉定位方法，包括在室内周边设置与监控服务器连接的可见光摄像头，可见光摄像头视角覆盖机器人行走全部区域，监控服务器通过可见光摄像头获取机器人的行走轨迹，向机器人发送导航坐标，调整可见光的摄像头的焦距、视角参数并在后续的操作中维持不变，首先对区域坐标标定，对多个机器人握手并分割多个机器人跟踪视频，然后对机器人行走进行跟踪；其中：

所述对机器人行走进行跟踪步骤包括：

a.确定跟踪帧视频中被跟踪机器人位置，建立初始目标跟踪框；

b.利用KCF算法进行视频目标跟踪；

c.向被跟踪的轮式机器人发送位置坐标跟踪信息并更新目标跟踪框；

d.获取下一帧跟踪帧视频，获取机器人位置坐标，将当前帧视频机器人位置坐标与前一帧轮式机器人位置坐标比较，判断当前帧视频机器人位置坐标变化是否超出了位置变化阈值；

e.如果超出了位置变化阈值，则利用GMM算法重新计算目标跟踪框后返回步骤b；否则提取上一帧的目标跟踪框返回步骤b。

方案进一步是：所述区域坐标标定,是将一个棋盘格标识物放置在行走区域场景中，利用棋盘格式标定法进行区域坐标标定，获得在当前摄像头安装状态下的行走区域坐标系。

方案进一步是：所述对多个机器人握手并分割多个机器人跟踪视频的步骤包括：

f.获得在当前摄像头安装状态下的行走区域坐标系后，撤出棋盘格标识物，仅保留场景的背景物体，获取室内行走区域图像，将获取的图像信息利用GMM算法模型确定背景图像信息；

g.监控服务器连续发出握手信号，多个机器人分别进入行走区域，连续获取行走区域帧视频，通过GMM算法从确定的背景图像信息中分割出含有机器人的帧视频；

h.随被依次单独分割出含有机器人的帧视频之后以及在下一个单独机器人帧视频之前，查询是否收到握手同步响应信号，如果没有同步响应信号，发出跟踪失败信号；

i.收到握手同步响应信号，进行编号标记，将编号赋予前一个并被单独分割出帧视频的机器人，并以此依次赋予编号实现监控服务器与多个机器人的握手；

j.在赋予编号的同时确定赋予编号机器人的随后的连续帧视频是该编号机器人的跟踪视频，并分别对不同编号机器人视频利用KCF算法进行持续跟踪；

方案进一步是：所述判断当前帧视频机器人位置坐标变化是否超出了位置变化阈值的方法是：

k.标记前一帧t时刻目标跟踪框的两个坐标点分别为将当前帧目标跟踪框与前一帧k时刻目标跟踪框的两个坐标点相对应的坐标点标记为

l.计算与中是否有任意一个成立，其中lim为位置变化阈值，如果任意一个成立，则前帧视频机器人位置坐标变化超出了位置变化阈值。

方案进一步是：所述位置变化阈值为0.4m。

本发明的有益效果是：

(1)本发明解决了传统的室内视觉导航算法无法应用于室内多轮式机器人存在的场景下的问题，扩展了轮式机器人的应用空间。

(2)本发明所设计的多WMR的室内视觉定位方法，通过降低可见光摄像头数量，可以显著的降低成本。

(3)本发明所设计的“GMM+KCF”视频跟踪算法，在继承传统KCF算法运行高效特点的同时，利用GMM算法进行在线模板更新，有效地克服了传统KCF算法无法适应的WMR成像大小随距离变化而变化所导致的跟踪丢失问题，在兼顾快速性的同时保证了视频跟踪的准确度。

下面结合附图和实施例对本发明进行详细描述。

附图说明

图1为多轮式机器人的室内视觉定位系统流程示意图；

图2为多轮式机器人的室内视觉定位系统可见光摄像头安装示意图；

图3为可见光摄像头标定流程示意图；

图4为标定用棋盘格拍摄图；

图5为标定结果示意图；

图6为场景的背景视频信息示意图；

图7为与GMM算法对WMR的区分结果示意图；

图8为多轮式机器人的检测与视频跟踪算法流程图；

图9为WMR相对摄像头较远成像示意图；

图10为WMR相对摄像头较近成像示意图；

图11为传统KCF算法流程图；

图12为GMM+KCF算法流程图；

图13为GMM+KCF算法运行结果示意图；

图14为多轮式机器人根据结果示意图；

图15为多轮式机器人定位结果示意图。

具体实施方式

一种多个轮式机器人行走的室内视觉定位方法，包括在室内周边设置与监控服务器连接的可见光摄像头，可见光摄像头视角覆盖机器人行走全部区域，监控服务器通过可见光摄像头获取机器人的行走轨迹，向机器人发送导航坐标，调整可见光的摄像头的焦距、视角参数并在后续的操作中维持不变，首先对区域坐标标定，对多个机器人握手并分割多个机器人跟踪视频，然后对机器人行走进行跟踪；其中：

所述对机器人行走进行跟踪步骤包括：

a.确定跟踪帧视频中被跟踪机器人位置，建立初始目标跟踪框；

b.利用KCF算法进行视频目标跟踪；

c.向被跟踪的轮式机器人发送位置坐标跟踪信息并更新目标跟踪框；

d.获取下一帧跟踪帧视频，获取机器人位置坐标，将当前帧视频机器人位置坐标与前一帧轮式机器人位置坐标比较，判断当前帧视频机器人位置坐标变化是否超出了位置变化阈值；

e.如果超出了位置变化阈值，则利用GMM算法重新计算目标跟踪框后返回步骤b；否则提取上一帧的目标跟踪框返回步骤b。

其中的所述区域坐标标定,是将一个棋盘格标识物放置在行走区域场景中，利用棋盘格式标定法进行区域坐标标定，获得在当前摄像头安装状态下的行走区域坐标系。

其中：所述对多个机器人握手并分割多个机器人跟踪视频的步骤包括：

f.获得在当前摄像头安装状态下的行走区域坐标系后，撤出棋盘格标识物，仅保留场景的背景物体，获取室内行走区域图像，将获取的图像信息利用GMM算法模型确定背景图像信息；

g.监控服务器连续发出握手信号，多个机器人分别进入行走区域，连续获取行走区域帧视频，通过GMM算法从确定的背景图像信息中分割出含有机器人的帧视频；

h.随被依次单独分割出含有机器人的帧视频之后以及在下一个单独机器人帧视频之前，查询是否收到握手同步响应信号，如果没有同步响应信号，发出跟踪失败信号；

i.收到握手同步响应信号，进行编号标记，将编号赋予前一个并被单独分割出帧视频的机器人，并以此依次赋予编号实现监控服务器与多个机器人的握手；

j.在赋予编号的同时确定赋予编号机器人的随后的连续帧视频是该编号机器人的跟踪视频，并分别对不同编号机器人视频利用KCF算法进行持续跟踪；

实施例中：所述判断当前帧视频机器人位置坐标变化是否超出了位置变化阈值的方法是：

k.标记前一帧t时刻目标跟踪框的两个坐标点分别为将当前帧目标跟踪框与前一帧k时刻目标跟踪框的两个坐标点相对应的坐标点标记为

l.计算与中是否有任意一个成立，其中lim为位置变化阈值，如果任意一个成立，则前帧视频机器人位置坐标变化超出了位置变化阈值，所述位置变化阈值为0.4m。

下面结合附图并举例，对上述跟踪进行详细描述。

上述实施例是对室内场景下的多轮式机器人简称WMR进行导航定位。该系统通过在环境中架设已经标定好的可见光摄像头，利用运动物体检测+视频跟踪的方法实现多WMR的轨迹追踪，将捕捉到WMR的图像信息利用摄像头标定参数转换至导航坐标系从而实现多WMR的定位。

本实施例的搜索原理是：首先，完成相关场景的布控与安装；其次使用棋盘格图像利用张正友标定算法完成对摄像头的标定工作，获取摄像头相关参数；再次利用“GMM+KCF”方法实现对摄像头视野内的WMR的视频识别与追踪，获取WMR的图像信息；最后利用摄像头相关参数对WMR的图像信息进行坐标转换(由图像信息转换为空间位置坐标信息)，从而得到WMR的位置信息。系统整体运行流程如图1所示。

具体流程包括以下步骤：

步骤1：将可见光摄像头固定安装在场景内，确保可见光摄像头可以充分覆盖全部WMR的运行区域，同时调整好可见光的摄像头的焦距、景深等参数并在后续的操作中维持不变。安装情况示意图如图2所示。

步骤2：将棋盘格标识物放置在场景中，利用该标识物完成对可见光摄像头的标定。标定流程如图3所示。该阶段可见光摄像头开机，但是WMR并不驶入场景，处于系统准备阶段。本标定采用的张正友标定法为业界广泛通用的摄像头标定方法，此处不再进行详细说明。通过完成可见光摄像头的标定，可以获得在当前摄像头安装状态下的图像坐标系与世界坐标系的坐标转换关系，利用该坐标转换关系可以将图像坐标系的位置坐标转换至世界坐标系，可见光摄像头的标定用图像如图4所示，世界坐标系定义如图5所示。

步骤3：撤出棋盘格等标识物，仅保留场景的背景物体，可见光摄像头开机拍摄一段场景的背景视频，该背景信息用于GMM的模型训练。该阶段可见光摄像头开机，但是WMR并不驶入场景，仍处于系统准备阶段，此时所得到的视频为WMR运行场景的背景，拍摄得到的背景信息用于训练GMM模型。GMM模型训练算法原理如下，首先介绍高斯混合背景模型的基础公式：

其中：

X_t：t帧的当前像素的RGB色彩向量；

K：高斯模型的总数，此处设为3；

μ_i,t：t帧的第i个高斯分布的均值向量；

t帧的第i个高斯分布的协方差矩阵，I₃代表3×3的单位矩阵，σ_i,_t代表方差；

ω_i,t：t帧的第i个高斯分布的权重，且满足条件

GMM训练过程可以拆分为如下三个步骤：

1.模型初始化

将视频序列中的第一帧图像的每一个像素点的彩色向量值初始化确定该像素的K个高斯分布的平均取值，同时确立较大的方差(此处设置为36)和的权重。

2.模型更新

模型更新首先要判断匹配性，当获得每一帧的图像时，将这帧图像的每个像素点与该像素已存在的K个高斯分布一一进行匹配，对于匹配的像素点进行更新，否则不予更新。匹配成功的判断准则是：

对匹配的第i个高斯分布的参数更新公式如下：

μ_i,t＝(1-ρ_i,t)μ_i,t-1+ρ_i,tX_t

其中，ρ_i,t＝0.005/ω_i,t，ρ_i,t为参数学习速率。

假设没有发现与X_t互相搭配的高斯分布，那么此时确定的该像素的高斯分布就要以X_t作为平均值分配给最小权重的高斯分布，接下来改高斯分布再次分配较小的权重和较大的方差。同时按照下式更新权重：

ω_i,t＝(1-α)ω_i,t-1+αM_i,t

上式中：

3.模型排序

将每个像素的所有高斯分布排序，由大到小，排序规则按照ω_i,t/σ_i,t的比值。

并根据下述公式：

计算出B的值，根据上述的排序取出B个值，作为背景像素的高斯分布结果，也就是最终训练得到的GMM算法模型。

当完成GMM算法的训练后，可以利用训练好的模型进行背景检测/前景分割，具体方法为：如果前B个高斯分布中其中任意一个能够与X_t匹配，则这一个匹配的就是背景点，否则是前景点。通过GMM算法可以准确地区分前景与背景信息。拍摄得到的背景信息如图6所示，GMM算法对于WMR区分结果如图7所示。

步骤4：将多WMR放置于场景内，利用“GMM+KCF”(Gaussian Mixture Model，高斯混合模型)(Kernelized Correlation Filter，核化相关滤波)算法实现多WMR的检测与视频跟踪。

该阶段中可见光摄像头开机，多WMR驶入场景，系统开始进行WMR的视频识别与跟踪。该阶段算法具体流程如图8所示。其中GMM算法用于从背景信息中分割出多WMR信息，KCF算法用于实现对于WMR算法的视频跟踪。其中，KCF算法为目前广泛应用的算法，此处仅做简单说明，仅对其核心原理作简要介绍：

KCF跟踪算法可以解决跟踪过程中样本过少导致无法准确定位目标这一问题，通过循环矩阵对基础数据多次位移产生大量样本，并且通过离散傅里叶变换的推导在频域实现快速模板匹配。KCF算法类似于一个滤波器，计算每个候选区域通过滤波器的响应值，响应值最大位置处就是当前帧中的目标位置。在训练KCF算法中的分类器时，目标信息作为正样本，背景信息作为负样本。分类器训练好以后，在当前帧中以上一帧跟踪结果为中心，将它的周围区域作为候选目标，对每个候选目标作相关滤波，计算候选目标滤波响应值，响应值最大位置就是目标位置。通常情况下，背景样本较少，算法中通过对样本进行循环位移，获得大量的样本。由于样本矩阵是通过循环位移得到，通过傅里叶变换之后，样本矩阵可以转换为对角矩阵来加速计算。在训练分类器时，使用线性分类器，需要计算线性岭回归系数，涉及到矩阵求逆的过程，但是该算法巧妙的将求逆过程通过傅里叶变换，转换到频域计算，将求逆过程转换为计算点积过程大大降低计算量。

但是在室内多WMR的应用场景下，传统的KCF算法并不能取得理想的效果。因为WMR是一个运动物体，导致其距离摄像头的距离时远时近，也就导致了其在视频中的成像时大时小，如图9、图10所示。这种成像的大小变化会导致传统KCF算法出现“跟丢”的情况。传统KCF算法无法有效解决这类问题，而在这里我们提出了一种“GMM+KCF”的方法，通过GMM算法进行KCF追踪模板的在线更新，不再跟踪的全程仅使用同一模板，而是随着WMR的运行对跟踪模板进行在线实时更新，确保跟踪的准确性与稳定性。“GMM+KCF”算法流程图如图12所示，传统KCF算法流程图如图11所示，“GMM+KCF”算法运行结果图如图13所示。其中，图12中的“GMM+KCF”算法流程中，决定“WMR位置是否发生显著变化”的计算流程简要描述如下：

为方便介绍，首先作如下的变量定义：对于每一个WMR而言，可以对该WMR所处的位置进行实时记录，例如：1号WMR在t时刻的所处的位置记为其在k时刻所处的位置记为基于此种定义，判定“1号WMR位置是否发生显著变化”的流程介绍如下：以t时刻作为判断起点，则将作为参考位置；在后续时刻中(以k时刻为例)，计算与中是否有任意一个成立，其中lim为误差检测限，本专利中设置为0.4m；如果存在任意一个成立，则认为1号WMR位置发生显著变化，同时将作为参考位置执行后续检测；如果二者均不成立，则认为1号WMR位置不发生显著变化，同时继续以作为参考位置执行后续检测。

步骤5：将步骤4中追踪到的多WMR视频信息进行坐标转换(由图像信息转换为空间坐标信息)，从而得到多WMR的位置信息。导航定位结果如图14、15所示。

综上所述，以上仅为本实施例的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种多个轮式机器人行走的室内视觉定位方法，包括在室内周边设置与监控服务器连接的可见光摄像头，可见光摄像头视角覆盖机器人行走全部区域，监控服务器通过可见光摄像头获取机器人的行走轨迹，向机器人发送导航坐标，调整可见光的摄像头的焦距、视角参数并在后续的操作中维持不变，首先对区域坐标标定，对多个机器人握手并分割多个机器人跟踪视频，然后对机器人行走进行跟踪；其特征在于：

所述对机器人行走进行跟踪步骤包括：

a.确定跟踪帧视频中被跟踪机器人位置，建立初始目标跟踪框；

b.利用KCF算法进行视频目标跟踪；

c.向被跟踪的轮式机器人发送位置坐标跟踪信息并更新目标跟踪框；

d.获取下一帧跟踪帧视频，获取机器人位置坐标，将当前帧视频机器人位置坐标与前一帧轮式机器人位置坐标比较，判断当前帧视频机器人位置标坐变化是否超出了位置变化阈值；

e.如果超出了位置变化阈值，则利用GMM算法重新计算目标跟踪框后返回步骤b；否则提取上一帧的目标跟踪框返回步骤b。

2.根据权利要求1所述的方法，其特征在于，所述区域坐标标定,是将一个棋盘格标识物放置在行走区域场景中，利用棋盘格式标定法进行区域坐标标定，获得在当前摄像头安装状态下的行走区域坐标系。

3.根据权利要求1所述的方法，其特征在于，所述对多个机器人握手并分割多个机器人跟踪视频的步骤包括：

f.获得在当前摄像头安装状态下的行走区域坐标系后，撤出棋盘格标识物，仅保留场景的背景物体，获取室内行走区域图像，将获取的图像信息利用GMM算法模型确定背景图像信息；

g.监控服务器连续发出握手信号，多个机器人分别进入行走区域，连续获取行走区域帧视频，通过GMM算法从确定的背景图像信息中分割出含有机器人的帧视频；

h.随被依次单独分割出含有机器人的帧视频之后以及在下一个单独机器人帧视频之前，查询是否收到握手同步响应信号，如果没有同步响应信号，发出跟踪失败信号；

i.收到握手同步响应信号，进行编号标记，将编号赋予前一个并被单独分割出帧视频的机器人，并以此依次赋予编号实现监控服务器与多个机器人的握手；

j.在赋予编号的同时确定赋予编号机器人的随后的连续帧视频是该编号机器人的跟踪视频，并分别对不同编号机器人视频利用KCF算法进行持续跟踪。

4.根据权利要求1所述的方法，其特征在于，所述判断当前帧视频机器人位置坐标变化是否超出了位置变化阈值的方法是：

k.标记前一帧t时刻目标跟踪框的两个坐标点分别为将当前帧目标跟踪框与前一帧k时刻目标跟踪框的两个坐标点相对应的坐标点标记为

l.计算与中是否有任意一个成立，其中lim为位置变化阈值，如果任意一个成立，则前帧视频机器人位置坐标变化超出了位置变化阈值。

5.根据权利要求4所述的方法，其特征在于，所述位置变化阈值为0.4m。