基于UWB的線段近似雙曲線定位算法

趙雅秋,樓喜中

(中國(guó)計(jì)量大學(xué) 信息工程學(xué)院,浙江 杭州 310018)

近幾年,隨著無線傳感器網(wǎng)絡(luò)(wireless sensor networks, WSNs)的不斷開發(fā)與更新,室內(nèi)定位技術(shù)已經(jīng)得到了快速的發(fā)展.室內(nèi)定位[1]是指利用無線移動(dòng)通信網(wǎng)絡(luò),測(cè)量接受到的無線信號(hào)參數(shù)(到達(dá)時(shí)間、到達(dá)角度等),根據(jù)特定算法對(duì)移動(dòng)終端的在某一時(shí)間的地理位置進(jìn)行定義,從而為用戶提供位置信息的服務(wù),或者實(shí)時(shí)檢測(cè)和追蹤的定位和導(dǎo)航服務(wù).

超寬帶[2]是一種無線載波通信技術(shù),利用納米級(jí)的脈沖作為其載波傳輸數(shù)據(jù).超寬帶具有抗干擾能力強(qiáng)、高傳輸速率、穿透性好等優(yōu)點(diǎn),在火災(zāi)救援、大型超市、醫(yī)院、實(shí)驗(yàn)室以及軍事領(lǐng)域方面,可以更好地實(shí)現(xiàn)高精度定位.

定位過程分為兩步：1)測(cè)距,就是測(cè)量?jī)牲c(diǎn)的距離;2)定位,根據(jù)測(cè)距的結(jié)果由特定的算法來確定移動(dòng)標(biāo)簽的位置.有四種經(jīng)典的定位方法：基于接收信號(hào)強(qiáng)度[3](Received signal strength,RSS)、基于信號(hào)到達(dá)角度[4](Angle of Arrival, AOA)、基于到達(dá)時(shí)間[5](Time of arrival, TOA)和基于到達(dá)時(shí)間差[6](TDOA).TOA/TDOA方案是超寬帶定位的主要測(cè)距方案,利用測(cè)出的TDOA時(shí)間,計(jì)算出各個(gè)基站(anchor)到移動(dòng)標(biāo)簽(Tag)的距離差,根據(jù)距離差建立多條雙曲線方程,通常情況下很難求得最優(yōu)解,因此需要相應(yīng)的算法求得這些非線性方程的最優(yōu)解.Chan算法[7]是基于到達(dá)時(shí)間差的一種經(jīng)典定位方法,不需要遞歸即可給出雙曲線方程的閉式解,計(jì)算簡(jiǎn)單,在理想高斯噪聲環(huán)境下精度高.但在非視距環(huán)境下,Chan算法的定位精度會(huì)下降.Taylor算法[8]是一種遞歸算法,需要初始位置的估計(jì)值,在每一次遞歸中求解TDOA測(cè)量誤差的局部最小二乘[9](Least squares, LS)解來實(shí)現(xiàn)的.卡爾曼濾波[10]是一種線性最小均方估計(jì)算法,它在線性系統(tǒng)中是最佳濾波.在多種傳感器測(cè)量中,卡爾曼濾波廣泛地用于狀態(tài)估計(jì)[11],假定實(shí)驗(yàn)過程和測(cè)量噪聲都是正態(tài)分布的,但由于頻繁的重尾異常值,無線定位系統(tǒng)的測(cè)量噪聲并不容易滿足正態(tài)分布這一條件.Fang算法[12]可以對(duì)TDOA雙曲線方程進(jìn)行線性化,然后求出雙曲線方程組的解為移動(dòng)目標(biāo)的位置.該算法原理易懂,計(jì)算簡(jiǎn)單.

本文提出一種分段近似法來近似雙曲線定位模型,分為三線段近似定位算法、三加四線段近似定位算法和五線段近似定位算法,用這些線段近似雙曲線,將二次曲線模型轉(zhuǎn)化為線性模型,求出線段的交點(diǎn)即可近似為雙曲線的交點(diǎn),然后采用三角形質(zhì)心定位算法[13],該質(zhì)心就是移動(dòng)標(biāo)簽的定位結(jié)果.

