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

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

1 概 述

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

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

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

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

2 UWB定位原理

UWB定位一般要先獲取測距信息，然后建立位置坐標(biāo)解算模型，以測距信息作為數(shù)學(xué)模型的輸入，求解坐標(biāo)信息，測距定位原理圖如圖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)，通過UWB測距可以得到目標(biāo)點(diǎn)距離各基站的距離，設(shè)為d1，d2…dn，則有：

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

AX=b

(6)

X=(ATA)-1ATb

(7)

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

3 測距濾波

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

Xk+1=FXk+wk

(8)

測距模型的觀測方程為：

Yk=HXk+vk

(9)

3.1 卡爾曼濾波

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

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

卡爾曼濾波的一般過程為：

式中，Pk為k時(shí)刻協(xié)方差矩陣，G是噪聲驅(qū)動矩陣，卡爾曼濾波首先對狀態(tài)進(jìn)行一步預(yù)測，然后預(yù)測協(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)典的分析方法又解決不了的場合，有著巨大的發(fā)展?jié)摿ΑＤ壳?，粒子濾波在機(jī)器人定位、目標(biāo)跟蹤等領(lǐng)域已經(jīng)得到較為廣泛的應(yīng)用。

粒子濾波的一般過程為：

2)重要性采樣。

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

權(quán)值歸一化：

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

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

3.3 測距濾波

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

圖3 KF/PF測距濾波誤差

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

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

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

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

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

表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é)果跳動范圍更大，經(jīng)濾波處理后的定位跳動范圍明顯減小，且PF測距濾波的范圍最小，KF次之。定位誤差見表3，UWB直接根據(jù)測距結(jié)果計(jì)算得到的定位結(jié)果存在較大的定位誤差，最大定位誤差為9.41cm，測距定位跳動較大，X方向最大誤差2.94cm，Y方向最大測距誤差為7.43cm，在經(jīng)過卡爾曼濾波和粒子濾波之后定位精度明顯提高，定位跳動誤差明顯減小，卡爾曼濾波后X方向最大定位誤差1.28cm，相比UWB測距直接解算降低了56.4%，Y方向最大定位誤差3.83cm，相比UWB測距直接解算降低了48.5%，最大定位誤差為4.51cm，相比UWB測距直接解算降低了39.3%，粒子濾波的靜態(tài)定位效果最好，X最大定位誤差小于1cm，相比UWB測距直接解算降低了70.7%，相比卡爾曼濾波降低了32.8%，Y方向最大定位誤差1.18cm，相比UWB測距直接解算降低了84.1%，相比卡爾曼濾波降低了69.1%，最大定位誤差為1.83cm，相比UWB測距直接解算降低了75.4%，相比卡爾曼濾波降低了59.4%。同時(shí)，受UWB基站布置形式的影響，X方向上的靜態(tài)定位誤差明顯小于Y方向上的誤差。

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

4.2 動態(tài)定位

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

圖6 動態(tài)測距濾波

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

圖7 KF/PF測距濾波動態(tài)定位

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

5 結(jié) 論

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

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

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

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