曹 波，王世博，葛世榮， 劉萬(wàn)里，馬光明，梁 耍

(1.中國(guó)礦業(yè)大學(xué) 機(jī)電工程學(xué)院，江蘇 徐州 221116；2.安徽科技學(xué)院 機(jī)械工程學(xué)院，安徽 蚌埠 233033)

0 引 言

煤礦智能開采是國(guó)內(nèi)外煤炭開采領(lǐng)域共同追求的前沿技術(shù)[1-2]。采煤機(jī)是綜采工作面的核心設(shè)備，要實(shí)現(xiàn)智能化開采必須實(shí)現(xiàn)采煤機(jī)智能化，監(jiān)測(cè)采煤機(jī)在工作面的位置是實(shí)現(xiàn)智能化、無(wú)人化開采的關(guān)鍵。針對(duì)采煤機(jī)的定位技術(shù)主要有：紅外線定位[3-4]、無(wú)線傳感網(wǎng)定位[5-6]、里程計(jì)定位[7-8]、激光定位[9]、超聲波定位[10]、慣性導(dǎo)航定位[11-13]。其中慣性導(dǎo)航定位技術(shù)廣泛應(yīng)用于采煤機(jī)定位，已得到煤機(jī)企業(yè)和煤礦普遍認(rèn)可[14]。但是慣性測(cè)量元件隨著時(shí)間和距離的推移存在漂移現(xiàn)象，導(dǎo)致定位誤差不斷累積增加，定位精度逐漸下降，無(wú)法滿足定位要求。因此需要其他定位技術(shù)輔助慣性導(dǎo)航定位，當(dāng)采煤機(jī)即將運(yùn)行到端頭時(shí)，為慣性導(dǎo)航提供采煤機(jī)的位置信息。

UWB(Ultra-Wideband，超寬帶無(wú)線通信技術(shù))具有抗干擾能力強(qiáng)、測(cè)距精度高、抗多徑及穿透性強(qiáng)等優(yōu)點(diǎn)[15]，已經(jīng)被應(yīng)用于采煤機(jī)定位，劉清[16]和王雪松[17]等提出基于超寬帶測(cè)距原理的采煤機(jī)定位系統(tǒng)，并在井下進(jìn)行了測(cè)試。樊啟高[18]提出采用自適應(yīng)模糊卡爾曼濾波技術(shù)進(jìn)行INS/UWB組合導(dǎo)航數(shù)據(jù)融合的方法提升定位精度；方文浩[19]提出基于區(qū)域判定的方法提高測(cè)距精度，采用三角形質(zhì)心算法進(jìn)一步提高定位精度。劉一鳴[20]提出了利用信息過濾算法對(duì)UWB定位結(jié)果進(jìn)行過濾，利用神經(jīng)網(wǎng)絡(luò)算法對(duì)刮板輸送機(jī)機(jī)頭位置每個(gè)時(shí)間段的數(shù)據(jù)進(jìn)行評(píng)估，提高采煤機(jī)定位精度，但是忽略了NLOS環(huán)境的存在；劉萬(wàn)里[21]提出一種基于UWB采煤機(jī)絕對(duì)位置精確校準(zhǔn)的方法，將UWB布置均勻布置在巷道中，由于NLOS的影響，定位精度較低。為減小NLOS誤差的影響，提高定位精度，田野[22]利用UWB測(cè)距建立采煤機(jī)誤差預(yù)測(cè)模型，提高NLOS下采煤機(jī)定位精度。筆者[23]采用高斯混合模型對(duì)測(cè)距值進(jìn)行平滑處理，然后利用交互式多模型濾波分別對(duì)視距環(huán)境(LOS)測(cè)量值和NLOS測(cè)量值進(jìn)行濾波，獲得精度較高的測(cè)距值，采用擴(kuò)展卡爾曼濾波進(jìn)行定位，可獲得較高的定位精度。孫繼平[24]采用卡爾曼濾波消除突發(fā)非視距(NLOS)對(duì)測(cè)量數(shù)據(jù)的影響，在此基礎(chǔ)上提出基于歷史和卡爾曼閾值的最近鄰居指紋定位方法，提高礦井定位的精度，但定位誤差仍然較大，平均定位誤差在1.4 m左右。李旭紅[25]針對(duì)井下NLOS環(huán)境下的定位精度不高的問題，提出以聯(lián)合定位得到的初始坐標(biāo)作為無(wú)跡卡爾曼濾波(UKF)算法的觀測(cè)值，并對(duì)UKF的測(cè)量更新方程進(jìn)行修正，但在實(shí)際工況中，系統(tǒng)的測(cè)量噪聲隨著時(shí)間動(dòng)態(tài)變化，使得的濾波性效果不佳。

