一種基于WSN的機器人三維精確定位算法

郭 龍，熊 偉，李牧東

(空軍工程大學電訊工程學院，陜西西安710077)

0 引言

WSN［1］主要是用來監測網絡部署區域內各種環境特性，如溫度、濕度、光照、聲強、磁場強度、壓力(強)、運動物體加速度(速度)和化學物質濃度(不同的特性需要不同的傳感器)，但對這些傳感數據在不知道相應位置信息的情況下，往往是沒有意義的。因此，如何確定無線傳感器網絡中節點的位置信息(節點定位)成為必須解決的關鍵問題之一。

1 節點定位

在傳感器網絡節點定位技術中，根據是否已知自身的位置，把傳感器節點分錨節點(Beacon Node)和盲節點(Unknown Node)。錨節點在網絡節點中所站的比例很小，可以通過攜帶GPS定位設備等手段獲得自身的精確位置。錨節點是盲節點定位的參考點。除了錨節點外，其他傳感器節點就是盲節點，它們通過錨節點的位置信息來確定自身位置。通信半徑內的所有節點都是該節點的鄰居節點。在傳感器節點定位［2，3］過程中，盲節點在獲得對于鄰近錨節點的距離后，或者獲得鄰近的錨節點與盲節點之間的相對角度后，一般使用三邊測量法、三角測量法和極大似然估計法來計算自己的位置此算法采用的是極大似然估計法。

已知錨節點坐標分別為 (x1，y1，z1) ，(x2，y2，z2) ，(x3，y3，z3)，…，(xn，yn，zn)，通過 TDOA［4-7］算法測得的錨節點與盲節點之間的距離分別為d1，d2，d3，…，dn，設盲節點 P 坐標為 (x，y，z)，則

變形為:

上式的線性方程表示方式為:AX=b，

最后，通過最小均方差算法求出P坐標為X=(ATA)-1ATb。

2 機器人定位算法

2.1 TDOA 定位

基于距離的定位機制(Range-based)是通過測量相鄰節點間的實際距離或方位進行定位。具體過程分為3個階段:①測距階段，盲節點首先測量到鄰居節點的距離或角度，然后進一步計算到鄰近錨節點的距離或方位，在計算到鄰近錨節點的距離時，可以計算盲節點到錨節點的直線距離，也可以用二者之間的跳段距離作為直線距離的近似;②定位階段，盲節點在計算出到達3個或3個以上錨節點的距離或角度后，利用三邊測量法、三角測量法或極大似然估計法計算盲節點的坐標;③ 修正階段，對求得的節點坐標進行求精，提高定位精度，減少誤差。

在基于距離的定位中，常用的測量節點間距離或角度的技術有 RSSI、TDOA、AOA和 TOA四種。RSSI和TDOA兩種方法最為常用。此算法采用精度高的TDOA技術。

TDOA測距技術被廣泛應用在無線傳感器網絡定位方案中。一般在節點上安裝超聲波收發器和RF收發器。測距時，在發射端2種收發器同時發射信號，利用聲波與電磁波在空氣中傳播速度的巨大差異，在接收端通過記錄2種不同信號到達時間的差異?；谝阎盘杺鞑ニ俣?，可以直接把時間差異轉化為距離。該技術的測距精度較RSSI高，可達到cm級，但受限于超聲波傳播距離有限以及NLOS問題對超聲波信號的傳播影響。

錨節點周期性地發射無線射頻信號與超聲波信號。假設RF的傳播速度為vr，超聲波信號的傳播速度為vu，ΔT為2種信號到達盲節點的時間差。則通過式(3)可以求得錨節點到盲節點的距離d:

此處，錨節點有2個作用:提供自身的位置信息以及ID:而為了減少功率消耗，超聲波信號只是一個窄脈沖，并不含任何數據的脈沖信號。在得到超聲波信號的傳播時間以及其傳播速度已知的情況下，就能計算出2個節點之間的距離。

2.2 機器人定位模型

為了對機器人進行精確定位，需要在機器人內部安裝3個固定的盲節點，這3個盲節點安裝要求是必須安裝在機器人某一平面上，此平面與機器人完全保持一致，這3個盲節點跟隨機器人一起運動。首先，3個盲節點的坐標利用無線傳感器網絡定位技術進行確定;然后，根據幾何關系就可以求出機器人的具體位置以及移動方位，如圖1所示。

