基于UWB技術(shù)的礦山電鏟定位算法研究

郭安斌，蘇紅俊，閆孝姮

(1.中國神華能源股份有限公司 哈爾烏素露天煤礦， 內(nèi)蒙古 鄂爾多斯 010300；2.遼寧工程技術(shù)大學(xué) 電氣與控制工程學(xué)院， 遼寧 葫蘆島 125105)

0 引言

礦山工作環(huán)境惡劣，傳統(tǒng)的電纜卷放方法無法保證電鏟長時間供電，造成電鏟工作效率低下，且供電過程中存在安全隱患。跟隨電鏟移動的新型電纜卷放車可解決上述問題。要實現(xiàn)電纜卷放車對電鏟的自主跟隨，電鏟的定位與跟蹤非常必要，定位是電纜卷放車完成路徑規(guī)劃、自主導(dǎo)航等復(fù)雜任務(wù)的前提[1]。傳統(tǒng)的無線定位技術(shù)包括全球定位系統(tǒng)(GPS)、WiFi、射頻識別(RFID)等，這些技術(shù)由于自身局限性，無法實現(xiàn)對電鏟的高精度定位，不適合電鏟設(shè)備的大規(guī)模裝備與長期化應(yīng)用。超寬帶(UWB)技術(shù)采用納秒級的非正弦波窄脈沖技術(shù)實現(xiàn)定位，具有定位精度高、抗噪聲能力強、多徑分辨率高與穿透力強的特點，在礦山環(huán)境中應(yīng)用具有很大優(yōu)勢[2-4]。此外，UWB的無線通信空間容量能很好地滿足電鏟定位需求[5]。文獻[6-7]驗證了UWB技術(shù)適用于礦山環(huán)境中大型機械的定位。

目前，常用的UWB定位算法包括到達時間(TOA)、飛行時間(TOF)、到達時間差(TDOA)、到達角(AOA)等，其中TDOA算法與TOF算法在工業(yè)應(yīng)用中使用較多。相較于TOF 算法，TDOA算法中定位標(biāo)簽只需要發(fā)射或接收1次UWB信號，縮短了工作時長，降低了功耗，提高了定位容量，且可實現(xiàn)動態(tài)定位，更適用于礦山工作環(huán)境。因此，本文采用TDOA算法實現(xiàn)電鏟定位。

針對基于TDOA算法的運動目標(biāo)定位問題，張麗[8]提出了一種基于TDOA的擴展卡爾曼濾波(EKF)算法，并將其應(yīng)用于UWB定位系統(tǒng)中，該算法具有良好的定位能力，實現(xiàn)了高精度的目標(biāo)運動軌跡跟蹤，但當(dāng)TDOA測量誤差較大時，該算法定位精度會降低且定位結(jié)果容易發(fā)散。當(dāng)定位系統(tǒng)處于穩(wěn)定狀態(tài)時，EKF算法的預(yù)測誤差協(xié)方差趨于最小，但是當(dāng)目標(biāo)運動軌跡突變時，EKF算法會失去對目標(biāo)的跟蹤能力。吳濤等[9]提出的強跟蹤擴展卡爾曼濾波(STFEKF)算法解決了上述問題，可明顯減小定位誤差，增強對移動物體的跟蹤定位能力。

本文將STFEKF算法應(yīng)用到礦山環(huán)境中，結(jié)合滑動平均濾波算法，實現(xiàn)目標(biāo)電鏟的定位與跟蹤，使電纜卷放車能夠跟隨電鏟位置移動，提高電纜卷放工作的效率及安全性，延長電鏟可工作時間，降低成本及工人勞動強度。

1 基于UWB技術(shù)的礦山電鏟定位原理

1.1 電鏟定位模型

TDOA算法的基本思想：測量不同基站發(fā)送到目標(biāo)點脈沖信號之間的時差，計算距離差，根據(jù)基站之間的距離差計算目標(biāo)點位置[10]。在電鏟跟蹤定位系統(tǒng)中，2個TDOA數(shù)據(jù)值可構(gòu)成一組關(guān)于目標(biāo)電鏟位置的雙曲線方程組，求解該方程組可得到需要定位的電鏟位置。

根據(jù)基站的設(shè)定位置建立坐標(biāo)系xoy,基于UWB技術(shù)的礦山電鏟定位模型如圖1所示， 3個UWB基站安裝在電纜卷放車上，定位標(biāo)簽安裝在要定位的電鏟上。

圖1 基于UWB技術(shù)的礦山電鏟定位模型Fig.1 Mine shovel positioning model based on UWB technology

(1)

求解式(1)即可得到電鏟位置坐標(biāo)(x,y)。

1.2 電鏟定位流程

在實際的UWB定位系統(tǒng)中，由于基站本身的內(nèi)部原因，會產(chǎn)生測量噪聲，對定位精度和穩(wěn)定性有一定影響。滑動平均濾波算法實現(xiàn)簡單、能實時且快速地處理不平穩(wěn)數(shù)據(jù)[11]，減小定位數(shù)據(jù)誤差波動，提高定位精度。因此，本文采用滑動平均濾波算法對距離測量噪聲進行濾波。為了提高電鏟定位精度，在計算出標(biāo)簽位置坐標(biāo)后，利用STFEKF算法進一步去噪。

