CN106997205B - 一种用于移动机器人对目标定位和跟踪的系统及方法 - Google Patents

Abstract

本发明公开了一种用于移动机器人对目标定位和跟踪的系统，其包括设置在目标上的RFID标签，设置在移动机器人上的RFID天线，与RFID天线相连接的RFID阅读器，与RFID阅读器相连接的运动控制系统，以及与运动控制系统相连接的激光传感器。本发明利用RFID信号强度信息和相位信息对标签的位置和运动速度进行估算，同时移动机器人通过运动控制系统，实现动态RFID标签的实时跟踪，并通过激光传感器进行避障。系统通过粒子滤波器来实现相位信息和信号强度信息的融合从而实现动态RFID标签的快速追踪。

Description

一种用于移动机器人对目标定位和跟踪的系统及方法

技术领域

本发明涉及物联网机器人领域，具体涉及一种用于移动机器人对目标定位和跟踪的系统及方法。

背景技术

随着物联网技术的发展，在互联网基础上，利用通信技术将传感设备、机器、人和物品连接起来，以实现智能化识别、定位、跟踪、监控和管理。射频识别(Radio FrequencyIdentification，RFID)使用无线射频信号进行对象识别，RFID阅读器不需要与标签建立机械或光学接触；RFID标签可附着在物体上用于货物自动识别和跟踪，可克服传统视觉必须在视野内进行识别的缺点。另外，RFID无源标签识别距离可达7-10米，价格低廉、体积小巧，广泛应用于超市、机场、物流、零售等方面。机器人领域一直是前沿高技术研究最活跃的一部分，而机器人作为一个移动智能体在某些环境中感知标签，可为人类提供帮助和导航等服务。

发明内容

针对现有技术中的上述不足，本发明提供的一种用于移动机器人对目标定位和跟踪的系统提供了一种智能化识别、定位和跟踪目标的系统。

为了达到上述发明目的，本发明采用的技术方案为：

提供一种用于移动机器人对目标定位和跟踪的系统，其包括设置在目标上的RFID标签，设置在移动机器人上的RFID天线，与RFID天线相连接的RFID阅读器，与RFID阅读器相连接的运动控制系统，以及与运动控制系统相连接的激光传感器；

RFID标签，用于反射RFID天线所发送的无线射频信号；

RFID天线，用于发送无线射频信号并接收由RFID标签所反射的无线射频信号；

RFID阅读器，用于通过RFID天线获取RFID标签的信号强度和相位信息；

激光传感器，用于通过激光测得目标障碍物的距离；

运动控制系统，用于通过粒子滤波器融合RFID标签的信号强度和相位信息，实现RFID标签的定位；同时根据RFID标签的相位信息估算RFID标签的运动速度，以及根据激光传感器测定的目标障碍物的距离，配合VFH+避障算法进行导航跟踪。

提供一种用于移动机器人对目标定位和跟踪的方法，其包括以下步骤：

S1、RFID天线发射无线射频信号并接收由设置在目标上的RFID标签所反射的信号，并将该反射信号发送至RFID阅读器；

S2、RFID阅读器根据该反射信号获取RFID标签的信号强度信息和相位信息，并将该信号强度信息和相位信息发送至运动控制系统；

S3、运动控制系统根据接收到的RFID标签的信号强度信息和相位信息，将每个RFID标签的位置用一个粒子滤波器表示，通过粒子滤波器融合RFID标签的信号强度和相位信息，当RFID标签的信号强度低于阈值时，返回步骤S1；RFID标签的信号强度大于或等于阈值时，实现RFID标签的定位；

同时，运动控制系统通过RFID标签的相位信息估算RFID标签的运动速度，以及根据激光传感器测定的目标障碍物的距离，配合VFH+避障算法进行导航跟踪。

进一步地，步骤S3中运动控制系统根据接收到的RFID标签的信号强度信息和相位信息，将每个RFID标签的位置用一个粒子滤波器表示，通过粒子滤波器融合RFID标签的信号强度和相位信息，实现RFID标签的定位的方法为：

(1)初始化粒子滤波器；

(2)根据RFID标签的相位信息估算RFID标签的速率；

(3)粒子滤波器利用相位估计的速率信息对粒子进行预测；

(4)利用RFID传感器模型更新每个粒子的权重；

