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

郭 龍，熊 偉，李牧東

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

0 引言

WSN［1］主要是用來監(jiān)測網(wǎng)絡(luò)部署區(qū)域內(nèi)各種環(huán)境特性，如溫度、濕度、光照、聲強(qiáng)、磁場強(qiáng)度、壓力(強(qiáng))、運(yùn)動(dòng)物體加速度(速度)和化學(xué)物質(zhì)濃度(不同的特性需要不同的傳感器)，但對這些傳感數(shù)據(jù)在不知道相應(yīng)位置信息的情況下，往往是沒有意義的。因此，如何確定無線傳感器網(wǎng)絡(luò)中節(jié)點(diǎn)的位置信息(節(jié)點(diǎn)定位)成為必須解決的關(guān)鍵問題之一。

1 節(jié)點(diǎn)定位

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

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

變形為:

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

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

2 機(jī)器人定位算法

2.1 TDOA 定位

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

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

TDOA測距技術(shù)被廣泛應(yīng)用在無線傳感器網(wǎng)絡(luò)定位方案中。一般在節(jié)點(diǎn)上安裝超聲波收發(fā)器和RF收發(fā)器。測距時(shí)，在發(fā)射端2種收發(fā)器同時(shí)發(fā)射信號(hào)，利用聲波與電磁波在空氣中傳播速度的巨大差異，在接收端通過記錄2種不同信號(hào)到達(dá)時(shí)間的差異。基于已知信號(hào)傳播速度，可以直接把時(shí)間差異轉(zhuǎn)化為距離。該技術(shù)的測距精度較RSSI高，可達(dá)到cm級，但受限于超聲波傳播距離有限以及NLOS問題對超聲波信號(hào)的傳播影響。

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

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

2.2 機(jī)器人定位模型

為了對機(jī)器人進(jìn)行精確定位，需要在機(jī)器人內(nèi)部安裝3個(gè)固定的盲節(jié)點(diǎn)，這3個(gè)盲節(jié)點(diǎn)安裝要求是必須安裝在機(jī)器人某一平面上，此平面與機(jī)器人完全保持一致，這3個(gè)盲節(jié)點(diǎn)跟隨機(jī)器人一起運(yùn)動(dòng)。首先，3個(gè)盲節(jié)點(diǎn)的坐標(biāo)利用無線傳感器網(wǎng)絡(luò)定位技術(shù)進(jìn)行確定;然后，根據(jù)幾何關(guān)系就可以求出機(jī)器人的具體位置以及移動(dòng)方位，如圖1所示。

圖1 機(jī)器人定位模型

圖1中，n為錨節(jié)點(diǎn)的個(gè)數(shù);P、Q、G為安裝在機(jī)器人內(nèi)部的盲節(jié)點(diǎn)。3個(gè)盲節(jié)點(diǎn)的坐標(biāo)P，Q，G可以通過無線傳感器網(wǎng)絡(luò)極大似然的方法計(jì)算得到。假設(shè)盲節(jié)點(diǎn)坐標(biāo) P，Q，G 的坐標(biāo)分別為:(xp，yp，zp)，(xq，yq，zq)，(xg，yg，zg)，機(jī)器人的坐標(biāo)可以用P和G的中點(diǎn)坐標(biāo)(xm，ym，zm)精確表示為:

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

當(dāng)xq-xg＜0，yq-yg＞0時(shí)，θ1為北偏西的角度;

當(dāng)xq-xg＞0，yq-yg＞0時(shí)，θ1為北偏東的角度;

當(dāng)xq-xg＜0，yq-yg＜0時(shí)，θ1為南偏西的角度;

當(dāng)xq-xg＞0，yq-yg＜0時(shí)，θ1為南偏東的角度;

當(dāng)zq-zg＜0，時(shí)，θ2為向下的角度;

當(dāng)zq-zg＜0，時(shí)，θ2為向上的角度。

3 實(shí)驗(yàn)結(jié)果和分析

試驗(yàn)中，有6個(gè)錨節(jié)點(diǎn)，它們的坐標(biāo)信息是預(yù)先知道的，它們各自的坐標(biāo)如表1所示。

表1 錨節(jié)點(diǎn)坐標(biāo)信息

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

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

表2 測量各個(gè)錨節(jié)點(diǎn)到盲節(jié)點(diǎn)的距離數(shù)據(jù)

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

根據(jù)式(5)可以計(jì)算得到 xm=46.04，ym=47.99，zm=13.85，可以用坐標(biāo) (46.04，47.99，13.85)精確表示機(jī)器人的位置。根據(jù)式(6)可以計(jì)算得到 θ1=51.110o，θ2=0.389o，由于 xq-xg＜0，yq-yg＞0且zq-zg＜0，所以機(jī)器人現(xiàn)在的移動(dòng)方向?yàn)楸逼?51.110o，下坡角度為 0.389o。達(dá)到了對機(jī)器人坐標(biāo)和運(yùn)動(dòng)姿態(tài)進(jìn)行判斷的效果。

圖2 錨節(jié)點(diǎn)與盲節(jié)點(diǎn)分布圖

4 結(jié)束語

通過TDOA技術(shù)測量位置節(jié)點(diǎn)到錨節(jié)點(diǎn)的距離得到距離信息后，采用最大似然估計(jì)算法求出盲節(jié)點(diǎn)的坐標(biāo)，進(jìn)而求出機(jī)器人的坐標(biāo)及運(yùn)動(dòng)方位。該方法在小區(qū)域內(nèi)能實(shí)現(xiàn)較高精度的定位，能夠滿足機(jī)器人的定位需求。運(yùn)用實(shí)驗(yàn)的數(shù)據(jù)對定位方法進(jìn)行了驗(yàn)證，達(dá)到了預(yù)期的效果。

［1］孫利民，李建中，陳 渝，等.無線傳感器網(wǎng)絡(luò)［M］.北京:清華大學(xué)出版社，2005.

［2］石琴琴.無線傳感器網(wǎng)絡(luò)節(jié)點(diǎn)自定位系統(tǒng)及其算法研究［M］.上海:上海交通大學(xué)，2009.

［3］王小平，羅 軍，沈昌祥.無線傳感器網(wǎng)絡(luò)定位理論和算法［J］.計(jì)算機(jī)研究與發(fā)展，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.