井下超寬帶人員定位的改進(jìn)卡爾曼濾波算法

劉永立， 吳聞軒

(黑龍江科技大學(xué) 礦業(yè)工程學(xué)院， 哈爾濱 150022)

0 引 言

超寬帶與短距通信技術(shù)相比擁有定位精度高、穿透性強(qiáng)，以及抗多徑干擾能力強(qiáng)、功耗低等特點(diǎn)[1-2]，因此，更適用于井下人員定位。但在定位過程中，由于煤礦井下復(fù)雜環(huán)境，導(dǎo)致超寬帶通過無線傳輸數(shù)據(jù)時(shí)，受采煤機(jī)械發(fā)出的電磁波及來自其他設(shè)備的噪聲干擾[3-4]，導(dǎo)致測(cè)距值偏移，定位精度波動(dòng)大，因此，如何減小噪聲帶來的干擾，成為了超寬帶定位技術(shù)應(yīng)用于煤礦的關(guān)鍵。

郭繼坤等[5]提出了奇異點(diǎn)帶內(nèi)濾波技術(shù)應(yīng)用于UWB通信，通過分析小波奇異性模極大值衰減速度描述UWB的奇異性，仿真實(shí)驗(yàn)驗(yàn)證了該技術(shù)有利于提升信噪比。譚玉新等[6]通過無損卡爾曼濾波技術(shù)(UFK)對(duì)井下機(jī)器人進(jìn)行定位，運(yùn)用UFK對(duì)機(jī)器人的坐標(biāo)及航向角度進(jìn)行濾波，得出該方法優(yōu)于光電碼盤定位的結(jié)論。邢智鵬等[7]提出LQI濾波和RSSI結(jié)合的定位算法，通過LQI指標(biāo)的濾波性能對(duì)數(shù)據(jù)進(jìn)行濾波提高抗干擾性，最終通過紫峰硬件平臺(tái)驗(yàn)證該算法的有效性及優(yōu)勢(shì)。

筆者基于卡爾曼濾波可消除噪聲的基礎(chǔ)上，結(jié)合卡爾曼濾波與超寬帶TWR算法，提出卡爾曼濾波改進(jìn)算法，對(duì)測(cè)距數(shù)據(jù)進(jìn)行濾波處理，分離測(cè)距數(shù)據(jù)與誤差，以使兩者互不干涉。

1 超寬帶及定位原理

1.1 超寬帶

超寬帶(Ultra-wide band，UWB)是一種可以給無線局域網(wǎng)WLAN和個(gè)人局域網(wǎng)PAN帶來低功耗、大帶寬并且能進(jìn)行短距離無線通訊的無線通信技術(shù)，與現(xiàn)在的主流的4G通信方式不同之處在于超寬帶可以進(jìn)行無載波通信[8]，其數(shù)據(jù)傳輸可以僅通過非正弦波的窄脈沖進(jìn)行傳輸。與其他短距通信技術(shù)相比，超寬帶擁有定位精度高、穿透性強(qiáng)、抗多徑干擾能力強(qiáng)、功耗低及傳輸距離遠(yuǎn)[9]等特點(diǎn)，使超寬帶技術(shù)更適用于煤礦井下人員定位。

由于井下工作環(huán)境復(fù)雜，大型機(jī)械設(shè)備的噪音、電磁波等對(duì)人員定位技術(shù)干擾大，故亟需改進(jìn)人員定位算法，提高井下人員定位技術(shù)的精度和抗干擾能力。

1.2 超寬帶三邊定位算法

為減弱采場(chǎng)地應(yīng)力變化對(duì)晶振時(shí)鐘的影響，選擇三邊定位算法提高定位精度[10]，原理如圖1所示。以基站本體為中心，半徑50～600 m的圓形范圍內(nèi)對(duì)標(biāo)簽信號(hào)進(jìn)行捕捉，上位機(jī)根據(jù)基站接收的測(cè)距值d0、d1、d2進(jìn)行定位。

圖1 三邊定位算法原理Fig. 1 Trilateral positioning principle

定位源于測(cè)距，基于TOA的TWR測(cè)距算法在煤礦井下因電磁干擾混入噪聲信號(hào)時(shí)易產(chǎn)生數(shù)據(jù)錯(cuò)位，導(dǎo)致定位精確度下降，因此,采用TWR改進(jìn)算法。

1.3 TWR改進(jìn)算法

TWR改進(jìn)算法[11]原理如圖2所示。

圖2 超寬帶TWR改進(jìn)算法Fig. 2 Improved UWB TWR algorithm

