CN105606092B - 一种室内机器人定位方法及系统 - Google Patents

Abstract

本发明提供一种室内机器人定位方法及系统，方法首先从机器人的视角实时拍摄室内的静止参照物，得到参照物图像，然后获取从前一帧参照物图像到当前帧参照物图像的光流，并对光流进行高斯滤波，得到机器人与所述参照物的相对速度，最后对运动速度进行时间维度积分，得到机器人在室内的位置。本发明仅用摄像头等拍摄装置即可求得机器人的运动速度和转动角速度，进而得到机器人位置，可以辅助机器人实现精简的导航。另外，通过高斯滤波与光流法结合，可有效滤除光流中的噪声，提高解算出的机器人行驶速度和转动角速度的精度。

Description

一种室内机器人定位方法及系统

技术领域

本发明属于智能机器人领域，尤其涉及一种室内机器人定位方法及系统。

背景技术

作为民用化最成功的机器人技术之一，扫地机器人正在走进越来越多的家庭，智能清扫正在让人们的家居生活变得越来越温馨。追求更高水平的智慧是机器人发展的一条主线。通过各种新功能的传感器，防碰撞、防跌落等一些扫地机器人的常见使用问题已得到成功解决，同时应用加装在充电桩上的信标实现了机器人低电量时的回充导航、极大地方便了人们的使用。

目前扫地机器人最新的发展趋势是通过自动规划清扫路线实现100％无死角的屋内环境清洁，这将大大优于目前常用的随机碰撞式寻路模式。自动规划清扫路线的技术核心是机器人要能够自主导航，实时获知自己在屋子中的位置和运动轨迹。

目前国内外的科研机构都在积极开展光学视觉系统与扫地机器人结合的相关研究。其中Neato公司的路径规划式导航系统通过激光定位实现，通过机器人上的激光扫描器件扫描整个房间的结构，然后通过所有数据生成数字地图，规划出最有效的清洁路线。在清洁时也会确保不留下死角，清扫完毕后还会自动返回底座充电。但激光扫描技术成本好高，故障率也偏高，当遇到玻璃等高透光物体时还会出现导航失效。而LG公司的路径规划式机器人导航系统则是采用视觉导航方案，其通过机体顶部安装的1至2个摄像头，扫描天花板生成数字地图、测定位机体所处位置，自动构建清扫路径。通常图像特征匹配可以测定机器人的行驶速度，但扫地机器人导航是以房间为参考进行路径规划的，视频获取的机器人运动速度需要换算到以房间为参考的地面坐标系中进行积分才能解算出它的行驶轨迹。通常的方案是将视频导航信息与惯导测向结合，通过陀螺或电子磁罗盘测出机器人运动的航向角，但这种方式大大增加了系统的复杂性，同时还给导航系统引入了新的干扰、加重了导航误差。

发明内容

(一)要解决的技术问题

本发明的目的在于，提供一种室内机器人定位方法及系统，能以较少的硬件设备对机器人进行实时定位，并且定位精度高。

(二)技术方案

本发明提供一种室内机器人定位方法，用于确定室内处于运动状态的机器人的位置，方法包括：

S1，从机器人的视角实时拍摄室内的参照物，得到参照物图像，其中，该参照物在室内处于静止状态；

S2，获取从前一帧参照物图像到当前帧参照物图像的光流，并对光流进行高斯滤波，得到机器人与所述参照物的相对速度，即机器人在室内的运动速度，其中，所述机器人的运动速度包括线速度和角速度；

S3，对所述运动速度进行时间维度积分，得到所述机器人在室内的位置。

本发明还提供一种室内机器人定位系统，用于确定室内处于运动状态的机器人的位置，系统包括：

拍摄装置，其置于机器人上，用于实时拍摄室内的参照物，得到参照物图像，其中，该参照物在室内处于静止状态；

图像分析装置，用于获取从前一帧参照物图像到当前帧参照物图像的光流，并对光流进行高斯滤波，得到机器人与所述参照物的相对速度，即机器人在室内的运动速度，其中，所述机器人的运动速度包括线速度和角速度；

计算装置，用于对所述运动速度进行时间维度积分，得到所述机器人在室内的位置。

(三)有益效果

本发明仅用摄像头等拍摄装置即可求得机器人的运动速度的转动角速度，进而得到机器人位置，可以辅助机器人实现精简的导航。另外，本发明通过高斯滤波与光流法结合，可有效滤除光流中的噪声，提高解算出的机器人行驶速度和转动角速度的精度。

附图说明