(5)使用重采样算法对更新权重后的粒子进行重采样，根据“达尔文进化论”，保留权重大的粒子，去除权重小的粒子；

(6)对粒子进行扰动，并对扰动后的粒子求平均，以得到RFID标签的估算位置。

进一步地，粒子滤波器为双重粒子滤波器，根据公式：

使用速率信息对RFID标签的状态进行预测，避免追踪失败；

其中V为RFID标签的速度，π(Z_t,X_t)＝∫P(Z_t|X_t)dX_t，表示均值为方差为v_t高斯模型，p(Z_t|X_t)为传感器模型表示在X_t处阅读器得到标签测量数据Z_t的概率，α为0至1之间的常数。

进一步地，RFID传感器模型为：

p(Z|(x，y))＝p(d|(x，y))p(s|d,(x，y))

根据在某一给定位置上RFID的信号强度遵循均值为u_x方差为σ_x的高斯分布，此位置RFID标签信号强度为s的概率为：

其中p(d|(x，y))表示在位置(x，y)处检测到RFID标签的概率大小，p(s|d,(x，y))表示在(x，y)处检测到RFID标签的信号强度为s的概率大小，π为常数。

进一步地，RFID传感器模型对粒子滤波器进行更新的方法为：

W_t＝η_t·W_t-1·p(Z_t|X_t)

其中W_t为粒子的权重，η_t为归一化参数，p(Z_t|X_t)是在位置X_t处阅读器得到标签测量数据Z_t的概率。

进一步地，对粒子进行重采样，对粒子进行扰动，并对扰动后的粒子求平均，以得到RFID标签的估算位置的方法为：根据公式

计算粒子的新位置，即RFID标签的位置；其中和来表示粒子滤波器的均值和协方差，ρ为常数，N为粒子数量，X⁽ⁱ⁾为粒子的位置。

本发明的有益效果为：

1、本发明利用RFID信号强度信息和相位信息对标签的位置和运动速度进行估算，同时移动机器人通过运动控制系统，实现动态RFID标签的实时跟踪，并通过激光传感器进行避障。系统通过粒子滤波器来实现相位信息和信号强度信息的融合从而实现动态RFID标签的快速追踪。

2、每个RFID标签提供唯一ID用于目标识别，因此RFID不需要复杂的目标分类识别算法，RFID使用无线射频信号与标签进行通信，无线信号可以穿透障碍物，使得本发明不需要与标签建立机械或光学接触，可以很好的解决视觉的遮挡和外界光线的影响问题。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

该用于移动机器人对目标定位和跟踪的系统包括设置在目标上的RFID标签，设置在移动机器人上的RFID天线，与RFID天线相连接的RFID阅读器，与RFID阅读器相连接的运动控制系统，以及与运动控制系统相连接的激光传感器；

RFID标签，用于反射RFID天线所发送的无线射频信号；

RFID天线，用于发送无线射频信号并接收由RFID标签所反射的无线射频信号；

RFID阅读器，用于通过RFID天线获取RFID标签的信号强度和相位信息；

激光传感器，用于通过激光测得目标障碍物的距离；

运动控制系统，用于通过粒子滤波器融合RFID标签的信号强度和相位信息，实现RFID标签的定位；通过RFID标签的相位信息估算RFID标签的运动速度，以及激光传感器测定的目标障碍物的距离，配合VFH+避障算法进行导航跟踪。

该用于移动机器人对目标定位和跟踪的方法包括以下步骤：

S1、RFID天线发射无线射频信号并接收由设置在目标上的RFID标签所反射的信号，并将该反射信号发送至RFID阅读器；

S2、RFID阅读器根据该反射信号获取RFID标签的信号强度信息和相位信息，并将该信号强度信息和相位信息发送至运动控制系统；

S3、运动控制系统根据接收到的RFID标签的信号强度信息和相位信息，将每个RFID标签的位置用一个粒子滤波器表示，通过粒子滤波器融合RFID标签的信号强度和相位信息，当RFID标签的信号强度低于阈值时，返回步骤S1；RFID标签的信号强度大于或等于阈值时，实现RFID标签的定位；

同时，运动控制系统通过RFID标签的相位信息估算RFID标签的运动速度，以及根据激光传感器测定的目标障碍物的距离，配合VFH+避障算法进行导航跟踪。