相較于傳統(tǒng)TWR算法，增加了RESP數(shù)據(jù)包，用于確保數(shù)據(jù)包在信息傳遞過程中不會(huì)發(fā)生錯(cuò)位現(xiàn)象，根據(jù)標(biāo)簽與基站傳輸?shù)臅r(shí)間差確定測(cè)距值為

(1)

式中:c——光速;

Tr——測(cè)距周期;

Ts——標(biāo)簽回復(fù)時(shí)間用于確認(rèn)數(shù)據(jù)包編號(hào)。

井下人員定位時(shí)，標(biāo)簽發(fā)出的信號(hào)會(huì)混入白噪音，基站接受的信號(hào)會(huì)捕捉到機(jī)械設(shè)備發(fā)出的電磁波，這些都會(huì)降低定位的精度。式(1)僅考慮了數(shù)據(jù)交互的準(zhǔn)確性，未考慮在數(shù)據(jù)傳輸過程中的噪聲和電磁波干擾，測(cè)距誤差在0.5 m左右。因此，需要加入一種能夠?yàn)V除噪聲和電磁波干擾的算法，減少測(cè)距數(shù)據(jù)偏移，提高定位精度。

2 井下人員定位算法

2.1 卡爾曼濾波算法原理

卡爾曼(Kalman)濾波算法是基于狀態(tài)空間的遞推濾波算法[12]，如圖3所示。其算法主要針對(duì)狀態(tài)空間和測(cè)量空間，前者通過狀態(tài)方程描述相近時(shí)段狀態(tài)轉(zhuǎn)移變化規(guī)律，后者反映觀測(cè)值與狀態(tài)值之間的關(guān)系。

圖3 卡爾曼濾波流程Fig. 3 Flow of Kalman filter

初始狀態(tài)和均方差的一步預(yù)測(cè)：

X(K)=φKX(K-1) ，

(2)

式中:X(K)——K時(shí)刻系統(tǒng)真實(shí)狀態(tài)矩陣；

φK——狀態(tài)轉(zhuǎn)移矩陣，包含位移、速度和加速度等關(guān)系式；

DX(K,K-1)——系統(tǒng)協(xié)方差矩陣。

系統(tǒng)真實(shí)值Z(K)的測(cè)量為

Z(K)=HkX(K)+V(K)，

式中:HK——m×n階矩陣，表示X(K)對(duì)測(cè)量變量Z(K)的增益；

V(K)——期望值為0的觀測(cè)噪聲。

卡爾曼收益為

式中，RK——觀測(cè)噪聲V(K)的協(xié)方差矩陣。

將收益代入狀態(tài)更新及均方差更新公式中，更新當(dāng)前狀態(tài)

X(K,K)=X(K,K-1)+KK[ZK-HKX(K,K-1)]，

DX(K,K)=(I-KK×HK)DK(K,K-1)，

式中，I——單位矩陣。

對(duì)超寬帶井下人員定位信號(hào)的濾波問題在于卡爾曼濾波算法和TWR改進(jìn)算法結(jié)合時(shí)，如何降低測(cè)距數(shù)據(jù)中的系統(tǒng)噪聲和白噪聲，結(jié)合測(cè)距數(shù)據(jù)及狀態(tài)轉(zhuǎn)移規(guī)律來得到定位的最優(yōu)估計(jì)結(jié)果。

2.2 卡爾曼濾波改進(jìn)算法

采用遞歸方法將卡爾曼濾波算法與TWR算法相結(jié)合，能夠有效解決噪音和電磁波干擾。根據(jù)測(cè)距數(shù)據(jù)及狀態(tài)轉(zhuǎn)移規(guī)律，使用當(dāng)前測(cè)距值以及前一個(gè)測(cè)距周期即可進(jìn)行估值計(jì)算，得到定位的最優(yōu)估計(jì)結(jié)果。流程如圖4所示。

圖4 卡爾曼濾波改進(jìn)算法流程Fig. 4 Improved flow with Kalman filter combined with UWB downhole positioning

使用隨機(jī)線性微分方程，用于預(yù)測(cè)當(dāng)前狀態(tài)X(K,K-1)，根據(jù)系統(tǒng)狀態(tài)變量K-1時(shí)刻的最優(yōu)值和系統(tǒng)輸入計(jì)算出K時(shí)刻的狀態(tài)預(yù)測(cè)值為

X(K,K-1)=φKX(K-1,K-1)+BKUK+WK，

(3)

式中:φK——在X(K,K-1)狀態(tài)下的變換矩陣，是井下人員定位算法對(duì)于狀態(tài)變量進(jìn)行預(yù)測(cè)的基礎(chǔ)；

