CN110442014A - 一种基于位置的移动机器人rfid伺服方法 - Google Patents

Abstract

本发明属于移动机器人伺服相关技术领域，其公开了一种基于位置的移动机器人RFID伺服方法，该方法包括以下步骤：(1)在目标物体存在的位置空间内随机生成预定数量的粒子；(2)设置在移动机器人上的RFID读写器不断地收集目标物体上的RFID标签的相位信息，进而得到相位差值的实际测量值；(3)利用对传感器信息进行处理以得到安装于移动机器人上的RFID天线的位置信息，进而得到相位差值的理论估计值；(4)通过粒子滤波算法计算得到RFID标签在当前移动机器人坐标系下的位置估计值；(5)评估位置估计值的稳定性，并根据评估结果进行相应处理，直至移动机器人与目标物体达到期望位置关系。本发明易于实施，适用性较好。

Description

一种基于位置的移动机器人RFID伺服方法

技术领域

本发明属于移动机器人伺服相关技术领域，更具体地，涉及一种基于位置的移动机器人RFID伺服方法。

背景技术

RFID(Radio Frequency Identification)亦称射频识别，是物联网的关键技术之一，相对于条形码与二维码等具有非视距、远距离、防伪性好、信息量大等优点，主要用于超市、仓库等领域物品的识别与管理。将RFID与移动机器人相结合以形成移动机器人RFID伺服系统，该系统可以将移动机器人的工作空间大的优势与RFID的唯一ID识别特点相结合，实现移动机器人在大范围的工作空间的伺服导航控制，有助于提高物品的管理效率。

目前利用RFID实现移动机器人伺服的方法大多需要在空间内布置较为密集的参考标签阵列，并且离线标定好每个参考标签的空间位置信息，该方法系统复杂，成本较高，实际实施困难。相应地，本领域存在着发展一种易于实施的基于位置的移动机器人RFID伺服方法的技术需求。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于位置的移动机器人RFID伺服方法，其基于现有移动机器人伺服的实现特点，研究及设计了一种易于实施的基于位置的移动机器人RFID伺服方法。所述方法利用安装有RFID读写器及RFID天线的移动机器人来计算装有RFID标签的目标物体相对于移动机器人的位置信息，并利用得到的位置信息与期望位置的偏差来对移动机器人进行反馈控制，直到两者满足期望的位置关系，该方法简单，实时性好，鲁棒性强，实施方便。

为实现上述目的，本发明提供了一种基于位置的移动机器人RFID伺服方法，该方法包括以下步骤：

(1)在目标物体存在的位置空间内随机生成预定数量的粒子，每个粒子代表了目标标签的备选位置点；

(2)随着移动机器人的移动，设置在所述移动机器人上的RFID读写器不断地收集所述目标物体上的RFID标签的相位信息，同时利用解缠算法对所述相位信息进行处理以得到解缠相位，并将当前时刻的解缠相位与前一时刻的解缠相位相减以得到相位差值的实际测量值；其中，RFID标签即为目标标签；

(3)基于所述移动机器人上安装的惯性传感器检测到的传感器信息，利用航迹推测法与旋转平移变换对所述传感器信息进行处理以得到安装于所述移动机器人上的RFID天线的位置信息，并根据所述RFID天线的位置信息与所述粒子的位置信息计算当前时刻的相位理论估计值，进而将当前时刻的相位理论估计值与前一时刻的相位理论估计值相减，以得到相位差值的理论估计值；

(4)基于相位差值的实际测量值与相位差值的理论估计值，通过粒子滤波算法计算得到RFID标签在当前移动机器人坐标系下的位置估计值；

(5)评估所述位置估计值的稳定性，如果所述位置估计值处于不稳定状态，则跳过伺服控制环节，直接转至步骤(2)；否则，将所述位置估计值与期望值进行比较，若所述移动机器人与所述目标物体达到期望的位置关系，则结束；否则，转至步骤(2)，直至所述移动机器人与所述目标物体达到期望的位置关系。

进一步地，所述RFID天线的数量大于等于2。

进一步地，粒子滤波算法的后验观测模型的数学表达式为：

其中，W_t,j表示在t时刻、第j个粒子的权重值；σ表示相位测量的标准差；Δθ_t是t时刻相位差值的实际测量值；是t时刻、第j个粒子相位差值的理论估计值。