通常情況下，假定基站的位置坐標(biāo)是非常準(zhǔn)確的，但是在實(shí)際的作業(yè)環(huán)境中，基站在安裝過程和位置坐標(biāo)確定時(shí)，不可避免存在位置誤差。影響UWB系統(tǒng)定位精度的主要因素是視距誤差、非視距誤差及系統(tǒng)自身的誤差，很多研究者容易忽略基站的位置誤差對(duì)定位精度的影響。基站的位置誤差也是引起定位精度較低的原因之一。盡管上述研究者對(duì)消除非視距誤差進(jìn)行了研究，但是均未考慮到基站位置誤差對(duì)定位精度的影響。尤其是在復(fù)雜的環(huán)境下，基站的位置坐標(biāo)很難準(zhǔn)確的測(cè)量，即使獲得高精度的測(cè)距值，也很難得到較高的定位精度。

針對(duì)上述問題，提出一種校準(zhǔn)方法，在LOS環(huán)境下，通過定位區(qū)域中的參考節(jié)點(diǎn)與基站進(jìn)行測(cè)距，對(duì)基站位置誤差、尺度因子誤差和偏差進(jìn)行補(bǔ)償，達(dá)到校準(zhǔn)的目的；針對(duì)NLOS環(huán)境，考慮到測(cè)量噪聲時(shí)變性，提出采用變分貝葉斯無(wú)跡卡爾曼濾波(VBUKF)對(duì)測(cè)距值進(jìn)行平滑，減小非視距誤差的影響；經(jīng)過校準(zhǔn)和VBUKF平滑處理后，采用Caffery定位算法(CL)求解目標(biāo)節(jié)點(diǎn)的坐標(biāo)，為進(jìn)一步提升定位精度，采用高斯-牛頓迭代算法(GN)對(duì)CL算法的結(jié)果進(jìn)行優(yōu)化。

1 UWB定位系統(tǒng)

為獲得采煤機(jī)的三維坐標(biāo)，有研究者提出將UWB基站安裝在液壓支架頂端和底端[2,16,18]，將目標(biāo)模塊安裝在采煤機(jī)機(jī)身上，通過UWB自組網(wǎng)的形式獲得目標(biāo)模塊與對(duì)應(yīng)基站之間的距離，采用相關(guān)的定位算法解算采煤機(jī)的位置坐標(biāo)。但是液壓支架的頻繁移動(dòng)，導(dǎo)致基站的位置坐標(biāo)不斷發(fā)生改變，很難準(zhǔn)確確定移動(dòng)后基站的坐標(biāo)，進(jìn)而導(dǎo)致采煤機(jī)的定位精度較低。基于此，筆者提出將UWB基站安裝在綜采工作面端頭位置，如圖1所示，當(dāng)采煤機(jī)機(jī)上的目標(biāo)節(jié)點(diǎn)運(yùn)行到距離端頭10 m左右時(shí)，由UWB定位系統(tǒng)獲得采煤機(jī)的三維位置坐標(biāo)，為慣性導(dǎo)航校準(zhǔn)提供基準(zhǔn)。

圖1 UWB基站部署Fig.1 UWB anchor nodes deployment

2 UWB定位方法

2.1 基站校準(zhǔn)補(bǔ)償