图1是本发明提供的室内机器人定位方法的流程图。

图2是本发明实施例中整幅天花板图像的光流处理效果图。

图3是本发明实施例中转弯时整幅天花板图像。

图4是本发明实施例中前半幅天花板图像光流计算和滤波效果图。

图5是本发明实施例中后半幅天花板图像光流计算和滤波效果图。

图6是本发明实施例中机器人在室内的运动轨迹图。

具体实施方式

本发明提供一种室内机器人定位方法及系统，方法实现从机器人的视角实时拍摄室内的静止参照物，得到参照物图像，然后获取从前一帧参照物图像到当前帧参照物图像的光流，并对光流进行高斯滤波得到机器人与所述参照物的相对速度，最后对运动速度进行时间维度积分，得到机器人在室内的位置。本发明仅用摄像头等拍摄装置即可求得机器人的运动速度和转动角速度，进而得到机器人位置，可以辅助机器人实现精简的导航。另外，通过高斯滤波与光流法结合，可有效滤除光流中的噪声，提高解算出的机器人行驶速度和转动角速度的精度。

图1是本发明提供的室内机器人定位方法的流程图，如图1所示，室内机器人定位方法包括：

S1，从机器人的视角实时拍摄室内的参照物，得到参照物图像，其中，该参照物在室内处于静止状态；室内机器人可以是扫地机器人等智能机器人，参照物可以是室内的天花板等静止参照物，需要说明的是，由于机器人的视角是不变的，随着运动，其拍摄的参照物也会发生变化，本发明只需要在前后帧图像中具有相同的参照物即可，并不局限于某一具体参照物。

S2，获取从前一帧参照物图像到当前帧参照物图像的光流，并对光流进行高斯滤波，得到机器人与所述参照物的相对速度，即机器人在室内的运动速度，其中，所述机器人的运动速度包括线速度和角速度；

S3，对所述运动速度进行时间维度积分，得到所述机器人在室内的位置。

根据本发明的一种实施方式，步骤S2包括：获取前后两帧参照物图像，计算像素点灰度值随时间的变化量I_t和随空间的变化量I_x、I_y：

其中，I为像素点灰度值，x,y为像素点在参照物图像中的位置，t为时间；

根据像素点的光流场方程：

联立多个像素点的光流场方程求解出参照物相对机器人的光流

其中，为x方向光流，为y方向光流，对参照物图像中的所有点都求取光流，得到的光流图代表了机器人相对于参照物的实时运动速度，光流图包括x方向光流图像和y方向光流图像；

对所述的两个方向的光流图像分别进行高斯滤波，得到机器人的线速度u、v，u为x方向速度，v为y方向速度，其中，高斯滤波函数的表达式为：

其中，B(x,y)是滤波后的光流图像，A(x,y)是待滤波的光流图像，由，(x₀,y₀)是滤波器的中心，g(x,y)是(x,y)点的滤波权重，σ为滤波参数；

滤波后的光流图像B(x,y)分为前后两半幅图像，根据机器人转动时，前半幅图像和后半幅图像的光流差异，得到参照物相对于机器人的旋转速度，即机器人的角速度ω：

ω＝α₁-α₂，

其中，α₁、α₂分别为前半幅光流图像和后半幅光流图像的光流角度，其表达式为：

其中，k＝1,2，B_k,u(x,y)为前半幅光流图像的滤波结果，B_k,v(x,y)为后半幅光流图像的滤波结，B_k为分别是前后两半幅图像的光流高斯滤波结果。

根据本发明的一种实施方式，步骤S2还包括：对机器人的线速度u、v进行坐标系转换，得到室内地面坐标系下的线速度u_n、v_n，其表达式为：

其中，β_t表示机器人的航向角，其表达式为：

β₀为0时刻航向角的初始值，ω_n为n时刻机器人的角速度，Δt为两帧图像间的时间间隔。

根据本发明的一种实施方式，步骤S3包括：

对机器人在室内地面坐标系下的线速度u_n、v_n在时间维度积分，得到机器人在室内地面坐标系下的实时位置坐标。

根据本发明的一种实施方式，方法还包括：S4，将机器人各时刻的位置坐标进行描点并连线，得到机器人在室内的运动轨迹。

根据本发明的一种实施方式，室内机器人定位系统包括：

拍摄装置，其置于机器人上，用于实时拍摄室内的参照物，得到参照物图像，其中，该参照物在室内处于静止状态；

