RFID和WSNs技术在机器人协同定位中的应用研究*

潘峥嵘，杨维满，朱 翔，张 宁

(兰州理工大学电气工程与信息工程学院，甘肃兰州730050)

0 引言

随着机器人技术的发展，机器人应用领域不断扩大，其执行任务的难度逐步增大。现代机器人可移入到人体血管内去除梗塞物，进入危险环境进行救险以及在其他星球上进行数据探测等任务。然而，在很多情况下，特定任务单个机器人无法完成，或者执行过程优化不够，需要多机器人系统协作完成。多机器人在协作过程中的相互定位问题成为机器人研究领域的一大难点。目前，国内外对机器人定位［1，2］问题的研究重点主要在于单个机器人，对于多机器人协同定位问题的研究较少，但也有少数学者在多机器人协同定位方面进行了一些研究并取得一定成绩，主要应用的技术包括红外线、超声波、GPS等。但这些技术都存在定位范围小、抗干扰能力差、定位精度低、实时性差等缺点。

本文采用有源射频识别(radio frequency identification，RFID)技术［3，4］，基于每个标签的 ID，利用接收到的信号强度指示(received signal strength indicator，RSSI)来实现多移动机器人在全局坐标系中的自定位。同时利用无线传感器网络(wireless sensor networks，WSNs)技术［5～7］实现多机器人之间的相对定位。两技术的有机结合实现了精度高，非接触和非视距的机器人自定位和多机器人之间的协同定位。其独特的控制器结构和良好的控制算法解决了机器人协同定位中定位范围小，精度低，实时性差的问题。

1 移动机器人自定位优化算法

在理想条件下，应用无线信号传播原理中的对数—常态分布强度传播损耗模型，可得出标签与其读写器间的距离d和读写器接收到的该标签RSSI值之间的函数关系式

其中，P(d0)为d=1 m时的RSSI值，n为信号传播常量。通过已知环境的电子标定确定出P(d0)和n后，根据测得的RSSI值按上式可计算出d。

假设1，2，3…n个电子标签的分布坐标分别为:(x1，y1)，(x2，y2)，(x3，y3)，…，(xn，yn)，测得距离机器人 J所在点的距离分别为d1，d2，d3，…，dn。此处设机器人 J对应的位置点坐标为(x，y)，则有坐标方程式(2)

方程式(2)中的前n－1个子式与最后一子式作差可得

将其记为AX=B，其中

用最小二乘估计法求解上述方程，得到机器人J所在位置点的坐标向量估计值

已知环境中n个标签的位置坐标，结合RSSI测得的距离信息di(0＜i＜n)，利用上式求出机器人在全局坐标系中的位置坐标估计值。

信号在无线信道内传播主要因多径传输、反射、非视距、天线增益等因素而产生衰减，但在距离越近的标签受到的上述因素影响越小，产生的误差也越小，即距离越近的点越“可信”［3］。

由于系统自定位完成后又启动协同定位，以牺牲实时性为代价提高定位精度。为此，本文采用极大似然估计定位优化算法，在一定程度上提高该系统定位的实时性。如图1所示，三位机器人 J1，J2，J3分别以位姿向量(v1，θ1)，(v2，θ2)，(v3，θ3)行进，并进行实时定位。它们分别位于A1，A2，A3点处，其对应的位置坐标分别为(x1，y1)，(x2，y2)，(x3，y3)。因机器人的自定位原理相同，此处以J1的定位过程详细说明移动机器人的自定位。在T1时刻，J1位于A1处选取RSSI最大的4个标签作为定位标签。其中，标签2坐标为(xT2，yT2)，标签3 坐标为(xT3，yT3)，标签4 坐标为(xT4，yT4)，标签 5 坐标为(xT5，yT5)。经过时间间隔 Δt，到达位置A1进行定位计算。实质上，J1到达A1、处所计算出来的坐标点(x1，y1)是J1根据T1时刻传感器所获得的信息计算出来的A1(x1，y1)点的坐标。

在Δt时间间隔内，J1在直角坐标系XOY中以速度v1行进的运动学方程为

机器人J1在做平面运动过程中满足纯滚动和不滑动的约束条件，所以，机器人系统存在约束方程

经分析得到A1与A'1之间的位置坐标关系

图1 多机器人行进状态图Fig 1 State chart of multi-robots travel

考虑有源RFID的无线RSSI的波动，所以，选取时间间隔Δt内的RSSI平均值作为J1检测到的RSSI值。近似得出位置坐标 M1(x(t1)，y(t1))，对应时间t1=(t2－t1)/2。

移动机器人J1实际位置A1'的坐标为

利用式(4)可得坐标向量

进而由式(9)得

同理，可求出移动机器人J2，J3在全局坐标系中的位置坐标 x'2，x'3，求取结果如式(12)、式(13)所示

其中