进一步地，RFID标签在当前移动机器人坐标系下的位置估计值的计算公式为：

式中，是目标标签在t时刻移动机器人坐标系下的位置估计值；η_t,j为t时刻、第j个粒子的归一化参数；表示粒子，i＝1，2….，M，表示的是粒子在移动机器人坐标系下的x坐标；表示的是粒子的y坐标；表示的是粒子的z坐标；M表示粒子的个数。

进一步地，相位解缠算法对所述相位信息进行处理时，令γ⁽¹⁾＝θ⁽¹⁾，并开始如下循环，直到处理完所有的相位信息：

当θ⁽ⁱ⁾-θ^(i-1)>Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))-2π；

当θ⁽ⁱ⁾-θ^(i-1)<-Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))+2π；

当-Ψ≤θ⁽ⁱ⁾-θ^(i-1)≤Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))；

其中，θ⁽ⁱ⁾是第i个相位测量时刻测得的相位；γ⁽ⁱ⁾是第i个相位测量时刻测得的相位对应的实际解缠相位；Ψ为0～2π中的任意值；i＝2～K，K为相位信息数量。

进一步地，旋转平移变换的方程式为：

其中，是第i个相位测量时刻对应的移动机器人方位，是移动机器人的位置，是移动机器人的方向，其通过航迹推测法得到；z_A是RFID天线在移动机器人坐标系中的z轴坐标；(ρ,α)是RFID天线在移动机器人坐标系中的极坐标；是第i个相位测量时刻对应的RFID天线的位置坐标。

进一步地，通过如下公式计算得到相位理论估计值：

其中，是指t时刻，RFID天线与第j个粒子的相位理论估计值；λ是信号波长。

进一步地，相位差值的理论估计值的计算公式为：

其中，为t时刻、第j个粒子的相位差值的理论估计值；是与t时刻相隔p个时刻时，第j个粒子的相位理论估计值。

进一步地，在执行控制算法之前，先将目标标签在移动机器人坐标系下的位置估计值由卡迪尔坐标形式转化为极坐标形式，转换公式为：

其中，是目标标签在t时刻移动机器人坐标系下的位置估计值；为移动机器人极坐标系下的极轴；为移动机器人极坐标系下的极角；如果则将极角调整为

进一步地，所述位置估计值的稳定性的判断公式为：

其中，是目标标签在i时刻移动机器人坐标系下的位置估计值；N代表稳定性判断中所用到的最近的标签位置估计值的数量；代表的是过去N个时刻目标标签在机器人坐标系下的位置估计值的平均值；threshold₁和threshold₂分别是需要设定的相关阈值。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的基于位置的移动机器人RFID伺服方法主要具有以下有益效果：

1.所述伺服方法利用安装有RFID读写器及其天线的移动机器人计算安装有RFID标签的目标物体相对于移动机器人的位置信息，并利用与期望位置的偏差，对移动机器人进行反馈控制，直到两者满足期望的位置关系，易于实施，适用性较好，鲁棒性强。

2.所述方法仅利用RFID系统及相应算法等进行实现，无需布置RFID参考标签系统，不需要离线标定标签的位置信息，流程简单，成本较低，适用性较强。

3.依据移动机器人的位姿构建移动机器人坐标系，不断计算目标标签在移动机器人坐标系下的位置，利用该位置信息对移动机器人进行伺服控制，直到目标标签与移动机器人之间满足期望的位置关系，实现了移动机器人的伺服控制，步骤简单，有利于推广应用。

4.得到标签在移动机器人坐标系下的位置信息后，可利用现有的诸多方法进行控制器设计，例如最优控制、PID控制及带有轨迹规划的控制算法设计控制器，灵活性较好，易于施行。

附图说明

图1是本发明提供的基于位置的移动机器人RFID伺服方法的流程示意图；

图2是图1中的基于位置的移动机器人RFID伺服方法的框架示意图；

图3是图1中的基于位置的移动机器人RFID伺服方法的目标示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

请参阅图1、图2及图3，本发明提供的基于位置的移动机器人RFID伺服方法，所述伺服方法主要包括以下步骤：

步骤一，在目标物体可能存在的位置空间内随机生成预定数量的粒子，每个粒子代表了目标标签的备选位置点。

