CN105716611A - 基于环境信息的室内移动机器人及其定位方法 - Google Patents

Abstract

本发明公开了一种基于环境信息的室内移动机器人及其定位方法，其中，该机器人带有无线数据接收器和一个位于顶部与周围环境有明显区别的彩色小球；该机器人的定位方法包括以下步骤：机器人定时根据上一时刻的位姿数据预测当前时刻的位姿，机器人接收当前时刻视频监控系统发来的数据帧并解码得到当前时刻的可能位置集合，机器人根据最有可能原则从候选位置集合选出一个位置作为当前位置。本发明的有益之处在于：该机器人不仅携带的设备数量和种类得到简化，而且能够与视频监控系统有机结合，该机器人的定位方法不仅无累积误差，而且能够满足实时定位要求。

Description

基于环境信息的室内移动机器人及其定位方法

技术领域

本发明涉及一种视频监控系统以及一种定位机器人的方法，具体涉及一种覆盖环境的视频监控系统，以及一种利用该覆盖环境的视频监控系统定位机器人的方法，属于机器人技术领域。

背景技术

术语解释：

1、移动机器人及其环境

一般来说，一个移动机器人系统由机械部分、传感部分和控制部分等三个部分组成；或由机械系统、驱动系统、感知系统、机器人——环境交互系统、人机交互系统和控制系统等六个子系统组成。其中，机械系统是由关节连在一起的许多机械连杆的集合体，形成开环运动学链系；驱动系统是使各种机械部件产生运动的装置；感知系统由机器人内部传感器模块和外部传感器模块组成，可获取内部和外部环境状态中有用信息；机器人——环境交互系统是实现机器人与外部环境中的设备相互联系和协调的系统；人机交互系统是人与机器人进行联系和参与机器人控制的装置，包括指令给定装置和信息显示装置；控制系统的任务是根据机器人的作业指令程序以及从传感器反馈回来的信号，支配机器人的执行机构去完成规定的运动和功能。

环境是指移动机器人所能到达的空间位置。

2、机器人定位和实时定位

机器人定位是确定移动机器人在所处环境的空间位置的过程。

机器人实时定位是指机器人采用的定位方法及其软硬件设备能够及时、正确地确定移动机器人在所处环境的空间位置的能力，满足实时定位要求是机器人控制系统能够正确、及时地控制机器人运动的前提条件之一。

3、机器人的位姿

位姿是位置和姿态的缩略语，机器人的位姿包括机器人的位置和机器人的姿态，机器人的姿态是指移动机器人在环境中的移动方向。

4、RGB(Red,Green,Blue)色彩模式

RGB色彩模式是工业界的一种颜色标准，这个模型中颜色的参数分别是：红色(R)、绿色(G)和蓝色(B)，通过三种颜色的变化以及它们相互之间的叠加可以得到各种颜色。RGB24使用24位二进制位来表示彩色图像中的一个像素，RGB每个分量都用8位二进制位表示，取值范围为0-255，是最常用的数字彩色图像采样模式。

5、HSV(Hue,Saturation,Value)颜色模型

HSV是根据颜色的直观特性创建的一种颜色空间，也称六角锥体模型。这个模型中颜色的参数分别是：色调(H)，饱和度(S)和亮度(V)。

移动机器人为了实现在环境中的自主移动，必须要解决的问题是：自身的导航与定位问题，即在移动机器人移动之前，它要解决“在哪里？”“到哪里？”和“如何去？”这三个问题。

解决“在哪里？”这个问题，就是确定移动机器人在环境中的位置。换句话说，“室内移动机器人定位”就是移动机器人确定其在室内环境中位置的过程。

室内移动机器人的定位有其自身的特点：

一、在室内环境中，由于卫星导航信号(例如GPS、北斗等)覆盖不好，所以室内移动机器人的定位无法使用卫星导航；

二、由于多径效应的影响，无线信号定位方式并不适用于室内移动机器人；

三、由于室内环境相比室外环境显得狭小，所以室内移动机器人的定位精度要求更高(一般为厘米级)，并且要求实时定位；

四、室内环境有较复杂的电磁场，所以含有磁性元件的惯性导航设备在室内环境中的应用受到限制。

根据环境模型的有无，移动机器人的定位方法分为三类：基于环境模型的定位、无环境模型的定位、同时建立环境和定位。其中，基于环境模型的定位又可分为三种：局部定位(也称相对定位)、全局定位(也称绝对定位)、组合定位(局部定位和全局定位组合)。

局部定位是移动机器人仅用自身携带的传感器即可实现的定位。目前，有基于里程计和惯性导航设备的两种航位推算方法，应用于室内移动机器人的局部定位方法是基于里程计的航位推算方法。

局部定位的优点是：1、机器人的位姿是自我推算出来的，不需要对外部环境的感知信息；2、定位间隔时间短；3、定位数据具有良好的连续性。

局部定位的缺点是：1、需要已知机器人初始位姿信息；2、定位误差随时间(惯性导航方式)或距离(里程计方式)累计，不适于长时间(惯性导航方式)或长距离(里程计方式)精确定位。

全局定位是移动机器人使用自身携带的传感器(例如超声波、激光雷达、视觉传感器等)感知外部的特征信息实现的定位。目前，应用于室内机器人的全局定位方法包括路标法和地图匹配法。

基于路标的定位方法依赖于环境中已知特征的信标，并需要在移动机器人上安装传感器，由传感器对信标进行观测，从而得到移动机器人的绝对位置信息。

基于地图匹配的定位方法是全局环境地图事先已知，并存入移动机器人中，进行地图匹配定位时，利用移动机器人携带的传感器探测周围环境并建立局部环境地图，通过与全局地图的对比确定移动机器人的全局位置。

全局定位的优点是：1、无需已知移动机器人的初始位姿信息；2、定位数据准确；3、定位误差不随时间、距离累计。

全局定位的缺点是：1、需要对外部环境的感知信息；2、定位间隔时间长；3、定位数据不连续、跳跃大；4、室内环境复杂，定位传感器易被遮挡。