1 TDOA算法原理

TDOA算法是采用各個(gè)基站(anchor)到達(dá)移動(dòng)標(biāo)簽(Tag)的時(shí)間差來實(shí)現(xiàn)對(duì)移動(dòng)標(biāo)簽的定位,而TDOA不需要各個(gè)基站和移動(dòng)標(biāo)簽的時(shí)鐘同步,只需要各個(gè)基站的時(shí)鐘同步,在實(shí)際應(yīng)用中,基站都是統(tǒng)一由主機(jī)(anchor1)控制,容易實(shí)現(xiàn)時(shí)鐘同步.TDOA定位方法的定位精度相對(duì)較高,比較適合室內(nèi)環(huán)境.

TDOA的定位原理[14]是以一組TDOA的測(cè)量值來求得各個(gè)基站到移動(dòng)標(biāo)簽的距離差,建立雙曲線方程.該雙曲線以基站為焦點(diǎn),需要定位的移動(dòng)標(biāo)簽點(diǎn)就在該雙曲線的某一條分支上,求得雙曲線方程組的解為預(yù)估的移動(dòng)標(biāo)簽點(diǎn)的位置.

假設(shè)anchor1(x1,y1),anchor2(x2,y2),anchor3

(x3,y3),anchor4(x4,y4)分別是各個(gè)基站的坐標(biāo),Tag(x5,y5)是移動(dòng)標(biāo)簽的坐標(biāo),如圖1,圖中的雙曲線是以基站anchor1和基站anchor3為焦點(diǎn)的曲線圖.

圖1 TDOA算法原理圖Figure 1 TDOA algorithm schematic

TDOA的兩點(diǎn)距離方程為

(1)

式(1)中,(xm,ym)是未知的移動(dòng)標(biāo)簽(Tag)的坐標(biāo),(xi,yi)、(xj,yj)分別是已知的基站(anchori、anchorj)的坐標(biāo),di、dj分別是基站(anchori、anchorj)到移動(dòng)目標(biāo)的距離,dij是基站(anchori、anchorj)到移動(dòng)標(biāo)簽的距離差.

根據(jù)TDOA的距離公式可以得到

(2)

式(2)中,d1,2、d1,3和d1,4分別可以用d1,2=(T2-T1)C、d1,3=(T3-T1)C和d1,4=(T4-T1)C來代替,顯然只需要知道時(shí)間差就可以解方程組,解上面的方程組就可以得到移動(dòng)標(biāo)簽點(diǎn)的坐標(biāo),但此種方法會(huì)存在模糊定位,可能會(huì)雙解兩交點(diǎn)的情況,需要優(yōu)化.

2 分段近似算法

在理想情況下,房間內(nèi)沒有任何障礙物遮擋,不存在非視距情況下,將房間內(nèi)四個(gè)角落的4個(gè)基站(anchor)分別作為雙曲線的焦點(diǎn),它們所形成的雙曲線會(huì)交于一點(diǎn),這一點(diǎn)就是移動(dòng)標(biāo)簽的坐標(biāo).但在實(shí)際環(huán)境下,由于室內(nèi)遮擋、非視距現(xiàn)象、多徑傳播等問題,雙曲線不會(huì)交于一點(diǎn),而是形成一個(gè)公共重疊區(qū)域.這公共重疊區(qū)域可以視為移動(dòng)標(biāo)簽點(diǎn)可能存在的區(qū)域.

三角形質(zhì)心定位的基本原理是：計(jì)算三條雙曲線的公共重疊部分的三個(gè)交點(diǎn)的坐標(biāo),這三個(gè)交點(diǎn)構(gòu)成三角形的頂點(diǎn),三角形的質(zhì)心就是我們預(yù)估的移動(dòng)標(biāo)簽的坐標(biāo).三角形質(zhì)心公式為：

(3)

式(3)中,(x1,y1)、(x2,y2)、(x3,y3)分別是三角形的三個(gè)頂點(diǎn)坐標(biāo),(x,y)是三角形的質(zhì)心坐標(biāo).

