基于KF/PF測(cè)距濾波的UWB精確定位試驗(yàn)研究

姚 健，劉送永，崔玉明，顧聰聰

(中國(guó)礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州 221116)

1 概 述

我國(guó)煤炭開(kāi)采已從機(jī)械化、自動(dòng)化正逐步向智能化邁進(jìn)[1]，發(fā)展智慧煤礦是我國(guó)煤炭工業(yè)發(fā)展的必由之路[2]，井下精確定位是智慧煤礦精準(zhǔn)控制的基礎(chǔ)[3]，UWB(Ultra Wide Band)技術(shù)因其功耗低、傳輸速率高[4-5]、穿透能力強(qiáng)[6]、高靈活性、高可靠性以及高精度測(cè)距等特性，在井下高精度定位領(lǐng)域得到廣泛的應(yīng)用。

在UWB定位系統(tǒng)中，當(dāng)移動(dòng)目標(biāo)不能滿足視距條件時(shí)，信號(hào)的傳播只能通過(guò)折射和衍射等非視距方式，此時(shí)就會(huì)造成一定的NLOS(Non-Line-of- Sight)誤差[7]，如圖1所示，在發(fā)射端信號(hào)無(wú)法直接到達(dá)接收端，會(huì)以反射和折射的方式繞過(guò)障礙物，得到的測(cè)距值就會(huì)大于真實(shí)值，從而造成測(cè)距和定位誤差，另外定位過(guò)程中的環(huán)境噪聲以及其它干擾都會(huì)對(duì)UWB定位結(jié)果產(chǎn)生影響，產(chǎn)生測(cè)距異常值[8]，因此，亟需對(duì)UWB定位過(guò)程中，NLOS誤差和噪聲的抑制進(jìn)行研究。

在UWB定位方面，國(guó)內(nèi)外學(xué)者已經(jīng)進(jìn)行了大量的研究，唐春玲等人[9]將井下人員定位作為研究對(duì)象，對(duì)比了UWB以及另外三種定位技術(shù)的環(huán)境適用性，結(jié)果表明，UWB定位技術(shù)在井下定位方面有著非常大的發(fā)展?jié)摿?；趙文生等人[10]結(jié)合井下工作面實(shí)際工作環(huán)境，對(duì)超寬帶脈沖信號(hào)模型和測(cè)距原理進(jìn)行了深入研究，通過(guò)對(duì)通信參數(shù)的優(yōu)化，將UWB測(cè)距誤差控制在50cm以內(nèi)；Guvenc等[11]提出一種基于多徑信道統(tǒng)計(jì)信息的NLOS識(shí)別技術(shù)，開(kāi)發(fā)了聯(lián)合似然比，實(shí)現(xiàn)LOS/NLOS的有效識(shí)別；肖竹等人[12，13]對(duì)UWB定位研究的發(fā)展歷程和研究現(xiàn)狀進(jìn)行了詳細(xì)的分析，指出了UWB定位仍存在的問(wèn)題和發(fā)展趨勢(shì)，通過(guò)對(duì)NLOS信道模型的仿真，證明基于最強(qiáng)路徑檢測(cè)的TOA估計(jì)方法，可以有效抑制NLOS誤差；張健銘等人[14]提出一種UWB/PDR融合定位方案，利用支持向量機(jī)回歸模型對(duì)復(fù)雜環(huán)境中UWB定位誤差進(jìn)行預(yù)測(cè)，并在擴(kuò)展卡爾曼濾波的基礎(chǔ)上添加自適應(yīng)調(diào)節(jié)系數(shù)，有效抑制了NLOS誤差，提高了UWB定位的精度和可靠性；Gonzalez等[15]將UWB測(cè)距值和里程計(jì)測(cè)距值通過(guò)粒子濾波器進(jìn)行融合，有效抑制了由于多徑效應(yīng)而產(chǎn)生的定位誤差；朱帶先等人[16]采用粒子濾波算法對(duì)UWB定位進(jìn)行估計(jì)，并采用TDOA/RSSI聯(lián)合定位技術(shù)進(jìn)行位置修正，有效降低了多徑和NLOS誤差的影響。