本实施方式中，在移动机器人上设置两个RFID天线，目标物体上设置有RFID标签，即目标标签，依据所述移动机器人的位姿构建移动机器人坐标系，不断计算目标标签在移动机器人坐标系下的位置，利用得到的位置信息对移动机器人进行伺服控制，直到目标标签与移动机器人之间满足期望的位置关系。

粒子可用i＝1，2….，M表示，其中表示的是粒子在移动机器人坐标系下的x坐标；表示的是粒子的y坐标；表示的是粒子的z坐标；M表示粒子的个数。

步骤二，随着移动机器人的移动，所述移动机器人上设置的RFID读写器不断地收集目标物体上的RFID标签的相位信息，同时利用解缠算法对所述相位信息进行处理以得到解缠相位，并将当前时刻的解缠相位与前一时刻的解缠相位相减以得到相位差值的实际测量值。

本实施方式中，利用基于里程计等惯性传感器的航迹推测法得到相位差值的理论估计值，利用RFID系统测量得到相位差值的实际测量值；在移动机器人与目标标签的相对位置估计环节，设计了基于相位差的后验观测模型的粒子滤波算法，以计算目标标签在移动机器人坐标系下的位置；在移动机器人控制环节，利用目标标签在当前机器人坐标系下的位置估计值与期望值的差值信息设计IP控制器，实现了移动机器人的伺服控制。

其中，相位解缠算法对所述相位信息进行处理时，令γ⁽¹⁾＝θ⁽¹⁾，并开始如下循环，直到处理完所有的相位信息：

当θ⁽ⁱ⁾-θ^(i-1)>Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))-2π；

当θ⁽ⁱ⁾-θ^(i-1)<-Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))+2π；

当-Ψ≤θ⁽ⁱ⁾-θ^(i-1)≤Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))；

其中，θ⁽ⁱ⁾是第i个相位测量时刻测得的相位，γ⁽ⁱ⁾是第i个相位测量时刻测得的相位对应的实际解缠相位，Ψ为0～2π中的任意值，i＝2～K，K为相位信息数量。

步骤三，基于所述移动机器人上安装的惯性传感器检测到的传感器信息，利用航迹推测法与旋转平移变换对所述传感器信息进行处理以得到所述移动机器人上的RFID天线的位置信息，并根据所述RFID天线的位置信息与所述粒子的位置信息计算当前时刻的相位理论估计值，进而将当前时刻的相位理论估计值与前一时刻的相位理论估计值相减，以得到相位差值的理论估计值。

本实施方式中，对于RFID天线的位置信息而言，利用航迹推测法与旋转平移变换得到RFID天线的位置信息，具体为：根据上一时刻(第i-1时刻)的移动机器人方位信息及移动机器人内部的惯性传感器信息，通过递推计算得到当前时刻(第i时刻)的移动机器人方位信息根据当前时刻的移动机器人方位信息通过旋转平移变换方程得到当前时刻RFID天线的位置信息

对于航迹推测法而言，其利用移动机器人内部的惯性传感器(例如里程计、陀螺仪、加速度计、地磁计等)的信息，借助上一时刻的移动机器人方位信息，通过递推计算得到下一时刻的移动机器人方位信息，即根据移动机器人内部的惯性传感器的信息即可得出移动机器人的移动信息，然后在上一时刻的移动机器人方位信息上叠加移动信息即可获得当前时刻的移动机器人方位信息，而上一时刻的移动机器人方位信息可采用相同方法递推计算获得。例如，对于两轮差分移动机器人而言，通过安装于两轮内的里程计传感器，可以分别感知到两个轮子转动的距离，进而通过其差分运动学模型，可由上一时刻的移动机器人方位信息递推得到当前时刻的移动机器人方位信息。其中，航迹推测法只能得到相对的方位信息，并不能得到绝对方位信息，由于本实施方式所提出的定位方法最后得到的是目标标签相对于移动机器人当前方位的位置坐标，只需要移动机器人的相对方位信息，故航迹推测法可满足本实施方式的要求。

具体的，旋转平移变换的方程式为：

其中，是第i个相位测量时刻对应的移动机器人方位(位置+方向)，是移动机器人的位置，是移动机器人的方向，可以通过航迹推测法得到；z_A是RFID天线在移动机器人坐标系中的z轴坐标(其为已知参数，即RFID天线在移动机器人坐标系的Z轴方向上的高度)；(ρ,α)是RFID天线在移动机器人坐标系中的极坐标(其为已知参数)；是第i个相位测量时刻对应的RFID天线的位置坐标(在机器人坐标系下)。