图像分析装置，用于获取从前一帧参照物图像到当前帧参照物图像的光流，并对光流进行高斯滤波，再对滤波后的光流进行分析，得到机器人与所述参照物的相对速度，即机器人在室内的运动速度，其中，所述机器人的运动速度包括线速度和角速度；

计算装置，用于对运动速度进行时间维度积分，得到机器人在室内的位置。

根据本发明的一种实施方式，图像分析装置执行：获取前后两帧参照物图像，计算像素点灰度值随时间的变化量I_t和随空间的变化量I_x、I_y：

其中，I为像素点灰度值，x,y为像素点在参照物图像中的位置，t为时间；

根据像素点的光流场方程：

联立多个像素点的光流场方程求解出参照物相对机器人的光流

其中，为x方向光流，为y方向光流，对视频图像中的所有点都求取光流，得到的光流图像代表了机器人相对于参照物的实时运动速度，光流图包括x方向光流图像和y方向光流图像；

对所述的两个方向的光流图像分别进行高斯滤波，得到机器人的线速度u、v，u为x方向速度，v为y方向速度，其中，高斯滤波函数的表达式为：

其中，B(x,y)是滤波后的光流图像，A(x,y)是待滤波的光流图像，由，(x₀,y₀)是滤波器的中心，g(x,y)是(x,y)点的滤波权重，σ为滤波参数；

滤波后的光流图像B(x,y)分为前后两半幅图像，根据机器人转动时，前半幅图像和后半幅图像的光流差异，得到参照物相对于机器人的旋转速度，即机器人的角速度ω：

ω＝α₁-α₂，

其中，α₁、α₂分别为前半幅光流图像和后半幅光流图像的光流角度，其表达式为：

其中，k＝1,2，B_k,u(x,y)为前半幅光流图像的滤波结果，B_k,v(x,y)为后半幅光流图像的滤波结，B_k为分别是前后两半幅图像的光流高斯滤波结果。

根据本发明的一种实施方式，图像分析装置还执行：

对机器人的线速度u、v进行坐标系转换，得到室内地面坐标系下的线速度u_n、v_n，其表达式为：

其中，β_t表示机器人的航向角，其表达式为：

β₀为0时刻航向角的初始值，ω_n为n时刻机器人的角速度，Δt为两帧图像间的时间间隔。

根据本发明的一种实施方式，计算装置执行：对机器人在室内地面坐标系下的线速度u_n、v_n在时间维度积分，得到机器人在室内地面坐标系下的实时位置坐标。

根据本发明的一种实施方式，系统还包括：轨迹绘制装置，机器人将各时刻的位置坐标进行描点并连线，得到机器人在室内的运动轨迹。

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

本发明实施例提供一种室内扫地机器人的定位方法，包括：

S1，利用安装在扫地机器人上的视频装置，实时拍摄室内的天花板，得到天花板图像，扫地机器人的行走属于地面上的平面运动，视频装置的镜头安装于机器人上表面，平行于天花板。考虑到一般家庭内的格局是天花板与地板平行，借助这种处处等高的特点，通过计算摄像头中天花板图像的运动趋势与旋转量即可直接获得扫地机器人的各种运动信息。

S2，对参照物图像进行分析，得到机器人与参照物的相对速度，即机器人在室内的运动速度，其中，机器人的运动速度包括线速度和角速度。

采用光流法计算视频画面中天花板的“运动”参数，求取的参数包括运动的线速度和转动的角速度。

机器人运动过程中，尽管天花板是静止的，但相对于所载摄像机，天花板呈现出一种相对“运动”趋势，速度通过摄像机画面中天花板中各个像素点的光流值计算。

令视频图像中的任意一点(x,y)在t时刻灰度值为I(x,y,t)。扫地机器人行驶时，该点的亮度在空间和时间维度上都会发生变化，即：

天花板的“运动”方向可以根据这些亮度分量的变化获知。天花板在x和y坐标轴方向的运动与亮度变化的关系可以通过光流场方程表示：

因为天花板的“运动”会造成图像的亮度呈区域性变化，将多点的光流 场方程联立可以求出天花板的光流，即天花板与机器人的相对运动速度：

其中，为x方向光流，为y方向光流，对天花板图像中的所有点都求取光流，得到的光流图代表了机器人相对于天花板的实时运动速度，光流图包括x方向光流图像和y方向光流图像。

由于每点所处的亮度变化不尽一致，图像中各个点光流的计算结果相当杂乱，本专利引入高斯滤波器对结果进行梳理，提取出真正反映机器人移动的光流低频信息。二维高斯平滑滤波器的函数为：