現(xiàn)有的研究多集中于對(duì)UWB整個(gè)定位系統(tǒng)以及聯(lián)合外部定位方式進(jìn)行數(shù)據(jù)融合，進(jìn)而實(shí)現(xiàn)定位誤差的修正。本文在UWB定位原理的基礎(chǔ)上，從UWB測(cè)距值入手，采用卡爾曼濾波和粒子濾波對(duì)UWB測(cè)距信息進(jìn)行濾波處理，從而抑制NLOS誤差和隨機(jī)噪聲的影響，以提高UWB定位精度和可靠性。

2 UWB定位原理

UWB定位一般要先獲取測(cè)距信息，然后建立位置坐標(biāo)解算模型，以測(cè)距信息作為數(shù)學(xué)模型的輸入，求解坐標(biāo)信息，測(cè)距定位原理圖如圖2所示，設(shè)目標(biāo)的坐標(biāo)為P(x，y)，定位區(qū)域內(nèi)有n個(gè)基站，坐標(biāo)分別為A0(x0，y0)，A1(x1，y1)，A2(x2，y2)，A3(x3，y3)….An(xn，yn)，通過(guò)UWB測(cè)距可以得到目標(biāo)點(diǎn)距離各基站的距離，設(shè)為d1，d2…dn，則有：

對(duì)式(1)進(jìn)行分解移項(xiàng)可得：

AX=b

(6)

X=(ATA)-1ATb

(7)

由此即可求得目標(biāo)點(diǎn)的坐標(biāo)。

3 測(cè)距濾波

UWB實(shí)現(xiàn)精確定位的基礎(chǔ)是精確的測(cè)距信息，在實(shí)際工作過(guò)程中，測(cè)距結(jié)果經(jīng)常因?yàn)镹LOS和噪聲干擾出現(xiàn)較大的誤差，因此亟需對(duì)測(cè)距值進(jìn)行濾波，以提高測(cè)距精度。以UWB基站和目標(biāo)節(jié)點(diǎn)間的測(cè)距值以及測(cè)距變化率作為狀態(tài)參數(shù)X=[dd′]T，則測(cè)距模型的狀態(tài)方程為：

Xk+1=FXk+wk

(8)

測(cè)距模型的觀測(cè)方程為：

Yk=HXk+vk

(9)

3.1 卡爾曼濾波

在工程實(shí)際中，觀測(cè)信號(hào)中不可避免的夾雜著噪聲，狀態(tài)變量的真實(shí)值往往不能直接得到，而卡爾曼濾波(Kalman Filter，KF)能有效降低噪聲影響，且其計(jì)算過(guò)程較為復(fù)雜的缺點(diǎn)，也已隨著計(jì)算機(jī)技術(shù)的發(fā)展得到有效解決[17，18]，目前卡爾曼濾波在目標(biāo)跟蹤、導(dǎo)航和制導(dǎo)等高科技領(lǐng)域得到越來(lái)越廣泛的應(yīng)用。

卡爾曼濾波通過(guò)對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)，抑制觀測(cè)數(shù)據(jù)中噪聲和干擾的影響，從而獲得更加準(zhǔn)確的狀態(tài)估計(jì)。

卡爾曼濾波的一般過(guò)程為：

式中，Pk為k時(shí)刻協(xié)方差矩陣，G是噪聲驅(qū)動(dòng)矩陣，卡爾曼濾波首先對(duì)狀態(tài)進(jìn)行一步預(yù)測(cè)，然后預(yù)測(cè)協(xié)方差，求濾波增益矩陣，而后再進(jìn)行狀態(tài)更新，協(xié)方差跟新，如此循環(huán)計(jì)算，就可以有效抑制NLOS誤差和隨機(jī)噪聲干擾。

3.2 粒子濾波

卡爾曼濾波是高斯模型條件下的狀態(tài)估計(jì)，而粒子濾波(Particle Filter，PF)是非線性非高斯模型條件下的狀態(tài)估計(jì)算法，是一種基于蒙特卡洛的近似貝葉斯濾波，從概率角度出發(fā)，以樣本均值作為最終的參數(shù)估計(jì)[17，19，20]。在一些精度要求較高而經(jīng)典的分析方法又解決不了的場(chǎng)合，有著巨大的發(fā)展?jié)摿?。目前，粒子濾波在機(jī)器人定位、目標(biāo)跟蹤等領(lǐng)域已經(jīng)得到較為廣泛的應(yīng)用。