组合定位是将局部定位与全局定位取长补短、融合构成的。组合定位方法是目前最常用的一种定位方式。在这种定位方式下，局部定位数据作为组合定位的输出数据，全局定位数据则用来消除随时间(惯性导航方式)或距离(里程计方式)累计的局部定位误差。

基于环境模型的组合定位方法是目前最常用的一种室内移动机器人定位方式。

在基于环境模型的组合定位方法中，环境模型可设为室内二维地平面全局坐标系，在环境中移动的机器人的位姿可用三元组(x,y,θ)来表示，其中，(x,y)表示移动机器人位于全局坐标系的位置，θ表示移动机器人在全局坐标系中的航向。

在基于环境模型的组合定位方法中，局部定位方案采用基于里程计的航位推算方法；全局定位方案中，由于视觉传感器相对于其它传感器而言能够提供最丰富的感知信息，因此，基于里程计和视觉传感器的机器人组合定位技术是最具代表性的室内机器人定位方法。

基于里程计的局部定位方法：

不失一般性，室内移动机器人采用轮式两轮差速驱动方式，左右两轮分别装有记录驱动轮的运行距离的里程计。如图1所示，假设两个驱动轮轴心连线的中点为M，那么在任意时刻机器人的位姿可用M点的位姿M(x,y,θ)来表示，其中(x,y)为M点在xoy坐标系中的位置，θ表示x轴正向与M点航向之间的夹角。在任意t_n时刻，n＝0,1,2,…,机器人的位姿可表示为M_n(x_n,y_n,θ_n)，其中在t₀时刻的位姿M₀(x₀,y₀,θ₀)是已知的。

图2是机器人位姿推导示意图。参照图2，环境全局坐标系为xoy，机器人两个驱动轮的轴间距为2a，M_k表示t_k时刻机器人两个驱动轮的轴中心位置。在任意t_n时刻，n＝0,1,2,…,左轮和右轮里程计的读数分别为m_L(n)和m_R(n)。从任意t_n-1时刻到t_n时刻(n＝1,2,…,)的时间间隔设为常数T，且充分小，

左轮里程计的运行距离为：

Δm_L(n)＝m_L(n)-m_L(n-1)(1)

右轮里程计的运行距离为：

Δm_R(n)＝m_R(n)-m_R(n-1)(2)

情况1：Δm_L(n)≠Δm_R(n)，已知(x_n-1,y_n-1,θ_n-1)，求(x_n,y_n,θ_n)，n＝1,2,…,

在这种情况下，机器人从t_n-1时刻到t_n时刻的时间段T，左轮和右轮移动距离不同，当T充分小，可以合理地假设机器人行走的轨迹是一段圆弧，参照图2。从t_n-1时刻到t_n时刻机器人的运动轨迹M_n-1M_n是一段圆弧，其圆心为o′，半径为o′M_n-1＝o′M_n＝R_n，圆心角为β；以o′为原点，o′M_n-1为x′轴，为建立局部坐标系x′o′y′。o′x′轴与全局坐标系ox轴之间的夹角为β_n-1，它与θ_n-1之间的关系为：

β_n-1＝θ_n-1-90°(3)

参照图2，有：

Δm_L(n)＝(R_n-a)β(4)

Δm_R(n)＝(R_n+a)β(5)

式(5)减去式(4)，并化简整理，有：

$\mathrm{\β}=\frac{{\mathrm{\Δm}}_R\left(n\right)-{\mathrm{\Δm}}_L\left(n\right)}{2a}---\left(6\right)$

式(4)加上式(5)，并将式(6)带入，化简整理，有：

$R_n=\frac{{\mathrm{\Δm}}_R\left(n\right)+{\mathrm{\Δm}}_L\left(n\right)}{{\mathrm{\Δm}}_R\left(n\right)-{\mathrm{\Δm}}_L\left(n\right)}a---\left(7\right)$

其中，Δm_L(n)≠Δm_R(n)。

参照图2，M_n点在局部坐标系的位置(x′_n,y′_n)可由下式给出：