步骤S3中运动控制系统根据接收到的RFID标签的信号强度信息和相位信息，通过粒子滤波器融合RFID标签的信号强度和相位信息，实现RFID标签的定位的方法为：

(1)初始化粒子滤波器；

(2)根据RFID标签的相位信息估算RFID标签的速率；

(3)粒子滤波器根据速率信息对粒子进行预测；

(4)粒子滤波器利用RFID传感器模型更新每个粒子的权重；

(5)使用重采样算法对更新权重后的粒子进行重采样，根据“达尔文进化论”，保留权重大的粒子，去除权重小的粒子；

(6)对粒子进行扰动，对扰动后的粒子求平均，以得到RFID标签的估算位置。此处粒子进行重采样的具体方法为对所有的粒子权重进行归一化，保证所有粒子权重加起来为1，用一个数组来表示粒子的累积权重和，然后随机产生0到1的均匀分布，判断这个随机数落在数组的哪个区间，然后这个区间对应的粒子就被保存下来，重复此过程，直到我们产生了N个粒子。假设我们有三个粒子(N＝3)，他们的权重分别为0.1，0.2，0.7，我们用如下数组来表示累计权重数组[0.1，0.3，1]，我们产生三个随机数，如0.2，0.54，0.78，根据此随机数我们会选择第二个粒子(0.54)一次，第三个粒子(0.78)两次来构成新的粒子组合来进行位置的估算。

粒子滤波器为双重粒子滤波器，在每次获得到RFID信号强度后，系统会根据当前的信号强度大小增加一定的随机粒子，并根据当前的概率分布函数赋予这些粒子一定的权重。使用双重粒子滤波器产生α粒子，剩下的粒子使用相位估计的速度v进行预测。但是新增加的这些粒子往往会破坏当前的粒子分布情况，这一问题根据当前的概率分布函数赋予这些粒子一定的权重：通过核函数来构造后验概率，然后决定粒子的权重大小。因为预先已经建立了一个非常精确的RFID传感器模型，因此在当前测量信号强度的基础上增加随机粒子是非常容易的。双重粒子滤波器产生α粒子，剩下的粒子根据相位信息得到的速度v进行预测，(0≤α≤1)根据公式：

使用速度信息对RFID标签的状态进行预测，并增加随机扰动的粒子，避免追踪失败；

其中π(Z_t,X_t)＝∫P(Z_t|X_t)dX_t，表示均值为方差为v_t高斯模型，p(Z_t|X_t)为传感器模型表示在X_t处阅读器得到标签测量数据Z_t的概率。

RFID传感器模型描述了在某一位置(x,y)处RFID天线得到了标签测量数据z的概率大小，由于RFID天线可以提供是否检测到这个标签和相应的信号强度，RFID的传感器模型可以表述为：

p(Z|(x，y))＝p(d|(x，y))p(s|d,(x，y))

根据在某一给定位置上RFID的信号强度遵循均值为u_x方差为σ_x的高斯分布，此位置RFID标签信号强度为s的概率为：

其中p(d|(x，y))表示在位置(x，y)处检测到RFID标签的概率大小，p(s|d,(x，y))表示在(x，y)处检测到RFID标签的信号强度为s的概率大小，π为常数。

在本发明的一个实施例中，移动机器人安装了一个SICK S300型号的激光传感器(该激光测传感器具有240°的测量角度)。此外，移动机器人还携带有一个超高频射频识别读写器(Impinj speedway R1000)，这个RFID阅读器提供最大识别范围可达10米。机器人在0.8米高度处安装2个与其前进方向成45°的圆极天线。

采用粒子滤波器融合RFID标签的相位信息和强度信息，将每个RFID标签的位置用一个粒子滤波器表示，每个粒子滤波器由若干个粒子组成，每个粒子包括一个位置假设以及相应的权重，根据公式

其中

即可得到RFID标签的径向速度v；

根据递归贝叶斯估计公式

p(X_t|Z_1：t，v_1：t)＝η_t·p(Z_t|X_t)·p(X_t|X_t-1，v_t)·p(X_t-1|Z_1：t-1，v_1：t-1)

得到标签位置的后验概率，这个后验概率代表标签的估算位置；

根据公式

w_t＝η_t·w_t-1·p(Z_t|X_t)

