[发明专利]一种用于移动机器人对目标定位和跟踪的系统及方法

权利要求书

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⁽ⁱ⁾为粒子的位置。