基于UWB的三維定位和優(yōu)化濾波方法

李楠梁冰

(1.華北科技學(xué)院 機(jī)電工程學(xué)院,河北 廊坊 065201；2.華北科技學(xué)院 電子信息工程學(xué)院,河北 廊坊 065201)

GPS定位系統(tǒng)在汽車、船舶、飛機(jī)等室外運(yùn)動(dòng)物體的定位和導(dǎo)航具有廣泛應(yīng)用.但GPS定位系統(tǒng)的不足之處在于定位信號(hào)到達(dá)地面較弱以及不能穿透建筑物等.因此出現(xiàn)了一種新型的無線通信技術(shù),即超寬帶(ultra wide band,UWB)技術(shù).該技術(shù)具有發(fā)射信號(hào)功率譜密度低、抗多徑、安全系數(shù)高、系統(tǒng)復(fù)雜度低、傳輸速率高和穿透能力強(qiáng)等特點(diǎn).從而使得超寬帶技術(shù)引起極大的關(guān)注,近年來該技術(shù)被廣泛應(yīng)用于靜止或者移動(dòng)物體以及人的定位和跟蹤[1-2].

基于UWB的三維定位算法設(shè)計(jì)與實(shí)現(xiàn)可用于無人機(jī)和室內(nèi)機(jī)器人等領(lǐng)域.故近幾年來國內(nèi)外對(duì)UWB應(yīng)用研究都十分熱門,主要用于通信、雷達(dá)以及精確定位[4-6].文獻(xiàn)[7]針對(duì)室內(nèi)三維定位結(jié)果準(zhǔn)確性的要求,提出了一種基于圖優(yōu)化的室內(nèi)超寬帶定位算法.文獻(xiàn)[8]為解決基于MEMS的微慣性導(dǎo)航系統(tǒng)誤差隨時(shí)間發(fā)散、航向角發(fā)散快的問題,提出了一種利用UWB輔助修正慣性導(dǎo)航系統(tǒng),實(shí)現(xiàn)室內(nèi)較高精度定位的方法.文獻(xiàn)[9]針對(duì)自主跟隨機(jī)器人定位精度低的問題,采用飛行時(shí)間測(cè)距法,設(shè)計(jì)一種基于UWB的自主跟隨機(jī)器人定位系統(tǒng),實(shí)現(xiàn)機(jī)器人與跟隨目標(biāo)之間的高精度測(cè)距.

因?yàn)閁WB可以獲得三維空間2點(diǎn)間精確距離以及三邊測(cè)量法(Trilateration)易于實(shí)現(xiàn)[10-15],故本文采用基于Python語言實(shí)現(xiàn)室內(nèi)三維定位和優(yōu)化濾波,為室內(nèi)四旋翼飛行器室內(nèi)定位選擇適合的定位方案.本文使用的UWB 技術(shù)提供三維空間坐標(biāo)定位信息的設(shè)備由4個(gè)錨點(diǎn)和無線UWB移動(dòng)漫游器組成.在三維空間中(即x、y、z軸)設(shè)定4個(gè)固定錨點(diǎn),移動(dòng)的物體攜帶無線UWB 移動(dòng)漫游器,通過其到4個(gè)固定錨點(diǎn)的距離即可確定出此刻自身所在三維空間的精確坐標(biāo).

1 三維定位的三邊測(cè)量法

在三維空間中建立x、y、z3個(gè)坐標(biāo)軸,此時(shí)空間中所有的點(diǎn)都可以用三維坐標(biāo)(x,y,z)來表示,在空間中的某一平面內(nèi)任取3個(gè)點(diǎn),而后在該平面外再取1個(gè)點(diǎn),以這4個(gè)點(diǎn)為圓心畫球,可以得出1個(gè)公共交點(diǎn),這4個(gè)隨意取的點(diǎn)就是錨點(diǎn).

三邊測(cè)量法是一種常用的定位算法,在平面中先找到3個(gè)錨點(diǎn),以它們?yōu)閳A心作圓,可以得到唯一的1個(gè)交點(diǎn),需滿足以下2個(gè)條件:

1)已知3點(diǎn)位置 (x1,y1),(x2,y2),(x3,y3)；

2)已知未知點(diǎn) (x0,y0) 到3點(diǎn)距離分別為d1,d2,d3.

以d1,d2,d3為半徑作3個(gè)圓,根據(jù)畢達(dá)哥拉斯定理,得出交點(diǎn)即未知點(diǎn)的位置,計(jì)算公式為