分段近似法的核心思想就是用多個(gè)線段來漸近雙曲線,分別用三條線段、三加四條線段和五條線段來近似雙曲線.根據(jù)TDOA的測(cè)量值得到的距離差的正負(fù),可以判斷移動(dòng)標(biāo)簽點(diǎn)在雙曲線的哪一條分支上,判斷規(guī)則如表1.

表1 判斷規(guī)則表

表1中,d1,i是基站anchor1到移動(dòng)標(biāo)簽的距離與基站anchori到移動(dòng)標(biāo)簽的距離的差.上側(cè)和下側(cè)表示焦點(diǎn)在y軸的雙曲線上側(cè)分支和下側(cè)分支,左側(cè)和右側(cè)表示焦點(diǎn)在x軸的雙曲線左側(cè)分支和右側(cè)分支.

1)當(dāng)d1,2<0時(shí),移動(dòng)標(biāo)簽點(diǎn)在以基站anchor1和基站anchor2為焦點(diǎn)的雙曲線的上側(cè);

2)當(dāng)d1,2>0時(shí),移動(dòng)標(biāo)簽點(diǎn)在以基站anchor1和基站anchor2為焦點(diǎn)的雙曲線的下側(cè);

3)同理可得當(dāng)d1,3<0時(shí),移動(dòng)標(biāo)簽點(diǎn)在以基站anchor1和基站anchor3為焦點(diǎn)的雙曲線的左側(cè);

4)當(dāng)d1,3>0時(shí),移動(dòng)標(biāo)簽點(diǎn)在以基站anchor1和基站anchor3為焦點(diǎn)的雙曲線的右側(cè);

5)當(dāng)d1,4<0時(shí),移動(dòng)標(biāo)簽點(diǎn)在以基站anchor1和基站anchor4為焦點(diǎn)的雙曲線的左側(cè);

6)當(dāng)d1,4>0時(shí),移動(dòng)標(biāo)簽點(diǎn)在以基站anchor1和基站anchor4為焦點(diǎn)的雙曲線的右側(cè).

三條線段來近似雙曲線的方法是：第一條線段和第三條線段分別是雙曲線的漸近線,第二條線段為雙曲線頂點(diǎn)的垂直線段.

經(jīng)過MATLAB仿真,三條線段來漸近雙曲線,定位誤差較大,除了系統(tǒng)誤差等影響外,線段不足夠逼近雙曲線是造成誤差的原因之一.于是,使用三加四條線段來近似雙曲線.

用三加四條線段來近似雙曲線,其中的四條線段近似雙曲線的方法是：第一條線段和第四條線段分別為原雙曲線的漸近線平移距離L之后的漸近線,第二條線段和第三條線段分別為雙曲線的頂點(diǎn)與第一條線段和第四條線段的連線.平移距離L的公式的是

(4)

式(4)中,(x6,y6)是雙曲線焦點(diǎn)的垂直線和雙曲線的漸近線的交點(diǎn)坐標(biāo),(x7,y7)是雙曲線焦點(diǎn)的垂直線和雙曲線的交點(diǎn)坐標(biāo).

五條線段漸近雙曲線的方法是：第一條線段和第五條線段分別為原雙曲線的漸近線平移一段距離之后的漸近線,平移距離公式如式(4),第三條線段為原雙曲線的焦點(diǎn)垂直線平移L1(5cm)之后的焦點(diǎn)垂直線,該垂直線與雙曲線的交點(diǎn)為(x8,y8)、(x9,y9),將交點(diǎn)分別平移L2(25cm)之后的坐標(biāo)為(x8,y8)、(x9,y9+L2),則第二條線段和第四條線段分別是點(diǎn)(x8,y8+L2)、點(diǎn)(x9,y9+L2)和漸近線的連線.