基于UWB 技術(shù)的礦山電鏟定位流程如圖2所示。建立二維定位坐標(biāo)系，根據(jù)TDOA測距算法，測量各基站到目標(biāo)電鏟位置的距離并計算差值；對獲取的距離差信息進行滑動平均濾波，以抑制測距過程中產(chǎn)生的噪聲，平穩(wěn)數(shù)據(jù)；根據(jù)濾波修正后的距離差計算出標(biāo)簽位置；用STFEKF算法跟蹤目標(biāo)電鏟位置，進一步消除噪聲，提高在運動過程中目標(biāo)電鏟的定位精度，得到平滑的運動軌跡。

圖2 基于UWB 技術(shù)的礦山電鏟定位流程Fig.2 Mine shovel positioning process based on UWB technology

2 電鏟定位算法實現(xiàn)

2.1 滑動平均濾波算法

在礦山環(huán)境中，要求電纜卷放車與電鏟的距離保持在25 m左右，當(dāng)卷放車與電鏟之間的距離增加或電鏟運動狀態(tài)發(fā)生突變時，電鏟的定位誤差會增大。滑動平均濾波可有效減小測距誤差，使獲得的距離差數(shù)據(jù)波動減小。

設(shè)定一個固定長度為N的滑動窗口，在測得的距離差數(shù)據(jù)(TDOA值)隊列上移動滑動窗口，距離差數(shù)據(jù)隨著窗口移動不斷更新。每移動一個單位長度，數(shù)據(jù)隊列前端將輸入新的距離差數(shù)據(jù)，并刪除隊列最后的一位數(shù)據(jù)，使窗口內(nèi)始終有N個數(shù)據(jù)。取每個滑動窗口內(nèi)N個數(shù)據(jù)的平均值組成新的隊列，完成濾波。濾波后的輸出數(shù)據(jù)為

(2)

式中：n為輸出數(shù)據(jù)的序號；p為輸入數(shù)據(jù)。

滑動平均濾波算法的效果取決于滑動窗口的大小，可根據(jù)實際濾波要求合理選擇。

2.2 STFEKF算法

EKF算法采用前一時刻的估計值作為參考點做線性化，其線性化模型更可靠?；赥DOA的EKF算法的狀態(tài)方程是線性的[12]，觀測方程是非線性的。EKF算法將非線性函數(shù)以泰勒級數(shù)展開的方式，保留一階項，實現(xiàn)對非線性函數(shù)的線性化，用Jacoby矩陣代替卡爾曼濾波方程中的狀態(tài)轉(zhuǎn)移矩陣，以此來解決非線性問題。在電鏟定位系統(tǒng)中，觀測方程為非線性，因此，EKF算法適用于對目標(biāo)電鏟進行定位跟蹤。

在實際的定位過程中，不可避免地存在測量誤差，因此有必要通過實時觀測系統(tǒng)的輸入輸出數(shù)據(jù)，實現(xiàn)對定位數(shù)據(jù)的濾波優(yōu)化。由文獻[13]可知，EKF應(yīng)用的非線性系統(tǒng)數(shù)學(xué)模型為

Xk=Ф(Xk-1)+Wk

(3)

Zk=h(Xk)+Vk

(4)

式中：Xk為電鏟在k時刻的狀態(tài)向量，Xk=[x,y,vx,vy]，vx，vy為k時刻電鏟在x軸和y軸的速度；Φ為狀態(tài)轉(zhuǎn)移矩陣；Wk，Vk為噪聲向量，且互不相關(guān)；Zk為k時刻的觀測值，即距離差信息；h(Xk)為量測函數(shù)，h(Xk)=(d21,d31)。

目標(biāo)電鏟的定位精度要求高，且在運動過程中會有運動狀態(tài)突然改變的情況，因此，采用STFEKF算法來提高定位的精度。當(dāng)目標(biāo)電鏟運動狀態(tài)突然發(fā)生變化時，輸出殘差γ(k)就會增大，STFEKF算法通過文獻[14]中的次優(yōu)方法得到時變漸消因子λ(k+1)，通過λ(k+1)來調(diào)節(jié)預(yù)測協(xié)方差矩陣Pk+1|k，進而調(diào)節(jié)增益矩陣Kk+1，得到電鏟的最優(yōu)狀態(tài)估計值Sk+1。

令殘差γ(k)=Zk-h(Sk+1|k)，Sk+1|k為狀態(tài)預(yù)測值，則實際輸出殘差序列的協(xié)方差矩陣為

(5)

式中ρ為遺忘因子，0<ρ≤1，一般取0.95～0.98。

令Hk為k+1時刻h(Sk+1|k)在Sk+1|k處的Jacoby矩陣[15]，即觀測矩陣，對h(Sk+1|k)進行Taylor級數(shù)展開，得