RFID传感器模块对每一个粒子的权重进行更新，然后使用重采样算法对粒子进行重采样，根据“达尔文进化论”，保留权重大的粒子，去除权重小的粒子；

重采样之后根据公式

对粒子进行扰动，让粒子往RFID标签的真实位置进行汇聚，防止粒子发散，对扰动后的粒子求平均得到RFID标签的估算位置，ρ为常数，其取值区间为0.95至0.98，取值越小，表示扰动越大；

其中λ为波长，f为载波频率，c为光速，t₀和t₁分别为在相邻时间同一RFID标签被检测的时间，θ₁和θ₀分别为在相邻时间同一RFID标签被检测的相位，X_t是RFID标签在t时刻的位置，Z_1：t是RFID标签在1：t时刻的RFID测量数据，v_1：t是使用相位信息获取的RFID标签的速度信息，p(Z_t|X_t)是在位置X_t处阅读器得到标签测量数据Z_t的概率，p(X_t-1|Z_1：t-1，v_1：t-1)为RFID标签在时刻t-1的状态，w_t为当前粒子的权重，η_t为归一化参数，和分别为粒子滤波器的均值和协方差，N为粒子数量。

传统方法对RFID标签的定位需要建立一个比较精确的传感器模型。这个模型可以预测机器人在不同位置检测到标签概率以及相应的信号强度。由于RFID读写特性受环境的影响很大，尤其是RFID信号对金属和液体反应比较敏感。所以本发明通过融合多传感器的信息来提高标签的定位精度通过粒子滤波器融合不同的运动信息来实现动态RFID标签的快速追踪。

构建RFID传感器模型的方法为：

在环境中安装若干已知位置的RFID标签，移动机器人移动并搜集环境中的RFID的信号强度信息，在这一阶段中RFID的信号强度和机器人本身的位置数据被记录了下来，然后通过以下两个步骤自动构建传感器模型：

a.数据预处理：

在每一个记录数据的时间点，通过机器人和天线的坐标关系得到标签和天线的之间的相对位置。这样每个测量数据相对每个标签产生一个测量样本，这个样本包括标签相对与天线的位置和标签的测量数据(是否检测到标签以及标签信号强度的大小)。假设RFID天线的最大测量距离为D，将传感器模型的范围限制在[0：D，-D/2：D/2]。y坐标的范围是对称的，因为RFID阅读器的信号传播特性是对称的。

b.构建模型：

使用上一阶段产生的样本来计算传感器模型p(z|x)，这个模型给出了在RFID天线的坐标系下X位置得到一个测量值z的概率。这个传感器模型将会被用于粒子滤波器的粒子权重的更新。使用二维栅格来表示传感器模型，整个模型空间被离散化二维栅格，对于每一个栅格，根据公式：

得到RFID标签在每个栅格内被检测的概率，并得到RFID标签的平均信号强度和方差，将其保存并用于RFID标签的定位和追踪；其中为落在每一个栅格内的正检测个数，为落在每一个栅格内的负检测个数；如果在某一时刻检测到某一标签，我们定义为正检测，否则为负检测。本模型可以预测移动机器人在不同位置处检测到RFID标签的概率以及相应的信号强度。

本发明在具体实施过程中，由于移动机器人仅在二维中移动，移动机器人在时间t时刻的状态可以表示为X_t＝(x_t,y_t,θ_t)。其中x_t和y_t是机器人相对于世界坐标系的坐标，θ_t是机器人的旋转角度。移动机器人的运动被看做是在2D空间中的转换，机器人在时间t的平移为Δd_t，旋转为Δθ_t。机器人的运动模型可表示为：

θ_t+1＝θ_t+Δθ_t

本发明使用的差速驱动机器人进行运动控制，机器人主体两侧配备有两个独立运行的车轮和电机。机器人前端安装一个额外万向轮来平衡机器人。电机编码器能提供关于机器人电机的运动的信息，可以被转换成车轮的速度或车轮行进的距离。机器人旋转的中心点被称为瞬时曲率中心(ICC)。这一点在短时间内几乎没有速度。机器人的运动通过改变两个电机的速度来实现。如果两个轮子以相同的速度和相同的方向旋转，机器人将直线移动。否则，根据两个车轮的速度的大小，机器人会进行相应的运动。

由于左右轮围绕ICC的旋转速度w相同，因此：

v_l＝w(R-L/2) v_r＝w(R-L/2)