其中，A(x,y)是待滤波的光流图像，(x₀,y₀)是滤波器的中心，g(x,y)是(x,y)点的滤波权重，σ为滤波参数。通过不断移动中心点(x₀,y₀)可以实现对图像的逐点滤波。σ决定了高斯滤波器的滤波能力，σ越大、高斯滤波器的去噪效果越好。整幅图像的光流处理效果如图2所示，经高斯滤波后扫地机器人的运动方向可以准确提取。

考虑到旋转时，将镜头轴线与机器人转动中心重合，当机器人车体前半部分转动时，后半部分将沿相反方向运动，转动的越快、视频前后两部(以天花板行驶方向为参考)分光流的角度差值越大。本实施例充分利用这一现象计算角速度。经高斯滤波处理后的光流角度计算公式如下：

其中，k＝1,2，B_k,u(x,y)为前半幅图像的滤波结果，B_k,v(x,y)为后半幅图像的滤波结，B_k为分别是每帧视频前后两半幅图像的光流高斯滤波结果。α_k是光流法算出的图像转动角度。本实施例中，视频装置位于机器人的中心(也是机器人的转动中心)，机器人不转动时前后两半幅图像计算出的光流方向相同；转动时，后半部分的图像光流与前半部分趋势反向(以顺时针转动为例，前半车体是从右向左运动、后半车体是从左向右运动)，转动角速度越大，前后两部分光流方向的差异越大，二者取差值即可求得机器人转动的角速度：

ω＝α₁-α₂，

转弯时光流法的处理效果如图3、图4、图5所示。图3是整幅图像，图4和图5分别是前半幅和后半幅图像的光流计算和滤波效果，可以看出经光流和高斯滤波处理后，扫地机器人的转动方向可以精确计算得到。

上述求得的天花板与机器人的相对移动速度是以机器人为参考坐标系，考虑到机器人的定位结果最终要反映到以室内地面为基准的坐标系中，因此采用欧拉角公式通过航向角β实现机器人坐标系与地面坐标系之间的换算，其公式为：

u_n、v_n表示地面坐标系中的扫地机器人速度，u、v表示机器人系中的扫地机器人行走速度。航向角β_t的更新公式由角速度累加获得：

其中，航向角β₀初值本实施例中取为0度，ω_n为n时刻机器人的角速度，Δt为两帧图像间的时间间隔。

S3，对运动速度进行时间维度积分，得到机器人在室内的位置。

因为平面运动中横滚角和俯仰角都为零，将机器人在地面平面中的速度u_n、v_n在时间维度积分后，即可获得扫地机器人的实时位置坐标。

S4，将机器人各时刻的位置坐标进行描点并连线，得到机器人在室内的运动轨迹，其轨迹如图6所示。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种室内机器人定位方法，用于确定室内处于运动状态的机器人的位置，其特征在于，方法包括：

S1，从机器人的视角实时拍摄室内的参照物，得到参照物图像，其中，该参照物在室内处于静止状态；

S2，获取从前一帧参照物图像到当前帧参照物图像中各像素点的光流，并对所有像素点的光流进行高斯滤波，得到机器人与所述参照物的相对速度，即机器人在室内的运动速度，其中，所述机器人的运动速度包括线速度和角速度，其中，所述角速度根据机器人转动时滤波后的前半幅光流图像和后半幅光流图像的光流差异解算得到；

S3，对所述运动速度进行时间维度积分，得到所述机器人在室内的位置。

2.根据权利要求1所述的室内机器人定位方法，其特征在于，所述S2包括：

获取前后两帧参照物图像，计算像素点灰度值随时间的变化量I_t和随空间的变化量I_x、I_y：

其中，I为像素点灰度值，x,y为像素点在参照物图像中的位置，t为时间；

根据像素点的光流场方程：

联立多个像素点的光流场方程求解出参照物相对机器人的光流

其中，为x方向光流，为y方向光流，对图像中的所有点都求取光流，得到光流图像，光流图包括x方向光流图像和y方向光流图像；

对x方向和y方向这两个方向的光流分别进行高斯滤波，得到机器人的线速度u、v，u为x方向速度，v为y方向速度，其中，高斯滤波函数的表达式为：

其中，B(x,y)是滤波后的光流图像，A(x,y)是待滤波的光流图像，(x₀,y₀)是滤波器的中心，g(x,y)是(x,y)点的滤波权重，σ为滤波参数；

滤波后的光流图像B(x,y)分为前后两半幅图像，根据机器人转动时，前半幅图像和后半幅图像的光流差异，得到参照物相对于机器人的旋转速度，即机器人的角速度ω：

