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.