其中L表示两个车轮之间的距离，这是已知参数；v_l和v_l分别是左右轮的速度，R是从瞬时曲率中心点(ICC)到机器人中心的距离。在任何时间间隔Δt处，机器人移动的距离Δd和方向Δθ可以表示为：

这意味着机器人的运动可以完全由两个车轮的速度决定。

综上所述，本发明提出了一种基于移动机器人和RFID结合的动态标签定位和追踪方法。该发明利用RFID信号强度信息和相位信息对标签的位置和运动速度进行估算，同时移动机器人通过运动控制系统，实现动态RFID标签的实时跟踪，并通过激光传感器进行避障。系统通过粒子滤波器来实现相位信息和信号强度信息的融合从而实现动态RFID标签的快速追踪。

Claims (4)

1.一种用于移动机器人对目标定位和跟踪的方法，其特征在于：包括以下步骤：

S1、RFID天线发射无线射频信号并接收由设置在目标上的RFID标签所反射的信号，并将该反射信号发送至RFID阅读器；

S2、RFID阅读器根据该反射信号获取RFID标签的信号强度信息和相位信息，并将该信号强度信息和相位信息发送至运动控制系统；

S3、运动控制系统根据接收到的RFID标签的信号强度信息和相位信息，将每个RFID标签的位置用一个粒子滤波器表示，通过粒子滤波器融合RFID标签的信号强度和相位信息，当RFID标签的信号强度低于阈值时，返回步骤S1；RFID标签的信号强度大于或等于阈值时，实现RFID标签的定位；

同时，运动控制系统根据RFID标签的相位信息估算RFID标签的运动速度，以及根据激光传感器测定的目标障碍物的距离，配合VFH+避障算法进行导航跟踪；

步骤S3中运动控制系统根据接收到的RFID标签的信号强度信息和相位信息，将每个RFID标签的位置用一个粒子滤波器表示，通过粒子滤波器融合RFID标签的信号强度和相位信息，实现RFID标签的定位的方法为：

(1)初始化粒子滤波器；

(2)根据RFID标签的相位信息估算RFID标签的速率；

(3)粒子滤波器利用相位估计的速率信息对粒子进行预测；

(4)利用RFID传感器模型更新每个粒子的权重；

(5)使用重采样算法对更新权重后的粒子进行重采样，根据“达尔文进化论”，保留权重大的粒子，去除权重小的粒子；

(6)对粒子进行扰动，并对扰动后的粒子求平均，以得到RFID标签的估算位置；

粒子滤波器为双重粒子滤波器，根据公式：

使用速率信息对RFID标签的状态进行预测，避免追踪失败；

其中V为RFID标签的速度，π(Z_t，X_t)＝∫P(Z_t|X_t)dX_t，表示均值为方差为v_t高斯模型，p(Z_t|X_t)为传感器模型表示在X_t处阅读器得到标签测量数据Z_t的概率，α为0至1之间的常数。

2.根据权利要求1所述的用于移动机器人对目标定位和跟踪的方法，其特征在于：RFID传感器模型为：

p(Z|(x，y))＝p(d|(x，y))p(s|d，(x，y))

根据在某一给定位置上RFID的信号强度遵循均值为u_x方差为σ_x的高斯分布，此位置RFID标签信号强度为s的概率为：

其中p(d|(x，y))表示在位置(x，y)处检测到RFID标签的概率大小，p(s|d，(x，y))表示在(x，y)处检测到RFID标签的信号强度为s的概率大小，π为常数。

3.根据权利要求1所述的用于移动机器人对目标定位和跟踪的方法，其特征在于，RFID传感器模型对粒子滤波器进行更新的方法为：

W_t＝η_t·W_t-1·p(Z_t|X_t)

其中W_t为粒子的权重，η_t为归一化参数，p(Z_t|X_t)是在位置X_t处阅读器得到标签测量数据Z_t的概率。

4.根据权利要求1所述的用于移动机器人对目标定位和跟踪的方法，其特征在于，对粒子进行重采样，对粒子进行扰动，并对扰动后的粒子求平均，以得到RFID标签的估算位置的方法为：根据公式

计算粒子的新位置，即RFID标签的位置；其中和来表示粒子滤波器的均值和协方差，ρ为常数，N为粒子数量，X⁽ⁱ⁾为粒子的位置。