BK——系統(tǒng)參數(shù)；

UK——K時(shí)刻對(duì)系統(tǒng)的控制增益或者控制量；

WK——白噪聲，當(dāng)X(K)為一維定位數(shù)據(jù)時(shí)，WK=1，UK不存在。

協(xié)方差方程用于時(shí)間預(yù)測(cè)為

(4)

式中,QK——系統(tǒng)噪聲的協(xié)方差，即系統(tǒng)誤差。

將QK納入時(shí)間預(yù)測(cè)可以使其在不斷向前預(yù)測(cè)的過程中迅速收斂。

利用式(4)的所得結(jié)果，代入卡爾曼增益方程，計(jì)算卡爾曼收益

(5)

式中,HK——標(biāo)簽的預(yù)測(cè)矩陣，當(dāng)DX(K)為一維矩陣時(shí)HK=1。

更新最優(yōu)值方程時(shí)，結(jié)合卡爾曼濾波的測(cè)量方程與TWR改進(jìn)算法，計(jì)算出K時(shí)刻狀態(tài)變量的最優(yōu)值為

X(K,K)=X(K,K-1)+KK[rho(K)-

(6)

式中:rho(k)——曲線飽滿值，用于濾除超寬帶信號(hào)測(cè)距傳輸過程中混入的噪聲，減少數(shù)據(jù)抖動(dòng)使得定位曲線平滑，使用當(dāng)前坐標(biāo)對(duì)X(K)的狀態(tài)方程進(jìn)行更新;

Δt——測(cè)距間隔周期值。

由式(4)、(5)可以推算更新協(xié)方差方程，得出K時(shí)刻的協(xié)方差矩陣DX(K,K)，同時(shí)為得出K+1時(shí)刻的輸出值做鋪墊

DX(K,K)=(I-KKHK)×DK(K,K-1)。

(7)

利用K時(shí)刻測(cè)距值對(duì)K+1時(shí)刻繼續(xù)進(jìn)行時(shí)間預(yù)測(cè)和測(cè)量更新，重復(fù)式(3)～(7)的計(jì)算過程，直到測(cè)距結(jié)束完成對(duì)井下人員定位數(shù)據(jù)的濾波過程。

卡爾曼濾波改進(jìn)算法是根據(jù)當(dāng)前基站的測(cè)距值、上一時(shí)刻預(yù)測(cè)的測(cè)距值及誤差，計(jì)算得出當(dāng)前最優(yōu)值，再進(jìn)入下一個(gè)預(yù)測(cè)循環(huán)中。該算法使混入的噪聲及電磁波產(chǎn)生的誤差獨(dú)立存在，降低誤差干擾。

3 仿真與實(shí)際測(cè)試

3.1 仿真實(shí)驗(yàn)

為了驗(yàn)證經(jīng)卡爾曼濾波算法改進(jìn)后的超寬帶煤礦井下人員定位系統(tǒng)的定位精度、可靠性及輸出數(shù)據(jù)是否穩(wěn)定，使用Matlab進(jìn)行仿真，仿真環(huán)境設(shè)置如圖5所示，長(zhǎng)度為30 m，寬度為3 m長(zhǎng)方形區(qū)域，設(shè)定采煤工作面生產(chǎn)過程中，采煤設(shè)備噪聲達(dá)到90 dB，巷道內(nèi)電纜符合MT818-2009規(guī)定，極少出現(xiàn)打火現(xiàn)象；T0到T2為三個(gè)基站，仿真設(shè)置由人員佩戴標(biāo)簽在該范圍內(nèi)從T0處出發(fā)沿規(guī)劃路線行走，數(shù)據(jù)掃描周期為5 ms共掃描30次。

圖5 仿真環(huán)境設(shè)置示意Fig. 5 Schematic of simulation environment setting

將卡爾曼濾波算法改進(jìn)前后超寬帶定位路線與規(guī)劃路線進(jìn)行比對(duì)，橫坐標(biāo)d為巷道寬度，縱坐標(biāo)l為巷道長(zhǎng)度，如圖6所示。對(duì)圖6c卡爾曼濾波改進(jìn)前后兩組路線數(shù)據(jù)進(jìn)行位置偏移值擬合，如圖7所示。

圖6 卡爾曼濾波算法改進(jìn)前后與仿真規(guī)劃路線比對(duì)Fig. 6 Kalman filter algorithm was compared with simulation planning route before and after improvement

圖7 卡爾曼濾波算法改進(jìn)前后定位偏移值擬合函數(shù)Fig. 7 Kalman filter function turns on front and rear positioning offset fitting function

由仿真結(jié)果可知，卡爾曼濾波算法改進(jìn)前定位數(shù)據(jù)受到干擾，導(dǎo)致給出的定位路線與設(shè)定的行動(dòng)路線偏差較大，而卡爾曼濾波算法改進(jìn)后，定位路線更加接近于規(guī)劃路線。由擬合函數(shù)表達(dá)式y(tǒng)=9.98x+41.262可知，斜率越大說明卡爾曼濾波改進(jìn)算法對(duì)于定位數(shù)據(jù)的降噪性越強(qiáng)。

3.2 實(shí)際測(cè)試

為了驗(yàn)證卡爾曼濾波改進(jìn)算法能使超寬帶井下人員定位系統(tǒng)在煤礦井下傳輸?shù)臄?shù)據(jù)更平穩(wěn)，定位更精確，依托黑龍江科技大學(xué)地下模擬礦井,現(xiàn)場(chǎng)實(shí)測(cè)如圖8所示。