具体地，通过如下公式计算得到相位理论估计值：

其中，是指t时刻，RFID天线与第j个粒子的相位理论估计值；λ是信号波长。

具体地，相位差值的理论估计值的计算公式为：

其中，为t时刻、第j个粒子的相位差值的理论估计值；是与t时刻相隔p个时刻时，第j个粒子的相位理论估计值。

步骤四，基于相位差值的实际测量值与相位差值的理论估计值，通过粒子滤波算法计算得到RFID标签在当前移动机器人坐标系下的位置估计值。

本实施方式中，粒子滤波算法的后验观测模型的数学表达式为：

其中，W_t,j表示在t时刻、第j个粒子的权重值；σ表示相位测量的标准差；Δθ_t是t时刻相位差值的实际测量值；是t时刻、第j个粒子相位差值的理论估计值。

目标标签在当前移动机器人坐标系下的位置估计值的计算公式为：

其中，是目标标签在t时刻移动机器人坐标系下的位置估计值；η_t,j为t时刻、第j个粒子的归一化参数。

步骤五，评估所述位置估计值的稳定性，如果所述位置估计值处于不稳定状态，则跳过伺服控制环节，直接转至步骤二；否则，将所述位置估计值与期望值进行比较，若所述移动机器人与所述目标物体达到期望的位置关系，则结束；否则，转至步骤二，直至所述移动机器人与所述目标物体达到期望的位置关系。

本实施方式中，所述位置估计值的稳定性的判断公式为：

其中，是目标标签在i时刻移动机器人坐标系下的位置估计值；N代表稳定性判断中所用到的最近的标签位置估计值的数量；代表的是过去N个时刻目标标签在机器人坐标系下的位置估计值的平均值；threshold₁和threshold₂分别是需要设定的相关阈值。本实施方式中，在一次伺服任务中，一旦判定目标标签的位置估计值处于稳定状态，则不再对该目标标签的位置估计值稳定性进行判断，直接进入伺服控制环节。

在执行控制算法之前，先将目标标签在移动机器人坐标系下的位置估计值由卡迪尔坐标形式转化为极坐标形式，转换公式为：

其中，为移动机器人极坐标系下的极轴；为移动机器人极坐标系下的极角；如果则将极角调整为

具体地，得到目标标签在移动机器热坐标系下的位置信息后，利用现有的诸多方法进行控制器的设计，如采用最优控制、PID控制及带有轨迹规划的控制算法设计控制器。以下以PI控制算法为例来说明控制器的设计，PI控制算法采用的方程式为：

其中，v_t是t时刻移动机器人的前进速度；w_t是t时刻移动机器人的旋转角速度；K_p1是前进速度比例系数；K_i1是前进速度的积分系数；K_p2是旋转速度比例系数；K_i2是旋转角速度的积分系数；(d^*,φ^*)是期望的标签在机器人极坐标下的极轴和极角。

目标标签与移动机器人之间是否达到期望的位置关系的判断公式为：

其中，threshold₃和threshold₄是相关的容许误差阈值。

本发明提供的基于位置的移动机器人RFID伺服方法，所述方法不需要布置RFID参考标签系统，不需要离线标定标签的位置信息，具有易于实施、成本较低、鲁棒性强等优点。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于位置的移动机器人RFID伺服方法，其特征在于，该方法包括以下步骤：

(1)在目标物体存在的位置空间内随机生成预定数量的粒子，每个粒子代表了目标标签的备选位置点；

(2)随着移动机器人的移动，设置在所述移动机器人上的RFID读写器不断地收集所述目标物体上的RFID标签的相位信息，同时利用解缠算法对所述相位信息进行处理以得到解缠相位，并将当前时刻的解缠相位与前一时刻的解缠相位相减以得到相位差值的实际测量值；其中，RFID标签即为目标标签；

(3)基于所述移动机器人上安装的惯性传感器检测到的传感器信息，利用航迹推测法与旋转平移变换对所述传感器信息进行处理以得到安装于所述移动机器人上的RFID天线的位置信息，并根据所述RFID天线的位置信息与所述粒子的位置信息计算当前时刻的相位理论估计值，进而将当前时刻的相位理论估计值与前一时刻的相位理论估计值相减，以得到相位差值的理论估计值；

