RFID和WSNs技術在機器人協(xié)同定位中的應用研究*

潘崢嶸，楊維滿，朱 翔，張 寧

(蘭州理工大學電氣工程與信息工程學院，甘肅蘭州730050)

0 引言

隨著機器人技術的發(fā)展，機器人應用領域不斷擴大，其執(zhí)行任務的難度逐步增大?，F(xiàn)代機器人可移入到人體血管內(nèi)去除梗塞物，進入危險環(huán)境進行救險以及在其他星球上進行數(shù)據(jù)探測等任務。然而，在很多情況下，特定任務單個機器人無法完成，或者執(zhí)行過程優(yōu)化不夠，需要多機器人系統(tǒng)協(xié)作完成。多機器人在協(xié)作過程中的相互定位問題成為機器人研究領域的一大難點。目前，國內(nèi)外對機器人定位［1，2］問題的研究重點主要在于單個機器人，對于多機器人協(xié)同定位問題的研究較少，但也有少數(shù)學者在多機器人協(xié)同定位方面進行了一些研究并取得一定成績，主要應用的技術包括紅外線、超聲波、GPS等。但這些技術都存在定位范圍小、抗干擾能力差、定位精度低、實時性差等缺點。

本文采用有源射頻識別(radio frequency identification，RFID)技術［3，4］，基于每個標簽的 ID，利用接收到的信號強度指示(received signal strength indicator，RSSI)來實現(xiàn)多移動機器人在全局坐標系中的自定位。同時利用無線傳感器網(wǎng)絡(wireless sensor networks，WSNs)技術［5～7］實現(xiàn)多機器人之間的相對定位。兩技術的有機結合實現(xiàn)了精度高，非接觸和非視距的機器人自定位和多機器人之間的協(xié)同定位。其獨特的控制器結構和良好的控制算法解決了機器人協(xié)同定位中定位范圍小，精度低，實時性差的問題。

1 移動機器人自定位優(yōu)化算法

在理想條件下，應用無線信號傳播原理中的對數(shù)—常態(tài)分布強度傳播損耗模型，可得出標簽與其讀寫器間的距離d和讀寫器接收到的該標簽RSSI值之間的函數(shù)關系式

其中，P(d0)為d=1 m時的RSSI值，n為信號傳播常量。通過已知環(huán)境的電子標定確定出P(d0)和n后，根據(jù)測得的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所在位置點的坐標向量估計值

已知環(huán)境中n個標簽的位置坐標，結合RSSI測得的距離信息di(0＜i＜n)，利用上式求出機器人在全局坐標系中的位置坐標估計值。

信號在無線信道內(nèi)傳播主要因多徑傳輸、反射、非視距、天線增益等因素而產(chǎn)生衰減，但在距離越近的標簽受到的上述因素影響越小，產(chǎn)生的誤差也越小，即距離越近的點越“可信”［3］。

由于系統(tǒng)自定位完成后又啟動協(xié)同定位，以犧牲實時性為代價提高定位精度。為此，本文采用極大似然估計定位優(yōu)化算法，在一定程度上提高該系統(tǒng)定位的實時性。如圖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)。經(jīng)過時間間隔 Δt，到達位置A1進行定位計算。實質上，J1到達A1、處所計算出來的坐標點(x1，y1)是J1根據(jù)T1時刻傳感器所獲得的信息計算出來的A1(x1，y1)點的坐標。

在Δt時間間隔內(nèi)，J1在直角坐標系XOY中以速度v1行進的運動學方程為

機器人J1在做平面運動過程中滿足純滾動和不滑動的約束條件，所以，機器人系統(tǒng)存在約束方程

經(jīng)分析得到A1與A'1之間的位置坐標關系

圖1 多機器人行進狀態(tài)圖Fig 1 State chart of multi-robots travel

考慮有源RFID的無線RSSI的波動，所以，選取時間間隔Δt內(nèi)的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的協(xié)同定位算法

三臺移動機器人中由瞬時任務領導機器人作為父節(jié)點，其余機器人作為子節(jié)點。此處假設J1為父節(jié)點，J2，J3為子節(jié)點。J2，J3將自己的狀態(tài)信息上傳到J1，由J1結合J2，J3的自定位坐標確定出的它們之間的相對位置，進而比較J2，J3與自己的距離大小，選擇與自己距離最小者作為協(xié)作伙伴，具體算法如下:

由已經(jīng)獲得的位置坐標信息M1，M2，M3得

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

在此，假設