式(1)可通過圖1進(jìn)行表示.

在上面所列舉的平面中的定位算法的基礎(chǔ)上,可推廣出三維空間定位法.在空間中的某一平面內(nèi)任取3個(gè)點(diǎn),以這3個(gè)點(diǎn)為圓心畫球,可以得出2個(gè)公共交點(diǎn),然后在該平面外任取1點(diǎn),以其為圓心畫球,可以確定出唯一的交點(diǎn),故其條件可改為

1)已知4 點(diǎn)位置 (x1,y1,z1),(x2,y2,z2),(x3,y3,z3),(x4,y4,z4)；

2)已知未知點(diǎn) (x0,y0,z0) 到4點(diǎn)距離分別為d1,d2,d3,d4.

以d1,d2,d3,d4為半徑作4個(gè)圓,根據(jù)二維向三維的擴(kuò)展,得出未知點(diǎn)的位置計(jì)算公式為

圖1 三邊測(cè)量法的二維平面原理Fig.1 Two dimensional plane schematic diagram of Trilateration method

通過式(2),可求出三維空間中的未知點(diǎn).

2 三維定位的優(yōu)化濾波法

通過三維定位的三邊測(cè)量法測(cè)得坐標(biāo)數(shù)值存在一定的誤差,需對(duì)這些坐標(biāo)數(shù)值進(jìn)行優(yōu)化濾波,獲得更精確的坐標(biāo)數(shù)據(jù).常用的優(yōu)化濾波方法主要有卡爾曼濾波法、平滑濾波、中值濾波和曲線擬合法等.

2.1 卡爾曼濾波

假設(shè)系統(tǒng)的離散狀態(tài)方程為

式(3)中,A和B是系統(tǒng)參數(shù)矩陣,H是系統(tǒng)測(cè)量矩陣,X(k)是k時(shí)刻的系統(tǒng)狀態(tài),U(k)是k時(shí)刻對(duì)系統(tǒng)的控制量,Z(k)是k時(shí)刻的測(cè)量值,W(k)和V(k)分別表示過程噪聲和測(cè)量噪聲.

根據(jù)系統(tǒng)的模型,可以根據(jù)系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài):

式(4)中,X(k|k-1)是利用上一狀態(tài)預(yù)測(cè)的結(jié)果,X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果,U(k)為現(xiàn)在狀態(tài)的控制量,可以為0.

此時(shí),系統(tǒng)結(jié)果已經(jīng)更新,但對(duì)應(yīng)于X(k|k-1)的協(xié)方差還沒更新,協(xié)方差P為

式(5)中,P(k|k-1)是X(k|k-1)對(duì)應(yīng)的協(xié)方差,P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)協(xié)方差,Q是系統(tǒng)過程的協(xié)方差,A′表示A的轉(zhuǎn)置矩陣.式(4)和(5)就是對(duì)系統(tǒng)狀態(tài)的預(yù)測(cè).

可得現(xiàn)在狀態(tài)(k)的最優(yōu)估算值X(k|k):

式(6)中,Kg為卡爾曼增益,其公式如下:

要使卡爾曼濾波不斷運(yùn)行下去直到系統(tǒng)過程結(jié)束,還需要更新k狀態(tài)下X(k/k)的協(xié)方差,

式(8)中,I是值為1的矩陣.當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí),P(k|k)就是式(5)中的P(k-1|k-1).算法就可以自回歸的運(yùn)算下去.

上文中的式(4)～(8)是卡爾曼濾波的5個(gè)基本公式,可用圖2來表示.

圖2 卡爾曼濾波器的工作原理Fig.2 Working principle of Kalman filter

2.2 平滑濾波

空間域中的平滑濾波一般是采用簡(jiǎn)單平均法進(jìn)行,也可以說是均值濾波,例如,一序列{0,3,4,0,7},此列若用平滑濾波,窗口取5,那么平滑濾波輸出為(0+3+4+0+7)/5=2.8.其公式可表示為

其中,S為模板,M為該模板中包含當(dāng)前坐標(biāo)在內(nèi)的坐標(biāo)總數(shù).

2.3 中值濾波

中值濾波就是用一個(gè)移動(dòng)窗口,將窗口的中心點(diǎn)的值用窗口內(nèi)的各點(diǎn)中值代替.假設(shè)窗口內(nèi)有5點(diǎn),其值為80、90、200、110和120,那么此窗口內(nèi)各點(diǎn)的中值為110.

