郭安斌，蘇紅俊，閆孝姮

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

0 引言

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

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

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

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

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

1.1 電鏟定位模型

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

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

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

(1)

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

1.2 電鏟定位流程

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

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

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

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

2.1 滑動(dòng)平均濾波算法

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

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

(2)

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

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

2.2 STFEKF算法

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

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

Xk=Ф(Xk-1)+Wk

(3)

Zk=h(Xk)+Vk

(4)

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

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

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

(5)

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

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

(6)

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

時(shí)變漸消因子的計(jì)算公式為[9]

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

(7)

(8)

(9)

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

STFEKF算法步驟：

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

Sk+1|k=ФSk

(10)

(2) 引入時(shí)變漸消矩陣Lk+1，計(jì)算上述預(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) 計(jì)算濾波增益：

(14)

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

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

(15)

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

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

(16)

式中I為單位矩陣。

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

3 仿真分析

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

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

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

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

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

(17)

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

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

4 結(jié)論

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

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

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

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

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

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