在UWB定位系統(tǒng)中，目標(biāo)節(jié)點(diǎn)與基站的真實(shí)距離ri可表示為：

(1)

UWB信號(hào)在傳輸過程中受到測(cè)距系統(tǒng)本身的誤差及非視距的影響，導(dǎo)致測(cè)量距離di往往大于真實(shí)距離，其數(shù)學(xué)表達(dá)式為：

di=(1+αi)ri+Bi+Ni+wi

(2)

其中：αi為尺度因子誤差；Bi為偏差；Ni為非視距誤差；wi為高斯白噪聲。

系統(tǒng)誤差可以通過適當(dāng)?shù)男?zhǔn)技術(shù)來(lái)進(jìn)行補(bǔ)償。 如果定位系統(tǒng)處于LOS環(huán)境下，則偏差和尺度因子誤差可以得到補(bǔ)償，通常情況下，假設(shè)基站的位置坐標(biāo)是可以準(zhǔn)確測(cè)量的。然而，在復(fù)雜環(huán)境下，在基站安裝過程中和對(duì)基站坐標(biāo)進(jìn)行確定時(shí)存在誤差，導(dǎo)致基站的坐標(biāo)很難精準(zhǔn)確定。因此，將式(2)改寫為：

(3)

為了對(duì)基站進(jìn)行位置校準(zhǔn)，需要忽略非視距誤差，在測(cè)量區(qū)域內(nèi)選擇多個(gè)參考節(jié)點(diǎn)，必須保證基站與參考節(jié)點(diǎn)處于LOS環(huán)境中，同時(shí)必須保證參考節(jié)點(diǎn)的位置坐標(biāo)盡可能準(zhǔn)確，則參考節(jié)點(diǎn)與基站之間距離di-j可表示為：

(4)

從式(4)可以看到，尺度因子誤差、偏差和位置誤差這3個(gè)誤差參數(shù)相互交織在一起。由于采用相同的UWB，每個(gè)基站在LOS環(huán)境下偏差可以認(rèn)為恒定不變，因此，用第j個(gè)測(cè)距方程減去第C個(gè)方程，消除偏差的影響，則有：

(5)

(6)

其中：

為方便計(jì)算，將上式簡(jiǎn)化為：

(7)

將式(7)轉(zhuǎn)換成矩陣形式：

Ui=GiXi+W

(8)

根據(jù)最小二乘法原理，第i個(gè)基站的位置誤差和尺度因子誤差為：

(9)

根據(jù)基站位置誤差，對(duì)每個(gè)基站進(jìn)行校準(zhǔn)補(bǔ)償，可得校準(zhǔn)后基站的位置坐標(biāo)為：

(10)

為求得偏差Bi,令

(11)

H=[1, 1,..., 1]T

(12)

根據(jù)式(11)和式(12)，將式(4)轉(zhuǎn)化為矩陣形式，如下：

Ri=HBi+W

(13)

由式(13)，可計(jì)算出偏差的估計(jì)值，即：

(14)

2.2 變分貝葉斯無(wú)跡卡爾曼濾波

根據(jù)上述方法，可以對(duì)每個(gè)基站的尺度因子誤差、偏差及基站的位置誤差進(jìn)行補(bǔ)償校準(zhǔn)。對(duì)于確定的空間定位區(qū)域，每個(gè)基站被校準(zhǔn)后，尺度因子誤差和基站的位置誤差是固定不變的。但是由于非視距的存在，偏差Bi和非視距誤差Ni很難分開，非視距誤差與定位空間的環(huán)境有密切的關(guān)系，存在不確定性和多變性，需要實(shí)時(shí)估計(jì)測(cè)距誤差，因此采用VBUKF對(duì)測(cè)距值進(jìn)行濾波，減小非視距誤差的影響。

在k時(shí)刻，建立距離的空間狀態(tài)向量模型為：

(15)

空間狀態(tài)方程和觀測(cè)方程為定義為：

Dm,k=FDm,k-1+Bwk-1

(16)

Zm,k=ADm,k+vk

(17)

(18)