設(shè)一維序列為x1,x2,x3,…,xn,取點(diǎn)數(shù)為n,對(duì)其進(jìn)行中值濾波,就是從輸入序列中相繼抽出n個(gè)數(shù).

把1個(gè)點(diǎn)的特定長度或形狀的鄰域稱作窗口.在一維情況下,中值濾波器是一個(gè)含有奇數(shù)個(gè)點(diǎn)的滑動(dòng)窗口.中值濾波很容易推廣到二維,此時(shí)可以利用二維形式的窗口.

2.4 曲線擬合

由于室內(nèi)多移動(dòng)機(jī)器人系統(tǒng)運(yùn)行環(huán)境處于室內(nèi),故利用設(shè)計(jì)的UWB模塊實(shí)測(cè)了已知真實(shí)距離的50組數(shù)據(jù),然后通過曲線擬合,得到其近似變化規(guī)律.本文曲線擬合以最小二乘法為基礎(chǔ),選用冥函數(shù){1,x,x2,…,xn}作為函數(shù)類,則

這就是擬合函數(shù).為了確定該擬合函數(shù)的系數(shù),需要求解正規(guī)方程組

解得a0,a1,…,an即可,將其代入

可得到擬合后的函數(shù).

3 基于UWB的三維定位與優(yōu)化濾波的實(shí)驗(yàn)分析

表1給出了6種定位各項(xiàng)技術(shù)性能,無論是定位精度和可靠性,還是使用成本和安全性,UWB 技術(shù)相比與其他的室內(nèi)定位技術(shù)都占有很大的優(yōu)勢(shì).綜合考慮本實(shí)驗(yàn)將選用UWB技術(shù)在室內(nèi)完成.

表1 室內(nèi)定位技術(shù)對(duì)比Tab.1 Comparison of indoor positioning technology

3.1 硬件環(huán)境

超寬帶在本次實(shí)驗(yàn)中采用的硬件由4個(gè)錨點(diǎn)和UWB漫游器等諸多元器件組成,表2是實(shí)驗(yàn)硬件列表,圖3是UWB錨點(diǎn).

表2 Trek 1 000的硬件組成Tab.2 Hardware composition of trek 1 000

3.2 基于UWB的三維空間定位實(shí)驗(yàn)

在實(shí)驗(yàn)中,4個(gè)錨點(diǎn)分置于1個(gè)正方體8個(gè)頂點(diǎn)的其中4個(gè)上,所需定位的物體(如四旋翼無人機(jī)等)攜帶tag(即UWB漫游器),如此實(shí)現(xiàn)定位,如圖4所示.

圖3 實(shí)驗(yàn)中的UWB錨點(diǎn)Fig.3 UWB anchor point in the experiment

圖4 測(cè)量的系統(tǒng)空間樣式Fig.4 Measured system space style

運(yùn)行如下循環(huán)賦值語句,賦予變量一些隨機(jī)的初值

在賦值完成后,可以通過運(yùn)行Python軟件中的IDLE來進(jìn)行檢測(cè),看是否能獲得大量隨機(jī)數(shù)據(jù).程序編寫主要完成Python串口程序、三維空間定位程序、多種優(yōu)化濾波程序等的編寫工作.通過UWB技術(shù)進(jìn)行數(shù)據(jù)采集,主要是采集4個(gè)錨點(diǎn)到需測(cè)點(diǎn)的距離d1、d2、d3、d4,并將其過串口連接程序輸入到三維定位算法的程序中,經(jīng)過計(jì)算,得到未知點(diǎn)的坐標(biāo),由于實(shí)驗(yàn)誤差,未知點(diǎn)的坐標(biāo)會(huì)在一片范圍內(nèi)波動(dòng),此時(shí)采用優(yōu)化濾波的程序,減小波動(dòng)的范圍,完成實(shí)驗(yàn).

在該程序中,選取了

這4個(gè)錨點(diǎn)在長寬高都為1的正方體的頂點(diǎn)上,設(shè)未知點(diǎn) (x0,y0,z0) 處于正方體的中心,則到4點(diǎn)距離分別為

最終經(jīng)過程序的計(jì)算定位得到未知點(diǎn)的坐標(biāo)為(0.5,0.5,0.5),該點(diǎn)剛好位于正方體的中心,由此證明此程序的正確性.