三線段近似定位算法、三加四線段近似定位算法和五線段近似定位算法的定位結(jié)果分別如圖2、圖3和圖4.從圖2-4可以看出,五條線段比三加四條線段更逼近雙曲線,三加四條線段比三條線段更逼近雙曲線.經(jīng)MATLAB仿真得出,三線段近似定位算法與雙曲線的最大距離誤差為56.295 cm,三加四線段近似定位算法與雙曲線的最大距離誤差為27.447 cm,五線段近似定位算法與雙曲線的最大距離誤差為13.520 cm,由此可以驗(yàn)證上文的猜測(cè).

圖2 三線段近似定位算法定位結(jié)果圖Figure 2 Three-line segmental approximate positioning algorithm positioning result

圖3 三加四線段近似定位算法定位結(jié)果圖Figure 3 Three-and-four segmental approximate positioning algorithm positioning result

圖4 五線段近似定位算法定位結(jié)果圖Figure 4 Five-line segmental approximate positioning algorithm positioning result

在MATLAB中計(jì)算程序運(yùn)行方法使用最多的是tic和toc組合,經(jīng)典格式為：

tic

中間程序

toc

由此來計(jì)算Chan算法運(yùn)行一個(gè)周期所用時(shí)間為13.580 s,五線段近似算法運(yùn)行一個(gè)周期所用時(shí)間為8.985 s.

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

3.1 實(shí)驗(yàn)環(huán)境

實(shí)驗(yàn)地點(diǎn)為室內(nèi)9.8 m×7.8 m×3.5 m的教室,在教室四個(gè)角落安裝四個(gè)用于發(fā)射信號(hào)的基站(anchor1、anchor2、anchor3、anchor4),安裝高度2 m左右,硬件實(shí)物和實(shí)驗(yàn)環(huán)境如圖5.圖5(a)中左邊的是基站(anchor)硬件實(shí)物,右邊是手持接收信號(hào)的移動(dòng)標(biāo)簽(Tag)硬件實(shí)物.

圖5 硬件實(shí)物和實(shí)驗(yàn)環(huán)境圖Figure 5 Hardware physical experimental environment map

3.2 實(shí)驗(yàn)結(jié)果

為了驗(yàn)證算法的可行性和正確性,將同一個(gè)移動(dòng)標(biāo)簽放置教室內(nèi)不同的位置,分別使用三線段近似定位算法、三加四線段近似定位算法和五線段近似定位算法進(jìn)行對(duì)比實(shí)驗(yàn).實(shí)驗(yàn)1為三線段近似定位算法,實(shí)驗(yàn)2為三加四線段近似定位算法,實(shí)驗(yàn)3為五線段近似定位算法,通過隨機(jī)選取的6個(gè)位置,每個(gè)位置最少采集1 000組TDOA數(shù)據(jù),并利用MATLAB仿真記錄實(shí)驗(yàn)數(shù)據(jù),基于UWB的高精度定位算法仿真結(jié)果如表2-4,基于UWB的高精度定位算法誤差分析結(jié)

果如圖6-7.實(shí)驗(yàn)1的定位算法最大距離誤差是118.332 cm,最小距離誤差是30.167 cm,平均距離誤差是60.74 cm;實(shí)驗(yàn)2的定位算法最大距離誤差是47.92 cm,最小距離誤差是10.83 cm,平均距離誤差是31.34 cm;實(shí)驗(yàn)3的定位算法最大距離誤差是22.539 cm,最小距離誤差是1.160 cm,平均距離誤差是14.08 cm.結(jié)果顯示,實(shí)驗(yàn)3的定位誤差比實(shí)驗(yàn)1和實(shí)驗(yàn)2的小,從表2、表3可以看出,定位編號(hào)2、6的距離誤差較大,表4中的距離誤差波動(dòng)較小,有很好的適應(yīng)能力；而繼續(xù)提高近似線段數(shù)量,定位精度提升有限,已經(jīng)基本達(dá)到精度性能上界,所以采用五線段近似雙曲線的定位算法.

表2 三線段近似定位算法定位結(jié)果

表3 三加四線段近似定位算法定位結(jié)果

表4 五線段近似定位算法定位結(jié)果

圖6 平均距離誤差比較圖Figure 6 Average distance error comparison