(19)

(20)

其中，0<β≤ 1，0 < ||B|| < 1。

(21)

VB近似的思想是通過最小化后驗(yàn)近似分布與真實(shí)分布之間的Kullback-Leibler(KL)散度值[26]，可得相關(guān)參數(shù)最優(yōu)估計(jì)，其散度定義為：

(22)

通過對(duì)式(22)散度值最小化，可得如下近似：

(23)

(24)

(25)

VBUKF的基本步驟如下：

1)協(xié)方差矩陣經(jīng)過SVD分解后，如下：

(26)

2)計(jì)算2n+1個(gè)Sigma 點(diǎn)及相應(yīng)的權(quán)重：

(27)

其中，λ=α2(n+δ)-n,λ和δ為尺度參數(shù)；α為一個(gè)較小的整數(shù)。

3)根據(jù)狀態(tài)方程計(jì)算預(yù)測(cè)狀態(tài)采樣點(diǎn)：

(28)

(29)

(30)

量測(cè)噪聲方差矩陣參數(shù)預(yù)測(cè)為：

基于HIS的住院擺藥單優(yōu)化與擺藥差錯(cuò)預(yù)警系統(tǒng)的設(shè)計(jì)與應(yīng)用 …………………………………………… 溫正旺等（19）：2713

(31)

(32)

5)量測(cè)更新

(33)

(34)

(35)

(36)

(37)

(38)

(39)

(40)

(41)

(42)

經(jīng)過VBUKF濾波后，可獲得濾波后的距離：

(43)

2.3 定位算法

當(dāng)測(cè)距值經(jīng)過VBUKF平滑后，結(jié)合校準(zhǔn)補(bǔ)償后的基站位置坐標(biāo)和CL算法求解目標(biāo)節(jié)點(diǎn)的估計(jì)位置，4個(gè)UWB基站組成的定位系統(tǒng)觀測(cè)方程組為：

(44)

將式(44)轉(zhuǎn)換成矩陣的形式：

CX=M

(45)

其中:

目標(biāo)節(jié)點(diǎn)的位置坐標(biāo)為：

X0=(CTC)-1CTM

(46)

X=(HTH)-1HTL+X0

(47)

2.4 定位性能評(píng)價(jià)

為了評(píng)價(jià)UWB系統(tǒng)的定位性能，選擇均方根誤差(RMSE)作為定位精度的評(píng)價(jià)指標(biāo)，計(jì)算公式為：

(48)

其中，(xr,yr,zr)為目標(biāo)節(jié)點(diǎn)的真實(shí)坐標(biāo)；(xt,yt,zt)為定位算法所求坐標(biāo)；為全面分析誤差的變化，x,y,z軸三個(gè)方向上的誤差計(jì)算公式為：

(49)

3 試驗(yàn)結(jié)果與分析

為驗(yàn)證所提的校準(zhǔn)方法和VBUKF的有效性，采用美國(guó)Time Domain公司生產(chǎn)的UWB P440模塊作為基站和目標(biāo)模塊。4個(gè)基站固定不動(dòng)，并將基站群在水平面的投影設(shè)置為V型布局，這種布局能獲得好的定位精度[29]。為很好地模擬井下UWB定位系統(tǒng)對(duì)采煤機(jī)定位，并考慮非視距環(huán)境的存在，將一個(gè)基站設(shè)置在NLOS條件下，分別進(jìn)行靜態(tài)定位和動(dòng)態(tài)定位試驗(yàn)，圖2給出了基站部署及運(yùn)動(dòng)軌跡的平面圖，圖3為靜態(tài)試驗(yàn)及動(dòng)態(tài)試驗(yàn)的真實(shí)實(shí)驗(yàn)場(chǎng)景，4個(gè)基站的坐標(biāo)分別為AN1 (3.636,6.062,0.478),AN2 (5.983,4.796,1.097),AN3 (3.598,6.033,2.179),AN4 (1.208,4.799,1.440)。為突出所提出方法的優(yōu)越性，分別對(duì)CL算法和CL-GN算法校準(zhǔn)平滑前后的定位精度進(jìn)行分析。