圖8 現(xiàn)場(chǎng)實(shí)測(cè)Fig. 8 Field test

測(cè)試環(huán)境設(shè)置為綜采工作面起始點(diǎn)至直線長(zhǎng)度為30 m的范圍內(nèi)，采煤機(jī)噪音等級(jí)達(dá)到97 dB，使用單個(gè)基站和標(biāo)簽，每3 m設(shè)置一個(gè)記錄點(diǎn)，記錄10次使用卡爾曼濾波前后標(biāo)簽與基站間的測(cè)距誤差數(shù)據(jù)，實(shí)測(cè)方案示意如圖9所示。

圖9 實(shí)測(cè)方案示意Fig. 9 Schematic of measured scheme

測(cè)距結(jié)果如表1所示。其中，ds為實(shí)際值，使用鋼卷尺測(cè)量，dq為卡爾曼濾波改進(jìn)前測(cè)距值，dh為卡爾曼濾波改進(jìn)后測(cè)距值。

表1 卡爾曼濾波改進(jìn)前后測(cè)距值

將表1測(cè)距數(shù)據(jù)導(dǎo)入Matlab進(jìn)行誤差處理分析，所得結(jié)果如圖10所示,其中，橫坐標(biāo)n表示記錄點(diǎn)，縱坐標(biāo)e表示誤差值。在卡爾曼濾波改進(jìn)算法對(duì)測(cè)距數(shù)據(jù)進(jìn)行處理前，測(cè)距誤差波動(dòng)極大且不穩(wěn)定，而卡爾曼濾波算法改進(jìn)后，測(cè)距誤差下降，且由于卡爾曼濾波具有向前預(yù)測(cè)性，使誤差逐漸趨向于0，數(shù)據(jù)更精確。

圖10 卡爾曼濾波前后測(cè)距誤差對(duì)比Fig. 10 Comparison of ranging errors before and after Kalman filtering

卡爾曼濾波算法改進(jìn)前后測(cè)距殘差如圖11所示，其中縱坐標(biāo)s表示殘差值。利用殘差圖判別隨機(jī)誤差，反復(fù)修正卡爾曼系統(tǒng)參數(shù)，在殘差圖結(jié)果趨于理想化的同時(shí)，使卡爾曼濾波算法與TWR改進(jìn)算法結(jié)合性進(jìn)一步提高。

圖11 卡爾曼濾波算法改進(jìn)前后測(cè)距殘差Fig. 11 Range residuals before and after Kalman filtering

結(jié)果表明，總體誤差基本控制在0～10 cm范圍內(nèi)，誤差范圍相較于單一的TWR改進(jìn)算法10～50 cm范圍有更大的提升。

4 結(jié) 論

為解決井下人員定位系統(tǒng)在運(yùn)行過程中存在的測(cè)距誤差波動(dòng)幅度較大，定位輸出數(shù)據(jù)不夠穩(wěn)定，導(dǎo)致定位精度不足的問題，提出了將卡爾曼濾波算法與超寬帶定位算法相結(jié)合，對(duì)各基站測(cè)距數(shù)據(jù)進(jìn)行了卡爾曼濾波處理。

(1) 卡爾曼濾波改進(jìn)算法使定位軌跡與實(shí)際路線相吻合，誤差范圍控制在0～30 cm，定位精度較卡爾曼濾波算法未改進(jìn)時(shí)有較大提升。

(2)卡爾曼濾波改進(jìn)算法使測(cè)距誤差降低至10 cm范圍，較原算法0.5 m誤差有較大提升符合礦井人員定位系統(tǒng)的安全要求規(guī)定。