粒子濾波的一般過(guò)程為：

2)重要性采樣。

3)粒子權(quán)值更新及歸一化權(quán)值更新：

權(quán)值歸一化：

4)重采樣。重要性采樣會(huì)帶來(lái)粒子重要性加權(quán)退化、有效粒子數(shù)減少的問(wèn)題，因此必須進(jìn)行重采樣，重采樣首先將粒子進(jìn)行排序，然后用權(quán)值大的粒子代替權(quán)值小的粒子。

5)狀態(tài)估計(jì)。

3.3 測(cè)距濾波

KF/PF測(cè)距濾波誤差如圖3所示，UWB測(cè)距結(jié)果中包含較多的噪聲以及誤差，靜態(tài)測(cè)距波動(dòng)明顯，誤差波動(dòng)范圍大于5cm，其中因?yàn)樵肼暤母蓴_，出現(xiàn)多個(gè)測(cè)距異常值，測(cè)距誤差超過(guò)15cm，經(jīng)過(guò)卡爾曼濾波以及粒子濾波處理后，有效抑制了測(cè)距異常值，且測(cè)距誤差明顯減小，卡爾曼濾波后最大測(cè)距誤差小于3cm，粒子濾波的效果明顯好于卡爾曼濾波的效果，測(cè)距誤差小于1cm。

圖3 KF/PF測(cè)距濾波誤差

4 實(shí)驗(yàn)與分析

實(shí)驗(yàn)中使用的UWB定位模塊為深圳空循環(huán)科技有限公司生產(chǎn)的Link-Track UWB定位模塊，模塊標(biāo)簽與基站一體，具體參數(shù)見(jiàn)表1，由四個(gè)基站和一個(gè)標(biāo)簽組成UWB定位系統(tǒng)。

表1 UWB定位模塊參數(shù)

實(shí)驗(yàn)場(chǎng)景為地下車庫(kù)，如圖4所示，使用四個(gè)基站，以基站A0為坐標(biāo)原點(diǎn)，A0-A3為定位坐標(biāo)系的X軸正方向，A1、A2在Y軸正方向一側(cè)，基站坐標(biāo)見(jiàn)表2。

圖4 實(shí)驗(yàn)場(chǎng)景

表2 基站坐標(biāo)

4.1 靜態(tài)定位

圖5 KF/PF靜態(tài)定位

靜態(tài)定位如圖5所示，五角星所示點(diǎn)為靜態(tài)定位點(diǎn)的實(shí)際坐標(biāo)，直接解算得到的靜態(tài)定位結(jié)果跳動(dòng)范圍更大，經(jīng)濾波處理后的定位跳動(dòng)范圍明顯減小，且PF測(cè)距濾波的范圍最小，KF次之。定位誤差見(jiàn)表3，UWB直接根據(jù)測(cè)距結(jié)果計(jì)算得到的定位結(jié)果存在較大的定位誤差，最大定位誤差為9.41cm，測(cè)距定位跳動(dòng)較大，X方向最大誤差2.94cm，Y方向最大測(cè)距誤差為7.43cm，在經(jīng)過(guò)卡爾曼濾波和粒子濾波之后定位精度明顯提高，定位跳動(dòng)誤差明顯減小，卡爾曼濾波后X方向最大定位誤差1.28cm，相比UWB測(cè)距直接解算降低了56.4%，Y方向最大定位誤差3.83cm，相比UWB測(cè)距直接解算降低了48.5%，最大定位誤差為4.51cm，相比UWB測(cè)距直接解算降低了39.3%，粒子濾波的靜態(tài)定位效果最好，X最大定位誤差小于1cm，相比UWB測(cè)距直接解算降低了70.7%，相比卡爾曼濾波降低了32.8%，Y方向最大定位誤差1.18cm，相比UWB測(cè)距直接解算降低了84.1%，相比卡爾曼濾波降低了69.1%，最大定位誤差為1.83cm，相比UWB測(cè)距直接解算降低了75.4%，相比卡爾曼濾波降低了59.4%。同時(shí)，受UWB基站布置形式的影響，X方向上的靜態(tài)定位誤差明顯小于Y方向上的誤差。