圖1 機器人定位模型

圖1中，n為錨節點的個數;P、Q、G為安裝在機器人內部的盲節點。3個盲節點的坐標P，Q，G可以通過無線傳感器網絡極大似然的方法計算得到。假設盲節點坐標 P，Q，G 的坐標分別為:(xp，yp，zp)，(xq，yq，zq)，(xg，yg，zg)，機器人的坐標可以用P和G的中點坐標(xm，ym，zm)精確表示為:

以世界坐標系為參考系，θ1和θ2分別為機器人偏離正北或正南方向和向上或向下方向的角度。根據幾何關系可以得到:

當xq-xg＜0，yq-yg＞0時，θ1為北偏西的角度;

當xq-xg＞0，yq-yg＞0時，θ1為北偏東的角度;

當xq-xg＜0，yq-yg＜0時，θ1為南偏西的角度;

當xq-xg＞0，yq-yg＜0時，θ1為南偏東的角度;

當zq-zg＜0，時，θ2為向下的角度;

當zq-zg＜0，時，θ2為向上的角度。

3 實驗結果和分析

試驗中，有6個錨節點，它們的坐標信息是預先知道的，它們各自的坐標如表1所示。

表1 錨節點坐標信息

在測量錨節點到盲節點距離過程中需要知道超聲波的傳播速度，超聲波的傳播速度由式(7)計算得到。通過傳感器節點的測距設備測量得到傳播時間差，再根據式(4)就可以計算得到各錨節點到盲節點的直線距離。

式中，v0=331.45;T0=273.15;T為溫度傳感器測量的環境溫度。每個錨節點到盲節點的距離通過試驗得到，為了減少測量誤差，每組做10次求平均值，測量結果如表2所示

表2 測量各個錨節點到盲節點的距離數據

根據公式X=(ATA)-1ATb，可以計算得到P的坐標是(41.47，50.58，12.60)，Q 的坐標是(45.23，46.34，12.60)，G 的坐標是(50.61，43.41，15.10)。錨節點和盲節點的分布情況如圖2所示。

根據式(5)可以計算得到 xm=46.04，ym=47.99，zm=13.85，可以用坐標 (46.04，47.99，13.85)精確表示機器人的位置。根據式(6)可以計算得到 θ1=51.110o，θ2=0.389o，由于 xq-xg＜0，yq-yg＞0且zq-zg＜0，所以機器人現在的移動方向為北偏西 51.110o，下坡角度為 0.389o。達到了對機器人坐標和運動姿態進行判斷的效果。

圖2 錨節點與盲節點分布圖

4 結束語

通過TDOA技術測量位置節點到錨節點的距離得到距離信息后，采用最大似然估計算法求出盲節點的坐標，進而求出機器人的坐標及運動方位。該方法在小區域內能實現較高精度的定位，能夠滿足機器人的定位需求。運用實驗的數據對定位方法進行了驗證，達到了預期的效果。

［1］孫利民，李建中，陳 渝，等.無線傳感器網絡［M］.北京:清華大學出版社，2005.

［2］石琴琴.無線傳感器網絡節點自定位系統及其算法研究［M］.上海:上海交通大學，2009.

［3］王小平，羅 軍，沈昌祥.無線傳感器網絡定位理論和算法［J］.計算機研究與發展，2011，48(3):353-363.

［4］SAVVIDES A，HAN C C，SRIVASTAVA M B.Dynamic Finge-grained Localization in Ad-hoc Networks of Sensor［C］.Italy:In:Proc 7th Annual Int′1 Conf on Mobile Computing and Networking(MobiCom)，2001:166-179.

［5］BULUSU N，HEIDEMANN J，ESTRIN D，et al.Selfconfiguringlocalization Systems:Design and Experimental Evaluation ［J］.ACM Transactions on Embedded Computing Systems，2004，3(1):24-60.

［6］CRAMER O.The Variation of the Specific Heat Ratio and the Speed of Sound in Air with Temperature，Pressure，Humidity，and CO2 Concentration ［J］.Acoustical Society of AmericaJournal，1993，93(5):2 510-2 516.