圖5 測(cè)量所得數(shù)據(jù)Fig.5 Measured data

實(shí)驗(yàn)通過測(cè)量四旋翼無人飛行器的三維空間坐標(biāo)證明所編寫的算法是可以實(shí)現(xiàn)的,利用四旋翼無人飛行器進(jìn)行的實(shí)驗(yàn)過程.電腦程序輸出的一部分坐標(biāo)值見圖5.實(shí)驗(yàn)得到眾多三維空間坐標(biāo),大范圍是一致的,但也存在一些誤差波動(dòng),圖5是實(shí)驗(yàn)測(cè)量得到三維坐標(biāo)換算成的數(shù)學(xué)數(shù)值,利用了相減而后求均值的方法,下面的優(yōu)化濾波都是對(duì)這些數(shù)值進(jìn)行的.

濾波是指利用一定的手段抑制無用信號(hào),增強(qiáng)有用信號(hào)的數(shù)字信號(hào)處理過程.優(yōu)化濾波是本設(shè)計(jì)的主要內(nèi)容之一,如果沒有優(yōu)化濾波,測(cè)得的三維空間坐標(biāo)就會(huì)存在很大的誤差,經(jīng)過濾波的數(shù)據(jù)可以使得定位更加精確.擴(kuò)展卡爾曼濾波算法原理圖如圖6所示.

在本設(shè)計(jì)中,三維定位算法計(jì)算得到的大范圍坐標(biāo)數(shù)據(jù)輸入卡爾曼濾波程序中,經(jīng)過篩選,留下小范圍內(nèi)的坐標(biāo).上述三維空間定位算法采用了矩陣的形式,此處卡爾曼濾波也使用相同的方法.將得到的坐標(biāo)換算成數(shù)值進(jìn)行4種優(yōu)化濾波:卡爾曼濾波、平滑濾波、中值濾波、曲線擬合.運(yùn)行優(yōu)化濾波程序,可得如圖7～11的5幅優(yōu)化濾波后的曲線或折線圖.

圖6 擴(kuò)展卡爾曼濾波算法原理圖Fig.6 Schematic diagram of extended Kalman filter algorithm

圖7 卡爾曼濾波Fig.7 Kalman filtering

圖8 卡爾曼濾波方差Fig.8 Kalman filter variance

圖9 平滑濾波Fig.9 Smooth filtering

圖10 中值濾波Fig.10 Median Filtering

圖11 曲線擬合Fig.11 Curve Fitting

對(duì)測(cè)得的空間坐標(biāo)進(jìn)行優(yōu)化濾波,表3給出了4種濾波方法的性能比較.通過表格的對(duì)比,可以看出,卡爾曼濾波程序運(yùn)行速度是第二高,而且其對(duì)環(huán)境的需求不是特別高,其性價(jià)比和精度很高,相比較于其他3種濾波,卡爾曼濾波是本次實(shí)驗(yàn)所得到的最優(yōu)選擇.

表3 4種濾波對(duì)比表格Tab.3 Four filter comparison tables

3.3 實(shí)驗(yàn)結(jié)論

本實(shí)驗(yàn)主要完成基于超寬帶的三維空間定位,同時(shí)對(duì)坐標(biāo)進(jìn)行優(yōu)化.實(shí)驗(yàn)環(huán)境為有書桌、椅子和柜子的實(shí)驗(yàn)室,2 m×2 m×2 m 的無障礙區(qū)域.利用超寬帶技術(shù)測(cè)距,而后經(jīng)過三維空間定位算法的程序得出概略坐標(biāo),最后由優(yōu)化濾波完成實(shí)驗(yàn),最終確定優(yōu)化濾波方案是卡爾曼濾波.

4 結(jié)論

超寬帶技術(shù)目前是一種主流的定位技術(shù),其具有發(fā)射信號(hào)功率譜密度低、抗多徑、安全系數(shù)高、系統(tǒng)復(fù)雜度低、傳輸速率高和穿透能力強(qiáng)等特點(diǎn).本文給出了一種基于超寬帶的三維定位算法,并對(duì)獲得三維空間的坐標(biāo)數(shù)據(jù)進(jìn)行優(yōu)化濾波比較,確定了室內(nèi)4旋翼飛行器定位的卡爾曼濾波方案.實(shí)驗(yàn)結(jié)果表明,本文所提出的基于超寬帶的三維定位和優(yōu)化濾波方法是有效的.