$\left\{\begin{array}{c}{x}_n^{\′}=R_n\cos \mathrm{\β}\\ y_n^{\′}=R_{n}sin\mathrm{\β}\end{array}\right.---\left(8\right)$

局部坐标系x′o′y′的原点(x′₀,y′₀)在全局坐标系xoy的坐标可由下式给出：

$\left\{\begin{array}{c}{x}_0^{\′}=x_{n-1}-R_n{\mathrm{cos\β}}_{n-1}\\ y_0^{\′}=y_{n-1}-R_n{\mathrm{sin\β}}_{n-1}\end{array}\right.---\left(9\right)$

所以，M_n(x_n,y_n)在全局坐标系xoy的坐标可由下式给出：

$\left\{\begin{array}{c}{x}_n=x_0^{\′}+x_n^{\′}{\mathrm{cos\β}}_{n-1}-y_n^{\′}{\mathrm{sin\β}}_{n-1}\\ y_n=y_0^{\′}+x_n^{\′}{\mathrm{sin\β}}_{n-1}+y_n^{\′}{\mathrm{cos\β}}_{n-1}\end{array}\right.---\left(10\right)$

将式(8)和式(9)代入式(10)，有：

$\left\{\begin{array}{c}{x}_n=x_{n-1}-R_n{\mathrm{cos\β}}_{n-1}+R_n{\mathrm{cos\βcos\β}}_{n-1}-R_n{\mathrm{sin\βsin\β}}_{n-1}\\ y_n=y_{n-1}-R_n{\mathrm{sin\β}}_{n-1}+R_n{\mathrm{cos\βsin\β}}_{n-1}+R_n{\mathrm{sin\βcos\β}}_{n-1}\end{array}\right.---\left(11\right)$

再将式(3)代入式(11)，又因为sinβ_n-1＝-cosθ_n-1和cosβ_n-1＝sinθ_n-1，有：

$\left\{\begin{array}{c}{x}_n=x_{n-1}+R_n\left(-{\mathrm{sin\θ}}_{n-1}+{\mathrm{cos\βsin\θ}}_{n-1}+{\mathrm{sin\βcos\θ}}_{n-1}\right)\\ y_n=y_{n-1}+R_n\left({\mathrm{cos\θ}}_{n-1}-{\mathrm{cos\βcos\θ}}_{n-1}+{\mathrm{sin\βsin\θ}}_{n-1}\right)\end{array}\right.---\left(12\right)$

其中，n＝1,2,…,。

由假设时间间隔常数T充分小，则β很小，有sinβ≈β和cosβ≈1，再由式(6)和式(7)，由式(12)可得：

$\left\{\begin{array}{c}{x}_n\≈x_{n-1}+R_n{\mathrm{\βcos\θ}}_{n-1}=x_{n-1}+\frac{{\mathrm{\Δm}}_R\left(n\right)+{\mathrm{\Δm}}_L\left(n\right)}{2}{\mathrm{cos\θ}}_{n-1}\\ y_n\≈y_{n-1}+R_n{\mathrm{\βsin\θ}}_{n-1}=y_{n-1}+\frac{{\mathrm{\Δm}}_R\left(n\right)+{\mathrm{\Δm}}_L\left(n\right)}{2}{\mathrm{sin\θ}}_{n-1}\end{array}\right.---\left(13\right)$

在使用式(13)推算机器人位置时，(x_n-1,y_n-1,θ_n-1)是已知的，Δm_L(n)和Δm_R(n)分别由式(1)和式(2)得到。

结合图2和式(6)，有：

$\begin{array}{c}{\mathrm{\β}}_n=\mathrm{\β}+{\mathrm{\β}}_{n-1}\\ =\frac{m_R\left(n\right)-m_R\left(n-1\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(n-1\right)}{2a}+{\mathrm{\β}}_{n-1}\\ =\frac{m_R\left(n\right)-m_R\left(n-1\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(n-1\right)}{2a}+\frac{m_R\left(n-1\right)-m_R\left(n-2\right)}{2a}-\frac{m_L\left(n-1\right)-m_L\left(n-2\right)}{2a}+{\mathrm{\β}}_{n-2}\\ =\frac{m_R\left(n\right)-m_R\left(n-2\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(n-2\right)}{2a}+{\mathrm{\β}}_{n-2}\\ =\mathrm{...}\\ =\frac{m_R\left(n\right)-m_R\left(0\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(0\right)}{2a}+{\mathrm{\β}}_0\end{array}---\left(14\right)$

再依据式(3)的结论，可得：

${\mathrm{\θ}}_n=\frac{m_R\left(n\right)-m_R\left(0\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(0\right)}{2a}+{\mathrm{\θ}}_0---\left(15\right)$

其中，n＝1,2,…,，θ₀已知。

由式(13)和(15)可求得(x_n,y_n,θ_n)。

图2所示是机器人向左转弯的情形，向右转弯可以得到相同的结论，不再赘述。

情况2：Δm_L(n)＝Δm_R(n)，已知(x_n-1,y_n-1,θ_n-1)，求(x_n,y_n,θ_n)，n＝1,2,…,

在这种情况下，机器人从t_n-1时刻到t_n时刻的时间段T，左轮和右轮移动相同距离，则机器人行走的轨迹是一段直线，如图2中的从t_n时刻到t_n+1时刻移动的轨迹M_nM_n+1是一段直线。又sin(180-θ_n)＝sinθ_n和cos(180-θ_n)＝-cosθ_n，有

综上前述，已知(x_n-1,y_n-1,θ_n-1)，求(x_n,y_n,θ_n)，n＝1,2,…,，的公式如下：

$\left\{\begin{array}{c}{x}_n=x_{n-1}+\frac{{\mathrm{\Δm}}_R\left(n\right)+{\mathrm{\Δm}}_L\left(n\right)}{2}{\mathrm{cos\θ}}_{n-1}\\ y_n=y_{n-1}+\frac{{\mathrm{\Δm}}_R\left(n\right)+{\mathrm{\Δm}}_L\left(n\right)}{2}{\mathrm{sin\θ}}_{n-1}\\ {\mathrm{\θ}}_n=\frac{m_R\left(n\right)-m_R\left(0\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(0\right)}{2a}+{\mathrm{\θ}}_0\end{array}\right.,n=1,2,\mathrm{...}---\left(17\right)$

其中，(x₀,y₀,θ₀)是已知的。

基于视觉的全局定位方法：

视觉定位指的是机器人通过摄像头获取周围环境景物的图像，对图像中包含的一些明显的自然和人造特征，利用图像处理的方法进行识别，根据这些特征的位置信息得到机器人的位置。

在基于视觉的定位方法中，摄像头的安装有两种方式：一种是安装在机器人身上；另一种是安装在环境中。

安装在机器人身上的摄像头所看到的景物变换多、易被障碍物遮挡、图像处理计算量大，定位能力有限。

安装在环境中的摄像头所看到的景物变换少、不易被障碍物遮挡。

下面以单个视频监控设备安装在环境中且环境中只有一个移动机器人为例，说明这种基于视觉的全局定位方案。

安装在室内墙壁上的视频监控设备通过视频图像对其监控范围内移动的机器人进行定位。为了满足对监控范围内的移动机器人实时定位的要求，假设如下：

(1)机器人顶部装有一个与周围环境有明显区别的红色小球，用该红色小球作为机器人的标识物；

(2)红色小球的球心距地平面的高度记为h，所有机器人的高度h相同且不变化；

(3)所有机器人在同一个地平面上运动；

(4)摄像机的安装高度和俯仰度已知；

(5)摄像机已标定，即图像已几何修正。

透视投影是最常用的摄像机投影模型，可以用小孔成像模型简化，如图3所示，ABCD为摄像机拍摄到的地平面上的梯形区域，F点为摄像机焦点，FO为摄像机光轴，O点为摄像机光轴与地平面的交点(同时也是梯形ABCD对角线的交点)，O_G为F点在地平面上的垂直投影，FO_G为F点距地平面距离为H。o、a、b、c、d分别为O、A、B、C、D在像平面上的像点。F_O为焦距f。

按习惯，摄像机像平面坐标系称为u-v坐标系，原点在左上角，u轴向右为正向，v向下为正向；为了利用像平面上的机器人像对位于地平面上的机器人进行定位，须将u-v坐标系平移得到像平面坐标系xoy，其原点o位于像平面的中心，x轴与u轴同向，y轴与v轴同向，如图3所示。已知每个像素点在u轴方向和v轴方向的尺寸分别为Δu和Δv，o点在u-v坐标系的坐标为(u₀,v₀)，因此，像素坐标(u,v)在xoy坐标系的坐标(x,y)由下式确定：

$\left\{\begin{array}{c}x=\left(u-u_0\right)\×\mathrm{\Δ}u\\ y=\left(v-v_0\right)\×\mathrm{\Δ}u\end{array}\right.---\left(18\right)$

位于摄像机监控范围内的一个机器人，利用视频图像对其定位的过程如下：

(1)拍摄一幅彩色数字图像

拍摄一幅其视频监控范围内的景物图像，生成u-v坐标系下的RGB24格式图像：

RGB24＝{R(i,j),G(i,j),B(i,j)|0≤i≤m-1,0≤j≤n-1}(19)

其中，m为u轴方向的像素数量，n为v轴方向的像素数量，0≤R(i,j)≤255，0≤G(i,j)≤255，0≤B(i,j)≤255。

(2)图像分割

为了将彩色图像中的红色部分分割出来，首先将RGB24格式图像转换为HSV模式图像，再按HSV模式中的红色集合将式(20)表示的彩色图像转换为一幅黑白二值图像，其中红色转换为白色像素，其它转换为黑色像素。

HSV＝{H(i,j),S(i,j),V(i,j)|0≤i≤m-1,0≤j≤n-1}(20)

其中，0≤H(i,j)≤360，0％≤V(i,j)≤100％，0≤R(i,j)≤255。

令M＝max[R(i,j),G(i,j),B(i,j)]，N＝min[R(i,j),G(i,j),B(i,j)]，则H(i,j)由下式确定：

H(i,j)＝0，M＝N(21a)

H(i,j)＝60×[G(i,j)-B(i,j)]，M≠N,M＝R(i,j)，(21b)

H(i,j)＝60×[B(i,j)-R(i,j)]+120，M≠N,M＝G(i,j)，(21c)

H(i,j)＝60×[R(i,j)-G(i,j)]+240，M≠N,M＝B(i,j)，(21d)

如果H(i,j)<0，则H(i,j)＝H(i,j)+360，(21e)

S(i,j)由下式确定：

S(i,j)＝0，M＝0(22a)

S(i,j)＝1-N/M，M≠0(22b)

V(i,j)由下式确定：

V(i,j)＝100×M/255，(23)

接着，我们确定红色在HSV模式下的阈值集合，有

Red＝{0≤H(i,j)≤11.or.341≤H(i,j)≤360；S(i,j)≥0.15；V(i,j)≥18}(24)

依据下式，将式(20)表示的HSV模式图像转换成黑白二值图。

BW＝{BW(i,j)|0≤i≤m-1,0≤j≤n-1}(25)

其中，

BW(i,j)＝1，{H(i,j),S(i,j),V(i,j)}∈Red(26a)

BW(i,j)＝0， $\{H(i,j),S(i,j),V(i,j)\}\&NotElement;\mathrm{Re}d---\left(26b\right)$

(3)求取机器人顶部红色小球(标识物)球心坐标p(x,y)。

首先，对式(25)表示黑白二值图像分别做行、列的白色像素直方图统计；再分别求出行、列直方图的局部最大值；接着根据行、列局部最大值确定白色像素聚集区；进一步依据机器人顶部小球标识物的先验知识确定出机器人位置区域；最后按式(27)求出机器人顶部红色小球(标识物)的球心坐标。

$\left\{\begin{array}{c}x=\left(\stackrel{\&OverBar;}{u}-u_0\right)\&times;\mathrm{\&Delta;}u\\ y=\left(\stackrel{\&OverBar;}{v}-v_0\right)\&times;\mathrm{\&Delta;}v\end{array}\right.---\left(27\right)$

其中，为白色像素聚集区域所有白色像素在u轴投影的算术平均值，为白色像素聚集区域所有白色像素在v轴投影的算术平均值。

机器人顶部红色小球(标识物)的先验知识包括其在地面投影的形状(聚集白色像素中任意两个像素的最长距离)、尺寸(转换成像素数量范围)等。

(4)机器人定位

机器人定位分为二步：第一步根据像素坐标p(x,y)求出对应的地平面位置坐标P(P_X,P_Y)；第二步根据机器人顶部红色小球球心高度h，求出机器人顶部红色小球在地平面投影(X,Y)。

像平面一点p(x,y)对应的地平面一点P(P_X,P_Y)，如图4所示。其中，点p在ox轴的投影为p_x，在oy轴的投影为p_y，且op_x＝x，op_y＝y；点p在O_GX_G轴的投影为P_X，在O_GY_G轴的投影为P_Y，且O_GP_X＝X_P，O_GP_Y＝Y_P；F_O＝f。

在图4中，直角三角形Fp_yp相似于直角三角形FP_YP，有

$\frac{P_{Y}P}{{\mathrm{FP}}_Y}=\frac{p_{y}p}{{\mathrm{Fp}}_y}---\left(28\right)$

将P_YP＝O_GP_X＝X_P， ${\mathrm{FP}}_Y=\sqrt{Y_P^2+H^2},$ p_yp＝op_x＝x， ${\mathrm{Fp}}_y=\sqrt{y^2+f^2}$ 代入式(28)，整理后可得：

$X_p=\sqrt{(Y_P^2+H^2)/(y^2+f^2)}\&times;x---\left(29\right)$

在图4中，由直角三角形Fop_y可得：

β＝arctan(op_y/Fo)＝arctan(y/f)(30)

在直角三角形FO_GP_Y中，有

Y_P＝O_GP_Y＝H×tan(γ+β)＝H×tan[γ+arctan(y/f)](31)

将式(31)代入式(29)，化简，并结合式(31)，可得依据p(x,y)求解P(P_X,P_Y)的公式：

$\left\{\begin{array}{c}{X}_p=\sqrt{\left\{1+{\tan }^2\&lsqb;\mathrm{\&gamma;}+\arctan \left(y/f\right)\&rsqb;\right\}/\left(y^2+f^2\right)}\&times;H\&times;x\\ Y_p=H\&times;\tan \&lsqb;\mathrm{\&gamma;}+\arctan \left(y/f\right)\&rsqb;\end{array}\right.---\left(32\right)$

再由图4，机器人顶部红色小球标识物位于空间位置点R处。点p和点P分别是空间位置点R在像平面和地平面的投影，因此，机器人的实际位置应是位置点R在地平面上的垂直投影Q(X,Y)。由图4和式(32)，可以得到求解Q(X,Y)的公式如下：

$\left\{\begin{array}{c}X=\sqrt{\left\{1+{\tan }^2\&lsqb;\mathrm{\&gamma;}+\arctan \left(y/f\right)\&rsqb;\right\}/\left(y^2+f^2\right)}\&times;\left(H-h\right)\&times;x\\ Y=\left(H-h\right)\&times;\tan \&lsqb;\mathrm{\&gamma;}+\arctan \left(y/f\right)\&rsqb;\end{array}\right.---\left(33\right)$

组合定位方法：

在基于里程计和视觉传感器的机器人组合定位设备中，采用基于里程计的局部定位装置的输出数据作为组合定位设备的定位输出数据，随着机器人运动距离的增加，局部定位装置的输出数据由于误差累积使得定位输出数据的误差越来越大，当机器人的运动距离达到一定值时，采用基于视觉传感器的全局定位装置的输出数据对机器人定位输出数据进行修正，如此这样，周而复始完成组合定位设备的定位功能。

由此可见，现有的定位方法主要存在以下一些问题：

(1)机器人定位过程与环境无交流：一般来说，需要依靠环境特征完成的全局定位过程由机器人独立完成，环境没有提供任何有效信息给机器人以协助完成定位工作；作为另一个极端，在某些方案中，完全依靠视频监控设备完成一个或几个机器人的全局定位，再将每个机器人的定位数据发送给机器人；不适用或难以适用多机器人应用场合；基于视觉的定位方法数据处理量大，难以满足实时定位要求；

(2)机器人自身携带的组合定位设备：现有的组合定位方案中，需要机器人携带完全的组合定位设备，设备复杂、重量大、耗电高；

(3)机器人定位数据有累积误差：机器人采用局部定位方法提供定位数据，而全局定位数据则被用于消除局部定位随时间、距离累计的误差；在两次定位误差消除的时间间隔内，定位误差随时间、距离累计而增大。

发明内容

为解决现有技术的不足，本发明的目的在于提供一种基于环境信息的室内移动机器人，以及该机器人的定位方法，其中，该机器人不仅携带的设备数量和种类得到简化，而且能够与视频监控系统有机结合，该机器人的定位方法不仅无累积误差，而且能够满足实时定位要求。

为了实现上述目标，本发明采用如下的技术方案：

一种基于环境信息的室内移动机器人，包括：机器人机械系统、驱动系统、感知系统、机器人—环境交互系统、人机交互系统和控制系统，其中，驱动系统包括左轮驱动器和右轮驱动器，感知系统包括左轮里程计和右轮里程计，其特征在于，前述机器人还带有无线数据接收器，用于接收覆盖环境的视频监控系统发出的全局定位数据信号，同时，机器人的顶部还装有一个与周围环境有明显区别的彩色小球作为其标识物，所有机器人在室内环境同一个地平面上运动，每个小球的球心距地平面的高度h相同且不变化，顶部的标识物的颜色和大小都一样。

前述的基于环境信息的室内移动机器人，其特征在于，前述机器人顶部的标识物的颜色为红色。

前述的基于环境信息的室内移动机器人，其特征在于，各个机器人之间均保持安全距离。

前述室内移动机器人定位的方法，其特征在于，包括以下步骤：

S1：机器人定时根据上一时刻的位姿数据(x_n-1,y_n-1,θ_n-1)，预测当前时刻机器人的位姿(x_n,y_n,θ_n)，n＝1,2,…,预测公式如下：

$\left\{\begin{array}{c}{x}_n=x_{n-1}+\frac{{\mathrm{\&Delta;m}}_R\left(n\right)+{\mathrm{\&Delta;m}}_L\left(n\right)}{2}{\mathrm{cos\&theta;}}_{n-1}\\ y_n=y_{n-1}+\frac{{\mathrm{\&Delta;m}}_R\left(n\right)+{\mathrm{\&Delta;m}}_L\left(n\right)}{2}{\mathrm{sin\&theta;}}_{n-1}\\ {\mathrm{\&theta;}}_n=\frac{m_R\left(n\right)-m_R\left(0\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(0\right)}{2a}+{\mathrm{\&theta;}}_0\end{array}\right.,n=1,2,\mathrm{...}$

其中，(x₀,y₀,θ₀)是已知的；

S2：机器人接收当前时刻视频监控系统发来的数据帧，解码得到当前时刻机器人的可能位置集合如下：

Loc＝{(X_i,Y_i)|i＝1,2,…,k₀}

其中，k₀≥1，表示该机器人的可能位置的数量，

所述数据帧的组成如下：帧同步、监控设备号、数据个数k₀、数据1、…、数据m、校验和；

S3：机器人根据最有可能原则从该机器人的候选位置集合选出一个位置作为该机器人的当前位置。

前述的定位方法，其特征在于，在步骤S1中，机器人的定时间隔与视频监控设备的拍摄间隔相同，并且保持同步，机器人接收到视频监控设备发来的数据信号后立即预测当前时刻机器人的位姿。

前述的定位方法，其特征在于，在步骤S3中，机器人从候选位置集合中找出一个与(x_n,y_n)最近距离的点(X_k,Y_k)作为机器人在当前t_n时刻的位置，即：

(x_n,y_n)＝(X_k,Y_k)，D_k＝min{D_i|i＝1,2,…,k₀}

其中， $D_i=\sqrt{{(x_n-X_i)}^2+{(y_n-Y_i)}^2},$ i＝1,2,…,k₀，1≤k≤k₀。

本发明的有益之处在于：

一、基于环境信息的室内移动机器人：

1、机器人与视频监控系统有机结合：处于视频监控系统覆盖环境中的每个机器人均采用基于里程计的局部定位方法预测自身当前时刻所处的位置，视频监控系统中的每个视频监控设备通过无线发射器发送该设备监控范围内所有机器人的全局位置信息，各个机器人通过自身携带的无线数据接收器接收到这个信息并作为自身的候选全局位置，结合预测位置并依据最有可能准则确定出自身位置；

2、简化机器人自身携带的组合定位设备：全局定位数据来自覆盖环境的视频监控系统，机器人自身只需携带局部定位设备，设备简单、重量轻、耗电低。

二、上述机器人的定位方法：

1、机器人定位数据取自全局定位数据，无累积误差；

2、同时在基于视觉的全局定位算法中，由于不需要明确各个定位数据确切属于哪一个机器人，大大简化了基于视觉的多机器人全局定位算法，满足实时定位要求。

附图说明

图1是轮式差速转向机器人的位姿定义示意图；

图2是机器人位姿推导示意图；

图3是透视投影模型；

图4是成像模型；

图5是覆盖环境的视频监控系统的组成示意图；

图6是本发明的机器人的结构示意图。

图中附图标记的含义：1-第一无线数据接收器天线、2-右手臂、3-壳体、4-右轮、5-左轮、6-左手臂、7-第二无线数据接收器天线、8-红色小球标识物。

具体实施方式

以下结合附图和具体实施例对本发明作具体的介绍。

第一部分：覆盖环境的视频监控系统

覆盖环境的视频监控系统包括若干视频监控设备，每个视频监控设备覆盖局部环境，该局部环境构成移动机器人局部坐标系，相邻视频监控设备覆盖的局部环境有部分重叠，整个覆盖环境构成移动机器人全局坐标系。

视频监控设备由摄像头、图像采集与处理计算机、无线数据发射器组成。其中，摄像头安装在室内墙壁上，安装高度和俯仰角度已知，并且其获取的景物图像已经过几何修正。

图5是本发明的覆盖环境的视频监控系统的一个具体实施例。该视频监控系统共由6个视频监控设备组成，分别记为视频监控设备CA、视频监控设备CB、视频监控设备CC、视频监控设备CD、视频监控设备CE、视频监控设备CF，每一个视频监控设备的功能都是相同的。每一个视频监控设备覆盖一部分环境(即局部环境)，该局部环境构成移动机器人局部坐标系，相应的，6个视频监控设备覆盖的局部环境分别记为局部环境A、局部环境B、局部环境C、局部环境D、局部环境E、局部环境F，相邻视频监控设备覆盖的局部环境有部分重叠(即图中阴影部分)，6个局部环境叠加覆盖全局环境(即共同构成整个覆盖环境)，整个覆盖环境构成移动机器人全局坐标系。

覆盖环境的视频监控系统作为监控范围内所有机器人的共用全局定位设备。

第二部分：基于环境信息的室内移动机器人

现有的机器人包括以下六个子系统：

(1)机器人的机械系统；

(2)驱动系统(包括左、右轮驱动器)；

(3)感知系统(包括左、右轮里程计，全局定位设备)；

(4)机器人—环境交互系统；

(5)人机交互系统；

(6)控制系统。

我们对现有的机器人的结构稍加做了改造，从而形成了本发明的基于环境信息的室内移动机器人，具体的，参照图6：

1、在机器人上安装一个无线数据接收器，使得机器人能够接收覆盖环境的视频监控系统发出的全局定位数据信号；

2、取消机器人感知系统中的全局定位设备，从而可以减轻机器人的重量、降低能源需求(包括由减轻重量引起的运动耗能减少和原有全局定位设备的耗电)；

3、在机器人顶部装有一个与周围环境有明显区别的红色小球，作为机器人的标识物，使得覆盖环境的监控系统对移动机器人的跟踪与定位变为对红色小球标识物的跟踪与定位，大大简化了基于视觉的全局定位算法。

每一个机器人的结构和功能都是相同的。所有机器人在室内环境同一个地平面上运动，并且每个机器人的顶部均装有一个相同的颜色(本实施例中选用的是红色)小球作为其标识物，每个小球的球心距地平面的高度h相同且不变化，各个机器人之间均保持安全距离(即机器人之间不会发生叠加现象)。

处于室内环境中的每一个移动机器人和覆盖室内环境的视频监控系统中的每一个视频监控设备处于同步工作状态，机器人和环境的有机结合可以实现机器人的实时定位。

本发明所涉及的机器人，其自身携带的组合定位设备中只有局部定位设备(覆盖环境的视频监控系统作为监控范围内所有机器人的共用全局定位设备)，大大简化了携带的设备数量和种类，从而使得机器人重量更轻，耗电更低。

第三部分：覆盖环境的视频监控系统定位室内移动机器人的方法

本发明的定位方法属于基于环境模型的组合定位方法，但与传统意义上的组合定位有较大差别。

下面详细介绍本发明的覆盖环境的视频监控系统定位室内移动机器人的方法。

步骤1：拍摄彩色数字图像

视频监控设备(摄像头)定时拍摄一幅监控区域的彩色数字图像，该彩色数字图像RGB24＝{R(i,j),G(i,j),B(i,j)|0≤i≤m-1,0≤j≤n-1}，其中，m为像平面u轴方向的像素数量，n为像平面v轴方向的像素数量，0≤R(i,j)≤255，0≤G(i,j)≤255，0≤B(i,j)≤255。

拍摄间隔应当满足机器人实时定位要求，在本实施例中，我们将拍摄间隔设定为完成步骤1到步骤4所花费的时间。

步骤2：将RGB24图像转换成HSV图像

视频监控设备(图像采集与处理计算机)将RGB24图像转换成HSV图像，具体过程如下：

HSV＝{H(i,j),S(i,j),V(i,j)|0≤i≤m-1,0≤j≤n-1}

其中，0≤H(i,j)≤360，0％≤V(i,j)≤100％，0≤R(i,j)≤255。

令M＝max[R(i,j),G(i,j),B(i,j)]，N＝min[R(i,j),G(i,j),B(i,j)]，则

H(i,j)由下式确定：

H(i,j)＝0，M＝N

H(i,j)＝60×[G(i,j)-B(i,j)]，M≠N,M＝R(i,j)，

H(i,j)＝60×[B(i,j)-R(i,j)]+120，M≠N,M＝G(i,j)，

H(i,j)＝60×[R(i,j)-G(i,j)]+240，M≠N,M＝B(i,j)，

如果H(i,j)<0，则H(i,j)＝H(i,j)+360，

S(i,j)由下式确定：

S(i,j)＝0，M＝0

S(i,j)＝1-N/M，M≠0

V(i,j)由下式确定：

V(i,j)＝100×M/255。

步骤3：确定红色在HSV模式下的阈值集合

Red＝{0≤H(i,j)≤11.or.341≤H(i,j)≤360；S(i,j)≥0.15；V(i,j)≥18}

步骤4：将彩色图像转换成黑白图像

BW＝{BW(i,j)0≤i≤m-1,0≤j≤n-1}，其中：

BW(i,j)＝1，{H(i,j),S(i,j),V(i,j)}∈Red

BW(i,j)＝0， $\{H(i,j),S(i,j),V(i,j)\}\&NotElement;\mathrm{Re}d$

步骤5：得到监控范围内所有机器人的白色像素聚集区域

横向的白色像素统计：

统计每行横向的白色像素数量,

$W=\left\{W\left(i\right)\right|W\left(i\right)={\mathrm{\&Sigma;}}_{j=0}^{n-1}W(i,j),0\&le;i\&le;m-1\},$

纵向的白色像素统计：

统计每列纵向的白色像素数量,

$H=\left\{H\left(j\right)\right|H\left(j\right)={\mathrm{\&Sigma;}}_{i=0}^{m-1}W(i,j),0\&le;j\&le;n-1\},$

其中，m为像平面u轴方向的像素数量，n为像平面v轴方向的像素数量，i为像素在像平面中的横坐标，j为像素在像平面中的纵坐标。

分别求出W所有局部最大值和H所有局部最大值，假设W有m₀个局部最大值、H有n₀个局部最大值，即

$W_{\max }=\{i_1,i_2,...,i_{m_0}\},0\&le;i_k\&le;m-1,k=1,2,...,m_0;$

$H_{\max }=\{j_1,j_2,...,j_{n_0}\},0\&le;j_k\&le;n-1,k=1,2,...,n_0;$

这样，BW图像中包含m₀×n₀个白色像素聚集区域，每个区域的几何中心点的坐标构成如下的集合：

R₀＝{R(i_k,j_l)|i_k∈W_max,j_l∈H_max,k＝1,2,…,m₀,l＝1,2,…,n₀}，

计算每个白色像素聚集区域的大小(像素数量)和形状(相距最远两个像素的距离)并依据机器人顶部小球标识物的先验知识，将大小和形状不符合先验知识的白色像素聚集区域的几何中心点的坐标从R₀中删除，得到监控范围内所有机器人的白色像素聚集区域几何中心点构成的集合R：

R＝{R(i_k,j_k)|R(i_k,j_k)∈R₀,k＝1,2,…,k₀}，

其中，k₀为该监控区域内机器人的数量。

步骤6：确定监控范围内所有机器人的位置集合并发送给该范围内每个机器人

首先，对每一个机器人的白色像素聚集区域求几何中心，得到像平面u-v坐标系下的像素坐标其中，为白色像素聚集区域所有白色像素在u轴投影的算术平均值，为白色像素聚集区域所有白色像素在v轴投影的算术平均值。

然后依据下式计算出像平面xoy坐标系下坐标(x,y)：

$\left\{\begin{array}{c}x=(\stackrel{\&OverBar;}{u}-u_0)\&times;\mathrm{\&Delta;}u\\ y=(\stackrel{\&OverBar;}{v}-v_0)\&times;\mathrm{\&Delta;}v\end{array}\right.$

其中，u₀、v₀、Δu和Δv为已知的摄像头参数。

得到所有机器人在像平面v坐标系下坐标集合：

R_xy＝{(x_k,y_k)|k＝1,2,…,k₀}

其次，如图4所示，根据(x_k,y_k)得到机器人在该监控设备局部空间坐标系X_GO_GY_G下的坐标(X_k,Y_k)：

$\left\{\begin{array}{c}{X}_k=\sqrt{\{1+{\tan }^2\&lsqb;\mathrm{\&gamma;}+arctan(y_k/f)\&rsqb;\}/\left(y_k^2+f^2\right)}\&times;(H-h)\&times;x_k\\ Y_k=(H-h)\&times;tan\&lsqb;\mathrm{\&gamma;}+arctan(y_k/f)\&rsqb;\end{array}\right.$

其中，h为机器人高度，H为摄像头安装高度，γ为摄像头安装的俯仰角，f为摄像头焦距。

得到所有机器人在该摄像头局部空间坐标系X_GO_GY_G下坐标集合：

R_XY＝{(X_k,Y_k)|k＝1,2,…,k₀}

再次，将R_XY转换成视频监控系统所覆盖环境的全局坐标系下的全局坐标：

$R_{XY}^G=\left\{(X_k^G,Y_k^G)\right|k=1,2,...,k_0\}$

最后，将组成数据帧并通过该监控设备的无线数据发射器将数据帧发送出去。其中，数据的组成如下：

帧同步、监控设备号、数据个数k₀、数据1、…、数据m、校验和。

其中，帧同步是无线数据接收器判别一帧数据开始的标志，监控设备号表示数据是谁发送的，数据个数k₀给出了数据帧的长度信息，m是有效数据数量，与数据格式和k₀有关；校验和是无线数据接收器检验接收数据正确性的主要依据。

步骤7：机器人预测当前时刻的位姿

机器人定时根据上一时刻的位姿数据(x_n-1,y_n-1,θ_n-1)，预测当前时刻机器人的位姿(x_n,y_n,θ_n)，n＝1,2,...，预测公式如下：

$\left\{\begin{array}{c}{x}_n=x_{n-1}+\frac{{\mathrm{\&Delta;m}}_R\left(n\right)+{\mathrm{\&Delta;m}}_L\left(n\right)}{2}{\mathrm{cos\&theta;}}_{n-1}\\ y_n=y_{n-1}+\frac{{\mathrm{\&Delta;m}}_R\left(n\right)+{\mathrm{\&Delta;m}}_L\left(n\right)}{2}{\mathrm{sin\&theta;}}_{n-1}\\ {\mathrm{\&theta;}}_n=\frac{m_R\left(n\right)-m_R\left(0\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(0\right)}{2a}+{\mathrm{\&theta;}}_0\end{array}\right.,n=1,2,\mathrm{...}$

其中，(x₀,y₀,θ₀)是已知的。

机器人的定时间隔与视频监控设备的拍摄间隔相同，并且保持同步，即机器人接收到视频监控设备发来的数据信号(数据帧)后，立即预测当前时刻机器人的位姿。

步骤8：机器人解码得到当前时刻的可能位置集合

机器人接收当前时刻视频监控系统发来的数据信号(数据帧)，解码得到当前时刻机器人的可能位置集合如下：

Loc＝{(X_i,Y_i)|i＝1,2,…,k₀}

其中，k₀≥1，表示该机器人的可能位置的数量。

在这一步中，一个机器人可以收到至少一个视频监控设备发来的数据信号。

如果一个机器人收到了两个或两个以上视频监控设备发来的数据信号，则说明该机器人处于监控的重叠区域。

一个机器人收到的某个视频监控设备发来的数据信号，如果只包含一个机器人的候选位置，那么这个位置信息可以作为该机器人的初始位置信息使用。

步骤9：机器人选出当前位置

机器人根据最有可能原则从该机器人的候选位置集合选出一个位置作为该机器人的当前位置。

在这里，最有可能即是最短距离的，即机器人从候选位置集合中找出一个与(x_n,y_n)最近距离的点(X_k,Y_k)作为机器人在当前t_n时刻的位置，即：

(x_n,y_n)＝(X_k,Y_k)，D_k＝min{D_i|i＝1,2,…,k₀}

其中， $D_i=\sqrt{{(x_n-X_i)}^2+{(y_n-Y_i)}^2},$ i＝1,2,…,k₀，1≤k≤k₀。

由于机器人之间留有安全距离并且时间间隔足够短，所以不可能发生有相同的最短距离出现的情形。

由此可见，本发明的机器人与视频监控系统有机的结合到了一起，其中，机器人携带的局部定位设备提供预测位置，覆盖环境的视频监控系统提供候选位置集合，机器人采用最有可能准则从候选位置集合中确定出机器人当前时刻位置。

由于全局定位数据集合提供的仅是监控范围内移动机器人的位置信息，并不指明是哪一个机器人的位置信息，所以满足了机器人实时定位的要求。

此外，由于机器人定位数据采用的是全局定位数据，所以定位数据无累积误差。

需要说明的是，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。

Claims (6)

1.一种基于环境信息的室内移动机器人，包括：机器人机械系统、驱动系统、感知系统、机器人—环境交互系统、人机交互系统和控制系统，其中，驱动系统包括左轮驱动器和右轮驱动器，感知系统包括左轮里程计和右轮里程计，其特征在于，所述机器人还带有无线数据接收器，用于接收覆盖环境的视频监控系统发出的全局定位数据信号，同时，机器人的顶部还装有一个与周围环境有明显区别的彩色小球作为其标识物，所有机器人在室内环境同一个地平面上运动，每个小球的球心距地平面的高度h相同且不变化，顶部的标识物的颜色和大小都一样。

2.根据权利要求1所述的基于环境信息的室内移动机器人，其特征在于，所述机器人顶部的小球标识物的颜色为红色。

3.根据权利要求1所述的基于环境信息的室内移动机器人，其特征在于，各个机器人之间均保持安全距离。

4.权利要求1、2或3所述的室内移动机器人的定位方法，其特征在于，包括以下步骤：

S1：机器人定时根据上一时刻的位姿数据(x_n-1,y_n-1,θ_n-1)，预测当前时刻机器人的位姿(x_n,y_n,θ_n)，n＝1,2,...,预测公式如下：

$\left\{\begin{array}{c}{x}_n=x_{n-1}+\frac{{\mathrm{\&Delta;m}}_R\left(n\right)+{\mathrm{\&Delta;m}}_L\left(n\right)}{2}{\mathrm{cos\&theta;}}_{n-1}\\ y_n=y_{n-1}+\frac{{\mathrm{\&Delta;m}}_R\left(n\right)+{\mathrm{\&Delta;m}}_L\left(n\right)}{2}{\mathrm{sin\&theta;}}_{n-1}\\ {\mathrm{\&theta;}}_n=\frac{m_R\left(n\right)-m_R\left(0\right)}{2a}-\frac{m_L\left(n\right)-m_L\left(0\right)}{2a}+{\mathrm{\&theta;}}_0\end{array}\right.,n=1,2,\mathrm{...}$

其中，(x₀,y₀,θ₀)是已知的；

S2：机器人接收当前时刻视频监控系统发来的数据帧，解码得到当前时刻机器人的可能位置集合如下：

Loc＝{(X_i,Y_i)|i＝1,2,...,k₀}

其中，k₀≥1，表示该机器人的可能位置的数量，

所述数据帧的组成如下：帧同步、监控设备号、数据个数k₀、数据1、…、数据m、校验和；

S3：机器人根据最有可能原则从该机器人的候选位置集合选出一个位置作为该机器人的当前位置。

5.根据权利要求4所述的定位方法，其特征在于，在步骤S1中，机器人的定时间隔与视频监控设备的拍摄间隔相同，并且保持同步，机器人接收到视频监控设备发来的数据信号后立即预测当前时刻机器人的位姿。

6.根据权利要求4所述的定位方法，其特征在于，在步骤S3中，机器人从候选位置集合中找出一个与(x_n,y_n)最近距离的点(X_k,Y_k)作为机器人在当前t_n时刻的位置，即：

(x_n,y_n)＝(X_k,Y_k)，D_k＝min{D_i|i＝1,2,…,k₀}

其中， $D_i=\sqrt{{(x_n-X_i)}^2+{(y_n-Y_i)}^2},i=1,2,...,k_0,1\&le;k\&le;k_0.$