圖2 基站布局及運(yùn)動(dòng)軌跡Fig.2 Anchor nodes deployment and movement tracks

圖3 現(xiàn)場(chǎng)試驗(yàn)場(chǎng)景Fig.3 Realistic experiment scenario

根據(jù)設(shè)置的基站坐標(biāo)，在定位區(qū)域內(nèi)選擇9個(gè)參考節(jié)點(diǎn)，并且確保這些參考節(jié)點(diǎn)與每個(gè)基站處于LOS環(huán)境下，9個(gè)點(diǎn)的位置坐標(biāo)盡可能的準(zhǔn)確，根據(jù)上述基站誤差補(bǔ)償原理，可求出校準(zhǔn)后4個(gè)基站的坐標(biāo)，尺度因子誤差和偏差，其求解參數(shù)見表1。

表1 校準(zhǔn)后基站坐標(biāo)、尺度因子誤差和偏差Table 1 Coordinates,scale factor error and bias of anchor nodes after calibration

為驗(yàn)證NLOS環(huán)境下，VBUKF方法能夠消除非視距誤差，圖4給出了AN2在第一個(gè)定位點(diǎn)的原始測(cè)距數(shù)據(jù)與VBUKF處理后的結(jié)果，從圖中可以看到，原始測(cè)量數(shù)據(jù)波動(dòng)較大，經(jīng)過VBUKF平滑后，測(cè)距數(shù)據(jù)波動(dòng)幅度明顯減小，證明了VBUKF能提高距離精度和改善數(shù)據(jù)的穩(wěn)定性，進(jìn)而提升UWB系統(tǒng)的定位精度和可靠性。

圖4 UWB測(cè)距信息Fig.4 UWB ranging information

在靜態(tài)條件下，設(shè)置19個(gè)定位點(diǎn)，定位點(diǎn)的軌跡如圖2所示，相鄰2個(gè)定位點(diǎn)之間的距離為0.15 m,對(duì)每個(gè)點(diǎn)進(jìn)行300次測(cè)距，根據(jù)4個(gè)基站所測(cè)得的距離，分別利用尺度因子誤差α和偏差B對(duì)LOS條件下的測(cè)距進(jìn)行重構(gòu)，對(duì)NLOS條件下的測(cè)距值采用VBUKF平滑對(duì)距離濾波處理，利用CL算法和CL-GN算法分別計(jì)算校準(zhǔn)前后目標(biāo)節(jié)點(diǎn)的位置坐標(biāo)，其定位結(jié)果如圖5所示。從圖中可以看到，無(wú)論是校準(zhǔn)平滑前還是校準(zhǔn)平滑后，GN算法都能夠有效的減少誤差；經(jīng)過校準(zhǔn)補(bǔ)償和VBUKF平滑后，定位精度顯著提升，而且CL-GN算法表現(xiàn)出較好的定位性能，定位精度在0.06～0.22 m，明顯優(yōu)于CL算法。圖6給出了最大誤差、最小誤差、平均誤差及誤差標(biāo)準(zhǔn)差，經(jīng)過校準(zhǔn)補(bǔ)償和濾波后，CL算法的平均定位精度提高了31.1%，CL-GN算法的定位精度提升了56.4%，而且CL-GN算法定位誤差的標(biāo)準(zhǔn)差最小，僅為0.059 m，有利于UWB系統(tǒng)獲得穩(wěn)定的定精度。

圖5 校準(zhǔn)平滑前后定位誤差對(duì)比Fig.5 Comparison the positioning error before and after calibration and smoothing

圖6 定位誤差特征統(tǒng)計(jì)Fig.6 Characteristic statistics of positioning errors