(6)

式中h1(Sk)，h2(Sk)分別為目標(biāo)到第2、第3個基站的距離與到基準(zhǔn)基站的距離差值。

時變漸消因子的計算公式為[9]

λ(k+1)=tr(N(k+1))/tr(M(k+1))

(7)

(8)

(9)

式中：tr()表示求矩陣的跡;β為弱化因子,β≥1；Pk為估計誤差協(xié)方差矩陣；Qk，R為分別噪聲向量Wk，Vk的協(xié)方差矩陣。

STFEKF算法步驟：

(1) 首先由狀態(tài)方程得到定位預(yù)測值，根據(jù)上一時刻最優(yōu)估計值Sk與狀態(tài)轉(zhuǎn)移矩陣等得到k+1時刻的狀態(tài)預(yù)測值Sk+1|k：

Sk+1|k=ФSk

(10)

(2) 引入時變漸消矩陣Lk+1，計算上述預(yù)測值的預(yù)測協(xié)方差矩陣Pk+1|k：

Pk+1|k=Lk+1ФPkФT

(11)

Lk+1=diag[λ1(k+1),λ2(k+1),…,λ4(k+1)]

(12)

(13)

式中j=1～4。

(3) 計算濾波增益：

(14)

(4) 得到濾波輸出值，即輸出定位結(jié)果：

Sk+1=Sk+1|k+Kk+1(Zk-h(Sk+1|k))

(15)

(5) 更新估計誤差協(xié)方差矩陣：

Pk+1=(I-Kk+1Hk)Pk+1|k

(16)

式中I為單位矩陣。

電鏟工作過程中會由于某些原因致其運動狀態(tài)發(fā)生改變，STFEKF算法可增強對目標(biāo)電鏟跟蹤定位的能力，從而保證電纜卷放車依據(jù)電鏟位置準(zhǔn)確移動。

3 仿真分析

為了驗證基于UWB技術(shù)的礦山電鏟定位算法的性能，采用Matlab軟件進行仿真分析。電鏟的實際移動速度約為3 km/h，假設(shè)目標(biāo)電鏟作勻速直線運動，采用3個觀測基站，基站位置分別為S1(0,0)，S2(3 m,0)，S3(0,5 m)，以S1為基準(zhǔn)基站。設(shè)采樣周期為1 s，滑動平均濾波數(shù)據(jù)長度為5，初始航向角為60°，速度vx=0.42 m/s，vy=0.72 m/s；運行20 s后，目標(biāo)轉(zhuǎn)向，航向角變?yōu)?，速度約為0.81 m/s，運動時間為30 s；采用本文算法和傳統(tǒng)EKF算法對目標(biāo)的運動軌跡進行追蹤。

當(dāng)R為0.000 2的純量矩陣(矩陣對角線元素均為0.000 2，其余元素為0)時，目標(biāo)跟蹤曲線如圖3(a)所示。將R增大為0.000 5的純量矩陣，目標(biāo)跟蹤曲線如圖3(b)所示。從圖3 可看出，在不同觀測噪聲的影響下，用本文算法得到的定位軌跡更接近于目標(biāo)的真實移動軌跡。

(a) R為0.000 2的純量矩陣

(b) R為0.000 5的純量矩陣

目標(biāo)定位誤差計算公式為

(17)

定位誤差對比如圖4所示。從圖4可看出，隨著電鏟運動狀態(tài)的改變及電鏟與電纜卷放車之間距離的增加，本文算法的定位誤差小于傳統(tǒng)EKF算法。

不同觀測噪聲狀況下2種算法的定位均方差對比見表1，其中E1，E2分別表示R為0.000 2，0.000 5的純量矩陣時的定位均方差。從表1可看出，在不同的噪聲影響下，本文算法定位均方差均不到傳統(tǒng)EKF算法的30%，符合電鏟定位應(yīng)用要求。

4 結(jié)論

(1) 提出一種基于UWB技術(shù)的礦山電鏟定位算法，采用TDOA算法實現(xiàn)電鏟定位，建立了定位模型。

(a) R為0.000 2的純量矩陣

(b) R為0.000 5的純量矩陣

表1 電鏟定位均方差對比Table 1 Comparison of mean square deviation of electric shovel positioning

(2) 由于在定位過程中會產(chǎn)生定位誤差，且電鏟移動軌跡會發(fā)生突變，采用滑動平均濾波算法對距離測量噪聲進行濾波，并在計算出標(biāo)簽位置坐標(biāo)后，利用STFEKF算法進一步去噪，提高了電鏟定位精度。

(3) 仿真結(jié)果表明，在不同觀測噪聲的影響下，滑動平均濾波+STFEKF的定位方案誤差小于傳統(tǒng)EKF算法，有效解決了距離增加或電鏟運動狀態(tài)突變時定位誤差增大的問題；定位均方差較傳統(tǒng)EKF算法降低70%以上，定位軌跡更接近于目標(biāo)的真實移動軌跡，具有良好的定位跟蹤及噪聲抑制能力。