故確定J2為協(xié)作伙伴，進而只分析J2的狀態(tài)信息W=(xt2，yt2，θ2，v2)，如圖1 所示，機器人 J1 根據(jù)自身的檢測裝置和射頻標簽測得自己的狀態(tài)信息，進一步確定出與J2的相對信息。分析狀態(tài)信息和相對信息后產(chǎn)生確定的命令消息M12，并傳送給機器人J2，使其執(zhí)行相關的命令指令

其中，Δθ12=θ1－θ2，φ 為J1與 J2之間的相對位置角，|M1M2|為J1 與 J2 之間的距離，f(Δθ12，φ，|M1M2|)為關于Δθ12，φ和|M1M2|的多參數(shù)命令函數(shù)。

3 實驗與結果分析

實驗選用了三位輪式機器人，各自配備了有源RFID和WSNs節(jié)點融合結構的特殊閱讀器，其結構如圖2所示。軟件開發(fā)平臺為Windos XP下的Visual C++和TinyOS，其流程框圖如圖3所示。在空曠的實驗樓大廳搭建了多移動機器人協(xié)同定位系統(tǒng)，電子標簽選用WS—HT06，其排列擺放方式矩陣形式，其邊長間隔為2 m。

圖2 控制器結構圖Fig 2 Structure diagram of controller

圖3 程序設計流程圖Fig 3 Flow chart of program design

在多種初始運行狀態(tài)下，實現(xiàn)機器人行進預定時間后啟動定位功能軟件，確定距離父節(jié)點機器人最近者為協(xié)作伙伴，向其發(fā)送命令指令，使其向父節(jié)點機器人方向行進，在指定位置停止，模擬鄰近接受任務動作。通過實驗分析2個子節(jié)點機器人的運行狀態(tài)，進一步分析驗證多移動機器人協(xié)同定位算法的有效性和可靠性。

3臺機器人在多種初始狀態(tài)下的實際行進路線如圖4所示。3臺機器人在多種初始狀態(tài)下的實際行進路線如圖4所示。初始狀態(tài)(1)中移動多機器人行進初始狀態(tài)為:機器人1的速度v1控制在0.8 m/s，機器人2，3的速度v2，v3設為0.6 m/s，三者均未設定加速度向量。運行2 s后各自啟動自定位算法，確定機器人1為父節(jié)點領導機器人，采用協(xié)同定位算法，尋求到的協(xié)作伙伴為機器人2，機器人行進路線符合期望路徑。初始狀態(tài)(2)，(3)所示實驗主要為分析算法精度和可靠性而進行，分別調(diào)整機器人2，3的初始位置，重復實驗100次。機器人1發(fā)出協(xié)同定位指令(機器人1停止行進)后，距離機器人2，3的距離差L12－L13大于0.3 m時，2個子節(jié)點機器人運行正常，當其距離差小于0.3 m時兩機器人出現(xiàn)誤動作。

圖4 多機器人協(xié)同定位運動軌跡圖Fig 4 Trajectory figure of multi-robots cooperative localization

4 結論

利用改進的極大似然估計算法，設計出基于有源RFID和WSNs技術的多機器人協(xié)同定位算法。通過實驗表明:融合結構閱讀控制器設計可行，算法實現(xiàn)容易，實時性較高。0.3 m的距離差可能會引起2臺甚至多臺機器人的誤動作，所以，在射頻場強值劃分上還需做進一步的研究工作。有源RFID和WSNs技術的結合，實現(xiàn)了非接觸和非視距單機器人自定位和多機器人之間的協(xié)同定位。其作用距離大，傳輸范圍遠，標簽體積較小，能耗少，成本較低。所以，在類似足球機器人或搶險機器人所在復雜環(huán)境的多機器人協(xié)同定位中具有很大的應用潛力。

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

［2］王景川，陳衛(wèi)東，曹其新.基于全景視覺與里程計的移動機器人自定位方法研究［J］.機器人，2005，27(1):41－45.

［3］王殿君，蘭云峰，任福君，等.基于有源RFID的室內(nèi)移動機器人定位系統(tǒng)［J］.清華大學學報:自然科學版，2010，50(5):673－676.

［4］白瑞林，楊文浩，劉 洋.井下安全定位的RFID模塊研究與設計［J］.控制工程，2010，17(1):86－89.

［5］朱 劍，趙 海，徐 久.無線傳感器網(wǎng)絡中的定位模型［J］.軟件學報，2011，22(7):1612－1625.

［6］李東岳，王英龍，魏 諾.信號強度和運動向量結合的無線傳感器網(wǎng)絡移動節(jié)點定位［J］.電子學報，2010(2A):221－224.

［7］趙文輝，姜宇，劉大昕.高精度無線傳感器網(wǎng)絡節(jié)點定位算法［J］.哈爾濱工程大學學報，2009，30(4):466－471.