李楠梁冰

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

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

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

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

1 三維定位的三邊測量法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 卡爾曼濾波

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

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

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

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

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

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

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

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

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

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

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

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

2.2 平滑濾波

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

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

2.3 中值濾波

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

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

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

2.4 曲線擬合

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

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

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

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

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

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

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

3.1 硬件環(huán)境

超寬帶在本次實驗中采用的硬件由4個錨點和UWB漫游器等諸多元器件組成,表2是實驗硬件列表,圖3是UWB錨點.

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

3.2 基于UWB的三維空間定位實驗

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

圖3 實驗中的UWB錨點Fig.3 UWB anchor point in the experiment

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

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

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

在該程序中,選取了

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

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

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

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

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

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

圖6 擴展卡爾曼濾波算法原理圖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

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

表3 4種濾波對比表格Tab.3 Four filter comparison tables

3.3 實驗結(jié)論

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

4 結(jié)論

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