(4)基于相位差值的实际测量值与相位差值的理论估计值，通过粒子滤波算法计算得到RFID标签在当前移动机器人坐标系下的位置估计值；

(5)评估所述位置估计值的稳定性，如果所述位置估计值处于不稳定状态，则跳过伺服控制环节，直接转至步骤(2)；否则，将所述位置估计值与期望值进行比较，若所述移动机器人与所述目标物体达到期望的位置关系，则结束；否则，转至步骤(2)，直至所述移动机器人与所述目标物体达到期望的位置关系。

2.如权利要求1所述的基于位置的移动机器人RFID伺服方法，其特征在于：所述RFID天线的数量大于等于2。

3.如权利要求1所述的基于位置的移动机器人RFID伺服方法，其特征在于：粒子滤波算法的后验观测模型的数学表达式为：

其中，W_t,j表示在t时刻、第j个粒子的权重值；σ表示相位测量的标准差；Δθ_t是t时刻相位差值的实际测量值；是t时刻、第j个粒子相位差值的理论估计值。

4.如权利要求3所述的基于位置的移动机器人RFID伺服方法，其特征在于：RFID标签在当前移动机器人坐标系下的位置估计值的计算公式为：

式中，是目标标签在t时刻移动机器人坐标系下的位置估计值；η_t,j为t时刻、第j个粒子的归一化参数；表示粒子，i＝1，2….，M，表示的是粒子在移动机器人坐标系下的x坐标；表示的是粒子的y坐标；表示的是粒子的z坐标；M表示粒子的个数。

5.如权利要求1所述的基于位置的移动机器人RFID伺服方法，其特征在于：相位解缠算法对所述相位信息进行处理时，令γ⁽¹⁾＝θ⁽¹⁾，并开始如下循环，直到处理完所有的相位信息：

当θ⁽ⁱ⁾-θ^(i-1)>Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))-2π；

当θ⁽ⁱ⁾-θ^(i-1)<-Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))+2π；

当-Ψ≤θ⁽ⁱ⁾-θ^(i-1)≤Ψ时，令γ⁽ⁱ⁾＝γ^(i-1)+(θ⁽ⁱ⁾-θ^(i-1))；

其中，θ⁽ⁱ⁾是第i个相位测量时刻测得的相位；γ⁽ⁱ⁾是第i个相位测量时刻测得的相位对应的实际解缠相位；Ψ为0～2π中的任意值；i＝2～K，K为相位信息数量。

6.如权利要求4所述的基于位置的移动机器人RFID伺服方法，其特征在于：旋转平移变换的方程式为：

其中，是第i个相位测量时刻对应的移动机器人方位，是移动机器人的位置，是移动机器人的方向，其通过航迹推测法得到；z_A是RFID天线在移动机器人坐标系中的z轴坐标；(ρ,α)是RFID天线在移动机器人坐标系中的极坐标；是第i个相位测量时刻对应的RFID天线的位置坐标。

7.如权利要求6所述的基于位置的移动机器人RFID伺服方法，其特征在于：通过如下公式计算得到相位理论估计值：

其中，是指t时刻，RFID天线与第j个粒子的相位理论估计值；λ是信号波长。

8.如权利要求7所述的基于位置的移动机器人RFID伺服方法，其特征在于：相位差值的理论估计值的计算公式为：

其中，为t时刻、第j个粒子的相位差值的理论估计值；是与t时刻相隔p个时刻时，第j个粒子的相位理论估计值。

9.如权利要求1-8任一项所述的基于位置的移动机器人RFID伺服方法，其特征在于：在执行控制算法之前，先将目标标签在移动机器人坐标系下的位置估计值由卡迪尔坐标形式转化为极坐标形式，转换公式为：

其中，是目标标签在t时刻移动机器人坐标系下的位置估计值；为移动机器人极坐标系下的极轴；为移动机器人极坐标系下的极角；如果则将极角调整为

10.如权利要求1-8任一项所述的基于位置的移动机器人RFID伺服方法，其特征在于：所述位置估计值的稳定性的判断公式为：

其中，是目标标签在i时刻移动机器人坐标系下的位置估计值；N代表稳定性判断中所用到的最近的标签位置估计值的数量；代表的是过去N个时刻目标标签在机器人坐标系下的位置估计值的平均值；threshold₁和threshold₂分别是需要设定的相关阈值。