基于UWB的三維定位和優化濾波方法

李楠梁冰

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

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

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

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

1 三維定位的三邊測量法

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

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

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

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

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

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

在上面所列舉的平面中的定位算法的基礎上,可推廣出三維空間定位法.在空間中的某一平面內任取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個圓,根據二維向三維的擴展,得出未知點的位置計算公式為

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

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

2 三維定位的優化濾波法

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

2.1 卡爾曼濾波

假設系統的離散狀態方程為

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

根據系統的模型,可以根據系統的上一狀態而預測出現在狀態:

式(4)中,X(k|k-1)是利用上一狀態預測的結果,X(k-1|k-1)是上一狀態最優的結果,U(k)為現在狀態的控制量,可以為0.

此時,系統結果已經更新,但對應于X(k|k-1)的協方差還沒更新,協方差P為

式(5)中,P(k|k-1)是X(k|k-1)對應的協方差,P(k-1|k-1)是X(k-1|k-1)對應協方差,Q是系統過程的協方差,A′表示A的轉置矩陣.式(4)和(5)就是對系統狀態的預測.

可得現在狀態(k)的最優估算值X(k|k):

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

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

式(8)中,I是值為1的矩陣.當系統進入k+1狀態時,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為該模板中包含當前坐標在內的坐標總數.

2.3 中值濾波

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

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

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

2.4 曲線擬合

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

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

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

可得到擬合后的函數.

3 基于UWB的三維定位與優化濾波的實驗分析

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

表1 室內定位技術對比Tab.1 Comparison of indoor positioning technology

3.1 硬件環境

超寬帶在本次實驗中采用的硬件由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漫游器),如此實現定位,如圖4所示.

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

圖4 測量的系統空間樣式Fig.4 Measured system space style

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

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

在該程序中,選取了

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

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

圖5 測量所得數據Fig.5 Measured data

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

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

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

圖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

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

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

3.3 實驗結論

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

4 結論

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