ω＝α₁-α₂，

其中，α₁、α₂分别为前半幅光流图像和后半幅光流图像的光流角度，其表达式为：

其中，k＝1,2，B_k,u(x,y)为前半幅或后半幅光流图像在x方向的滤波结果，B_k,v(x,y)为前半幅或后半幅光流图像在y方向的滤波结果，B_k为分别是前后两半幅图像的光流高斯滤波结果。

3.根据权利要求2所述的室内机器人定位方法，其特征在于，所述S2还包括：

对所述机器人的线速度u、v进行坐标系转换，得到室内地面坐标系下的线速度u_n、v_n，其表达式为：

其中，β_t表示机器人的航向角，其表达式为：

β₀为0时刻航向角的初始值，ω_n为n时刻机器人的角速度，Δt为两帧图像间的时间间隔。

4.根据权利要求3所述的室内机器人定位方法，其特征在于，所述S3包括：

对机器人在室内地面坐标系下的线速度u_n、v_n在时间维度积分，得到机器人在室内地面坐标系下的实时位置坐标。

5.根据权利要求4所述的室内机器人定位方法，其特征在于，还包括：

S4，将机器人各时刻的位置坐标进行描点并连线，得到机器人在室内的运动轨迹。

6.一种室内机器人定位系统，用于确定室内处于运动状态的机器人的位置，其特征在于，系统包括：

拍摄装置，其置于机器人上，用于实时拍摄室内的参照物，得到参照物图像，其中，该参照物在室内处于静止状态；

图像分析装置，用于获取从前一帧参照物图像到当前帧参照物图像中各像素点的光流，并对所有像素点的光流进行高斯滤波，得到机器人与所述参照物的相对速度，即机器人在室内的运动速度，其中，所述机器人的运动速度包括线速度和角速度，其中，所述角速度根据机器人转动时滤波后的前半幅光流图像和后半幅光流图像的光流差异解算得到；

计算装置，用于对所述运动速度进行时间维度积分，得到所述机器人在室内的位置。

7.根据权利要求6所述的室内机器人定位系统，其特征在于，所述图像分析装置执行：

获取前后两帧参照物图像，计算像素点灰度值随时间的变化量I_t和随空间的变化量I_x、I_y：

其中，I为像素点灰度值，x,y为像素点在参照物图像中的位置，t为时间；

根据像素点的光流场方程：

联立多个像素点的光流场方程求解出参照物相对机器人的光流

其中，为x方向光流，为y方向光流，对图像中的所有点都求取光流，得到光流图像，光流图包括x方向光流图像和y方向光流图像；

对x方向和y方向这两个方向的光流图分别进行高斯滤波，得到机器人的线速度u、v，u为x方向速度，v为y方向速度，其中，高斯滤波函数的表达式为：

其中，B(x,y)是滤波后的光流图像，A(x,y)是待滤波的光流图像，(x₀,y₀)是滤波器的中心，g(x,y)是(x,y)点的滤波权重，σ为滤波参数；

滤波后的光流图像B(x,y)分为前后两半幅图像，根据机器人转动时，前半幅图像和后半幅图像的光流差异，得到参照物相对于机器人的旋转速度，即机器人的角速度ω：

ω＝α₁-α₂，

其中，α₁、α₂分别为前半幅光流图像和后半幅光流图像的光流角度，其表达式为：

其中，k＝1,2，B_k,u(x,y)为前半幅或后半幅光流图像在x方向的滤波结果，B_k,v(x,y)为前半幅或后半幅光流图像在y方向的滤波结果，B_k为分别是前后两半幅图像的光流高斯滤波结果。

8.根据权利要求7所述的室内机器人定位系统，其特征在于，所述图像分析装置还执行：

对所述机器人的线速度u、v进行坐标系转换，得到室内地面坐标系下的线速度u_n、v_n，其表达式为：

其中，β_t表示机器人的航向角，其表达式为：

β₀为0时刻航向角的初始值，ω_n为n时刻机器人的角速度，Δt为两帧图像间的时间间隔。

9.根据权利要求8所述的室内机器人定位系统，其特征在于，所述计算装置执行：

对机器人在室内地面坐标系下的线速度u_n、v_n在时间维度积分，得到机器人在室内地面坐标系下的实时位置坐标。

10.根据权利要求9所述的室内机器人定位系统，其特征在于，还包括：

轨迹绘制装置，机器人将各时刻的位置坐标进行描点并连线，得到机器人在室内的运动轨迹。