圖7 距離誤差的方差比較圖Figure 7 Distance error variance comparison

為了進(jìn)一步驗(yàn)證算法的實(shí)時(shí)性和精度,選取同一個(gè)移動(dòng)標(biāo)簽,沿著預(yù)設(shè)的路線行走,用XCOM V2.0來收集TDOA測(cè)量數(shù)據(jù),經(jīng)過MATLAB仿真,實(shí)驗(yàn)結(jié)果如圖8.從圖8中可以明顯的看出,五線段近似定位算法相比于其他兩種定位方法,定位精度更高,可以很好的適應(yīng)環(huán)境,能滿足一般室內(nèi)定位的要求,例如建筑工地、化工廠、監(jiān)獄等場(chǎng)景.

圖8 動(dòng)態(tài)定位結(jié)果圖Figure 8 Dynamic positioning result

從定位誤差角度來看(如圖8),假設(shè)第一條路徑是縱坐標(biāo)y為500.8 cm,橫坐標(biāo)x范圍為(103.1,980.2),第二條路徑是縱坐標(biāo)y范圍為(139.1,500.8 cm),橫坐標(biāo)x為103.1cm,第三條路徑是縱坐標(biāo)y為139.1 cm,橫坐標(biāo)x范圍為(103.1,980.2),動(dòng)態(tài)定位誤差如表5.三線段近似法的誤差平均值為38.59 cm,三加四線段近似法的誤差平均值為20.65 cm,五線段近似法的誤差平均值為15.28 cm.

在Whistle算法[15]中,用多個(gè)異步接收器記錄人工生成的連續(xù)信號(hào).通過雙信號(hào)檢測(cè)和采樣計(jì)數(shù)技術(shù),不需要時(shí)間同步,同時(shí)可以實(shí)現(xiàn)高時(shí)間分辨率,該算法靜態(tài)定位精度是13.7cm.在基于無線傳感器網(wǎng)絡(luò)的室內(nèi)定位實(shí)驗(yàn)評(píng)估[16]中,提出一種改進(jìn)的TDOA搜索技術(shù),計(jì)算成本最小,同時(shí)保留了基于相變算法的導(dǎo)向響應(yīng)功率的魯棒性,該算法的定位精度為10.7cm.在60 GH的正交頻分復(fù)用通信系統(tǒng)[17]中,使用多輸入單輸出和超寬帶技術(shù),該算法的定位精度為18 cm.與本文提出的五線段近似定位精度比較,如表6.

表5動(dòng)態(tài)定位誤差比較表

Table 5 Dynamic positioning error comparison

cm

表6 多種算法定位精度比較表

4 結(jié)論與展望

本文提出了在UWB定位中,用幾條線段近似代替雙曲線的定位算法.從MATLAB仿真結(jié)果可以看出,在靜態(tài)定位中五線段近似定位算法比三加四線段近似定位算法的定位精度提高了17.26 cm,三加四線段近似定位算法比三線段近似定位算法的定位精度提高了29.40 cm;在動(dòng)態(tài)定位中五線段近似定位算法比三加四線段近似定位算法的定位精度提高了11.05 cm,三加四線段近似定位算法比三線段近似定位算法的定位精度提高了17.94 cm;在靜態(tài)定位結(jié)果和動(dòng)態(tài)定位結(jié)果中,五線段近似法相比于三線段和三加四線段近似法的定位精度更高,故采用五線段近似定位算法;同時(shí)該算法將二次曲線轉(zhuǎn)換成線性方程,與Chan算法相比,一周期的運(yùn)行時(shí)間減少了4.595 s,復(fù)雜度也大大的下降.

另外,NLOS效應(yīng)是在多徑環(huán)境下影響測(cè)距精度的主要原因之一,對(duì)NLOS狀態(tài)進(jìn)行鑒別,并正確的檢測(cè)到接收信號(hào)中的直達(dá)路徑,然后在定位程序中加入鑒別結(jié)果,可以進(jìn)一步提高測(cè)距定位的性能,這是接下來的重要工作.