2 基于WSNs的协同定位算法

三台移动机器人中由瞬时任务领导机器人作为父节点，其余机器人作为子节点。此处假设J1为父节点，J2，J3为子节点。J2，J3将自己的状态信息上传到J1，由J1结合J2，J3的自定位坐标确定出的它们之间的相对位置，进而比较J2，J3与自己的距离大小，选择与自己距离最小者作为协作伙伴，具体算法如下:

由已经获得的位置坐标信息M1，M2，M3得

选取 min{|M1M2|，|M1M3|}。

在此，假设

故确定J2为协作伙伴，进而只分析J2的状态信息W=(xt2，yt2，θ2，v2)，如图1 所示，机器人 J1 根据自身的检测装置和射频标签测得自己的状态信息，进一步确定出与J2的相对信息。分析状态信息和相对信息后产生确定的命令消息M12，并传送给机器人J2，使其执行相关的命令指令

其中，Δθ12=θ1－θ2，φ 为J1与 J2之间的相对位置角，|M1M2|为J1 与 J2 之间的距离，f(Δθ12，φ，|M1M2|)为关于Δθ12，φ和|M1M2|的多参数命令函数。

3 实验与结果分析

实验选用了三位轮式机器人，各自配备了有源RFID和WSNs节点融合结构的特殊阅读器，其结构如图2所示。软件开发平台为Windos XP下的Visual C++和TinyOS，其流程框图如图3所示。在空旷的实验楼大厅搭建了多移动机器人协同定位系统，电子标签选用WS—HT06，其排列摆放方式矩阵形式，其边长间隔为2 m。

图2 控制器结构图Fig 2 Structure diagram of controller

图3 程序设计流程图Fig 3 Flow chart of program design

在多种初始运行状态下，实现机器人行进预定时间后启动定位功能软件，确定距离父节点机器人最近者为协作伙伴，向其发送命令指令，使其向父节点机器人方向行进，在指定位置停止，模拟邻近接受任务动作。通过实验分析2个子节点机器人的运行状态，进一步分析验证多移动机器人协同定位算法的有效性和可靠性。

3台机器人在多种初始状态下的实际行进路线如图4所示。3台机器人在多种初始状态下的实际行进路线如图4所示。初始状态(1)中移动多机器人行进初始状态为:机器人1的速度v1控制在0.8 m/s，机器人2，3的速度v2，v3设为0.6 m/s，三者均未设定加速度向量。运行2 s后各自启动自定位算法，确定机器人1为父节点领导机器人，采用协同定位算法，寻求到的协作伙伴为机器人2，机器人行进路线符合期望路径。初始状态(2)，(3)所示实验主要为分析算法精度和可靠性而进行，分别调整机器人2，3的初始位置，重复实验100次。机器人1发出协同定位指令(机器人1停止行进)后，距离机器人2，3的距离差L12－L13大于0.3 m时，2个子节点机器人运行正常，当其距离差小于0.3 m时两机器人出现误动作。

图4 多机器人协同定位运动轨迹图Fig 4 Trajectory figure of multi-robots cooperative localization

4 结论

利用改进的极大似然估计算法，设计出基于有源RFID和WSNs技术的多机器人协同定位算法。通过实验表明:融合结构阅读控制器设计可行，算法实现容易，实时性较高。0.3 m的距离差可能会引起2台甚至多台机器人的误动作，所以，在射频场强值划分上还需做进一步的研究工作。有源RFID和WSNs技术的结合，实现了非接触和非视距单机器人自定位和多机器人之间的协同定位。其作用距离大，传输范围远，标签体积较小，能耗少，成本较低。所以，在类似足球机器人或抢险机器人所在复杂环境的多机器人协同定位中具有很大的应用潜力。

［1］Cai Yunfei，Tang Zhenmin，Zhao Chunxia.Multi-robots cooperative online fastLAM［J］.Journal of Electronics，2011，20(2):223－227.

［2］王景川，陈卫东，曹其新.基于全景视觉与里程计的移动机器人自定位方法研究［J］.机器人，2005，27(1):41－45.

［3］王殿君，兰云峰，任福君，等.基于有源RFID的室内移动机器人定位系统［J］.清华大学学报:自然科学版，2010，50(5):673－676.

［4］白瑞林，杨文浩，刘 洋.井下安全定位的RFID模块研究与设计［J］.控制工程，2010，17(1):86－89.

［5］朱 剑，赵 海，徐 久.无线传感器网络中的定位模型［J］.软件学报，2011，22(7):1612－1625.

［6］李东岳，王英龙，魏 诺.信号强度和运动向量结合的无线传感器网络移动节点定位［J］.电子学报，2010(2A):221－224.

［7］赵文辉，姜宇，刘大昕.高精度无线传感器网络节点定位算法［J］.哈尔滨工程大学学报，2009，30(4):466－471.