為進(jìn)一步分析CL算法和CL-GN 算法校準(zhǔn)前后誤差在3個(gè)坐標(biāo)軸上的變化情況，圖7給出了誤差在X,Y,Z3個(gè)坐標(biāo)軸上的分布情況。從圖7可以看出，無(wú)論校準(zhǔn)前后，GN算法對(duì)3個(gè)坐標(biāo)軸的定位精度有著很大的貢獻(xiàn)，能夠提高定位精度；Y軸誤差明顯大于X軸和Z軸誤差，說(shuō)明Y軸誤差是引起定位精度較低的主要原因。未校準(zhǔn)前，X軸能夠獲得較高的定位精度，Y軸和Z軸定位性能較差。經(jīng)過校準(zhǔn)平滑，CL-GN算法的Y軸和Z軸誤差大大減小，定位精度大幅度提升，X軸的某些定位點(diǎn)定位精度變化不明顯，但是滿足實(shí)驗(yàn)室定位精度要求。在實(shí)際綜采工況中，UWB定位系統(tǒng)可能會(huì)受到粉塵、水霧及采煤機(jī)運(yùn)行時(shí)振動(dòng)的影響，這些因素對(duì)定位精度產(chǎn)生怎么樣的影響，有待進(jìn)一步深入研究。

圖7 校準(zhǔn)平滑前后誤差在三個(gè)坐標(biāo)軸上的分布Fig.7 Errors distribution on three axes before and after calibration and smoothing

為更好地證明所提校準(zhǔn)補(bǔ)償和VBUKF平滑方法的適用性和有效性，本文進(jìn)行動(dòng)態(tài)試驗(yàn)，根據(jù)測(cè)距結(jié)果和CL-GN算法，分別計(jì)算出校準(zhǔn)平滑前后的定位結(jié)果。從圖8種可以看到，經(jīng)過校準(zhǔn)平滑后，目標(biāo)節(jié)點(diǎn)在3個(gè)坐標(biāo)軸上的波動(dòng)程度有所較小，運(yùn)動(dòng)軌跡更加接近實(shí)際軌跡，證明了本文所提的校準(zhǔn)方法能夠有效減小定位誤差，提升動(dòng)態(tài)定位精度。在實(shí)際應(yīng)用中，安裝在機(jī)身上的目標(biāo)節(jié)點(diǎn)可能會(huì)受到采煤機(jī)震動(dòng)的影響，需要將目標(biāo)節(jié)點(diǎn)安裝在振動(dòng)較薄弱的地方，振動(dòng)、粉塵、水霧對(duì)UWB系統(tǒng)定位精度的影響需要進(jìn)一步的研究。

圖8 校準(zhǔn)前后動(dòng)態(tài)定位結(jié)果軌跡曲線Fig.8 Trajectory curve of dynamic positioning result before and after calibration

4 結(jié) 論

1)為提高UWB系統(tǒng)的定位精度，考慮基站位置誤差、尺度因子誤差和偏差對(duì)定位精度的影響，提出對(duì)基站的位置誤差進(jìn)行校準(zhǔn)補(bǔ)償，根據(jù)校準(zhǔn)補(bǔ)償原理，可以獲得校準(zhǔn)后基站的坐標(biāo)。

2)采用VBUKF對(duì)NLOS測(cè)距信息進(jìn)行濾波處理，減小NLOS誤差的影響，提高距離估計(jì)精度；在校準(zhǔn)補(bǔ)償和VBCKF平滑基礎(chǔ)上，采用 CL算法求得目標(biāo)節(jié)點(diǎn)的坐標(biāo)，利用GN算法對(duì)CL算法的結(jié)果進(jìn)行優(yōu)化。

3)試驗(yàn)結(jié)果表明，經(jīng)過校準(zhǔn)補(bǔ)償和VBUKF平滑后，CL和CL-GN算法的平均定位誤差分別由0.347、0.250 m減小為0.239、0.109 m,平均定位精度分別提高了31.1%，56.4%；同時(shí)動(dòng)態(tài)運(yùn)動(dòng)軌跡更加接近實(shí)際軌跡，表明本文的方法能有效地提升UWB系統(tǒng)定位精度，是一種可行的方法，能夠?yàn)榫露ㄎ患夹g(shù)提供理論參考。