表3 靜態(tài)定位誤差

4.2 動(dòng)態(tài)定位

手持目標(biāo)節(jié)點(diǎn)，按預(yù)設(shè)軌跡行走一圈，四個(gè)基站與移動(dòng)節(jié)點(diǎn)間的動(dòng)態(tài)測(cè)距結(jié)果如圖6所示，行走過(guò)程中因?yàn)槿梭w的遮擋存在一定的NLOS誤差，又因?yàn)闇y(cè)距過(guò)程中隨機(jī)噪聲的影響，UWB測(cè)距值存在明顯的抖動(dòng)現(xiàn)象，在經(jīng)卡爾曼和粒子濾波處理之后，抖動(dòng)明顯減少，曲線更加光滑。

圖6 動(dòng)態(tài)測(cè)距濾波

動(dòng)態(tài)定位結(jié)果如圖7所示，按預(yù)定行走過(guò)程中，因?yàn)槿梭w遮擋產(chǎn)生的NLOS誤差和噪聲干擾，直接根據(jù)測(cè)距值解算得到的UWB定位結(jié)果存在明顯的定位跳動(dòng)現(xiàn)象，經(jīng)KF以及PF測(cè)距濾波處理后，得到的定位結(jié)果相比直接解算得到的結(jié)果，軌跡更加平滑，更加貼近真實(shí)軌跡。定位誤差見(jiàn)表4，UWB直接測(cè)距解算的定位結(jié)果在整個(gè)行走過(guò)程相較于參考軌跡存在明顯的跳動(dòng)誤差，X方向最大誤差12.12cm，Y方向最大測(cè)距誤差為24.85cm，最大定位誤差為36.41cm，在經(jīng)過(guò)卡爾曼以及粒子測(cè)距濾波之后，UWB定位跳動(dòng)誤差明顯減少，定位曲線更加光滑，卡爾曼濾波后，X方向最大誤差11.31cm，Y方向最大誤差為21.77cm，最大定位誤差22.13cm，相較于UWB直接測(cè)距解算，X方向降低了6.68%，Y方向降低了12.39%，最大定位誤差降低39.21%，粒子濾波后，X方向最大誤差8.94cm，Y方向最大測(cè)距誤差為10.92cm，最大定位誤差為18.03cm，相較于UWB直接測(cè)距解算，X方向降低了26.24%，Y方向降低了56.1%，最大定位誤差降低了50.48%，相較于卡爾曼濾波，X方向降低了20.9%，Y方向降低了49.8%，最大定位誤差降低了18.5%。同時(shí)，受UWB基站布置形式的影響，X方向上的動(dòng)態(tài)定位誤差明顯小于Y方向上的誤差。

圖7 KF/PF測(cè)距濾波動(dòng)態(tài)定位

表4 動(dòng)態(tài)定位誤差

5 結(jié) 論

1)采用卡爾曼和和粒子濾波方式，可以有效抑制UWB測(cè)距過(guò)程中的NLOS誤差和隨機(jī)噪聲干擾，粒子濾波相較于卡爾曼濾波抑制效果更好。

2)靜態(tài)定位中，卡爾曼和粒子濾波都可以很好的抑制隨機(jī)噪聲的干擾，卡爾曼測(cè)距濾波后的定位精度可以達(dá)到4.51cm，粒子測(cè)距濾波相較于卡爾曼濾波靜態(tài)定位精度提高59.4%。

3)采用卡爾曼和粒子測(cè)距濾波的方式進(jìn)行UWB動(dòng)態(tài)定位，可以有效降低NLOS誤差和噪聲的影響，提高UWB定位的精度和可靠性，卡爾曼測(cè)距濾波動(dòng)態(tài)定位精度可以達(dá)到22.13cm，粒子測(cè)距濾波動(dòng)態(tài)定位精度相較于卡爾曼濾波提高18.5%。

4)UWB基站布置方式會(huì)對(duì)UWB定位精度有產(chǎn)生很大影響，針對(duì)基站布置方式與定位誤差之間的關(guān)系，還需進(jìn)行更加深入的研究。