基于自適應(yīng)CKF 的改進(jìn)LANDMARC 井下定位算法研究

苗作華 陳澳光 朱良建 趙成誠 劉代文

(1.武漢科技大學(xué)資源與環(huán)境工程學(xué)院,湖北 武漢 430081;2.冶金礦產(chǎn)資源高效利用與造塊湖北省重點(diǎn)實(shí)驗(yàn)室,湖北 武漢 430081)

2022 年我國主要礦山總產(chǎn)量超50 億t,有超300萬名礦工在井下惡劣、危險(xiǎn)環(huán)境下從事開采作業(yè)。 但是,井下巷道為密閉空間,井下人員的位置信息無法依靠衛(wèi)星導(dǎo)航信號輔助獲取[1]。 隨著礦產(chǎn)資源開采規(guī)模和深度的不斷增大,生產(chǎn)作業(yè)和開采難度不斷加大,因此精準(zhǔn)實(shí)時(shí)地掌握井下人員的位置狀況顯得十分重要[2]。 在井下定位的多種解決方案中,基于無線射頻識別 ( Radio Frequency Identification,RFID)[3-4]的定位技術(shù),因其抗干擾性強(qiáng)、時(shí)間短、精度高,不受視距通信問題困擾的特點(diǎn)[5],在礦山井下定位中得到了十分廣泛的應(yīng)用。

LANDMARC[6]、SpotON[7]等都是基于RFID 定位技術(shù)的研究成果,LANDMARC(Location Identification Based on Dynamic Active RFID Calibration)定位算法是通過實(shí)時(shí)獲取的參考標(biāo)簽的信號接收強(qiáng)度(Received Signal Strength Indicator,RSSI)[8]作為參考,來計(jì)算其與閱讀器之間的距離從而達(dá)到推算出待測標(biāo)簽位置的效果。 然而隨著井下巷道距離增長及岔路增多,信號的多徑效應(yīng)十分嚴(yán)重,導(dǎo)致獲取到的RSSI值準(zhǔn)確度不高,使得多數(shù)基于LANDMARC 的定位算法的精準(zhǔn)度不高。 近年來,濾波技術(shù)在提高射頻定位精度上有著廣泛的應(yīng)用。 曹春萍等[9]將卡爾曼濾波(Kalman Filtering,KF)應(yīng)用于射頻定位中,利用待測目標(biāo)最新的位置測量值進(jìn)行位置迭代,得到一個(gè)關(guān)于目標(biāo)位置較準(zhǔn)確的預(yù)估值,極大地提高了射頻定位精度,但卡爾曼濾波在環(huán)境噪聲不確定性的情況下,其魯棒性較低,且當(dāng)實(shí)際測量模型處于非線性狀態(tài)時(shí),定位性能會(huì)急劇下降。 王鵬飛等[10]引入H∞濾波(H-Infinity Filter,HIF)用于定位結(jié)果優(yōu)化,該方法無需進(jìn)行有關(guān)噪聲統(tǒng)計(jì)特性的假設(shè),既不使用自由加權(quán)矩陣,也不需要任何模型進(jìn)行轉(zhuǎn)換,可以有效減少算法計(jì)算量,并大大提高了定位系統(tǒng)的魯棒性,但H∞濾波不能保證定位結(jié)果均方根誤差最小,算法精確度不夠高。 朱晨迪等[11]引入粒子濾波(Particle Filter,PF)對待測目標(biāo)位置進(jìn)行最優(yōu)值估計(jì),很好地解決了非線性系統(tǒng)的位置預(yù)估問題,但是對于連續(xù)動(dòng)態(tài)空間的樣本更新過程仍需更新全部粒子及其權(quán)值,計(jì)算的冗余度與粒子數(shù)呈正相關(guān),導(dǎo)致算法計(jì)算量較大。

為此,基于RFID 的LANDMARC 定位算法急需克服井下非線性環(huán)境干擾所造成的定位精度不高、自適應(yīng)能力差以及算法收斂速度慢的問題。 趙德康等[12]研究表明:BP 神經(jīng)網(wǎng)絡(luò)具備很強(qiáng)的非線性映射能力和自適應(yīng)性,針對井下環(huán)境能夠有效提高定位算法的收斂速度及學(xué)習(xí)能力,使定位系統(tǒng)性能達(dá)到最優(yōu)。 高嵩等[13]研究表明:容積卡爾曼濾波(CKF)具有在強(qiáng)非線性系統(tǒng)中保持穩(wěn)定的特點(diǎn),而且對于高維的狀態(tài)空間定位,其濾波精度與粒子濾波[14]最為接近且計(jì)算量較小,是解決井下實(shí)時(shí)狀態(tài)估計(jì)問題強(qiáng)有力的工具。 目前,應(yīng)用神經(jīng)網(wǎng)絡(luò)方法對井下定位算法的改進(jìn)研究較薄弱,因此本研究提出基于自適應(yīng)CKF 的改進(jìn)LANDMARC 井下定位算法,充分發(fā)揮了BP 神經(jīng)網(wǎng)絡(luò)的泛化映射能力和CKF 濾波強(qiáng)穩(wěn)定性的特點(diǎn),極大提高了井下LANDMARC 算法的定位精度、自適應(yīng)能力以及定位系統(tǒng)的穩(wěn)定性。

1 動(dòng)態(tài)LANDMARC 定位

LANDMARC 定位算法本質(zhì)上是一種基于待測目標(biāo)信號接收強(qiáng)度RSSI 的質(zhì)心權(quán)重算法[15]。 本研究結(jié)合LANDMARC 定位算法抽象出井下三維空間動(dòng)態(tài)模型。 LANDMARC 定位系統(tǒng)由參考標(biāo)簽、目標(biāo)標(biāo)簽、閱讀器構(gòu)成,其布局如圖1 所示。

圖1 LANDMARC 定位系統(tǒng)Fig.1 LANDMARC positioning system

傳統(tǒng)的LANDMARC 算法是對靜止目標(biāo)進(jìn)行定位,而井下待測目標(biāo)位置在不斷變化,對于移動(dòng)目標(biāo)則需建立動(dòng)態(tài)模型[16]。 在礦山井下高斯分布是應(yīng)用最廣泛的分布模型,因此本研究重點(diǎn)針對高斯域非線性的狀態(tài)估計(jì)問題進(jìn)行分析,將三維狀態(tài)[17]空間系統(tǒng)中的待測目標(biāo)在k時(shí)刻的位置狀態(tài)向量定義為Xk,則LANDMARC 定位系統(tǒng)下的待測目標(biāo)狀態(tài)轉(zhuǎn)移方程可表示為

式中,f(·) 為系統(tǒng)非線性轉(zhuǎn)移函數(shù);wk為系統(tǒng)狀態(tài)轉(zhuǎn)移噪聲。 同時(shí),LANDMARC 定位系統(tǒng)下的待測目標(biāo)觀測方程可表示為

式中,zk為目標(biāo)標(biāo)簽k時(shí)刻的觀測向量,由k時(shí)刻每個(gè)閱讀器所接收的信號強(qiáng)度值組成;h(·) 是k時(shí)刻定位系統(tǒng)的觀測函數(shù);X︿k為LANDMARC 定位系統(tǒng)第k時(shí)刻的待測目標(biāo)位置狀態(tài)向量預(yù)估值,作為觀測值;vk為觀測噪聲,由障礙物對信號傳播的遮蔽特性決定;wk和vk均為高斯白噪聲且不相關(guān)。

利用式(2)含有噪聲的觀測函數(shù)可以預(yù)測待測目標(biāo)位置狀態(tài)向量值Xk,聯(lián)立式(1)與式(2)可得到移動(dòng)目標(biāo)井下動(dòng)態(tài)定位模型[18]。

2 LANDMARC 動(dòng)態(tài)模型求解

2.1 容積卡爾曼濾波原理

現(xiàn)將式(1)、式(2)模型進(jìn)行概率化,則對高斯域非線性系統(tǒng)模型的狀態(tài)估計(jì)[19]的一般形式為

式中,Pk|k為k時(shí)刻位置狀態(tài)向量Xk的誤差協(xié)方差矩陣;Pk|k-1為k時(shí)刻位置狀態(tài)向量預(yù)估值X︿k的誤差協(xié)方差矩陣;為k時(shí)刻的位置狀態(tài)估計(jì)值;Kk為卡爾曼增益矩陣;Pzz為k時(shí)刻觀測向量zk的自相關(guān)協(xié)方差矩陣;Pxz為k時(shí)刻位置狀態(tài)向量Xk與觀測向量zk的互協(xié)方差矩陣。

CKF 選取權(quán)值相等的容積點(diǎn)采用球面徑向容積形式進(jìn)行積分計(jì)算[20-22],故本研究使用容積點(diǎn)集方式計(jì)算式(3)中的各參量,可得到CKF 在高斯域非線性系統(tǒng)下的遞推公式。 由移動(dòng)目標(biāo)井下定位模型可知:利用CKF 算法對LANDMARC 定位系統(tǒng)得到的待測目標(biāo)的位置狀態(tài)值進(jìn)行濾波更新,可以得到關(guān)于目標(biāo)位置的一個(gè)初步預(yù)測值。 CKF 濾波算法實(shí)現(xiàn)方程表達(dá)如下。

2.1.1 時(shí)間更新

假設(shè)k時(shí)刻位置狀態(tài)Xk的誤差協(xié)方差矩陣Pk已知,對其通過Cholesky 分解可得矩陣Sk:

計(jì)算k時(shí)刻容積點(diǎn):

式中,i=1,2,…,2n,n為狀態(tài)維數(shù);ζi為第i個(gè)容積點(diǎn); [1 ]i為單位矩陣第i列;wi為權(quán)值;m為容積點(diǎn)數(shù)。

在上述計(jì)算基礎(chǔ)上,進(jìn)一步計(jì)算k+ 1 時(shí)刻狀態(tài)轉(zhuǎn)移方程的傳播容積點(diǎn),以及k+ 1 時(shí)刻待測目標(biāo)位置狀態(tài)預(yù)測值及誤差協(xié)方差預(yù)測值。

2.1.2 量測更新

對k+ 1 時(shí)刻的Pk+1|k通過Cholesky 分解得平方根矩陣Sk+1|k:

通過計(jì)算k+ 1 時(shí)刻容積點(diǎn)、觀測值的預(yù)測值及觀測向量zk+1的自相關(guān)協(xié)方差矩陣,繼而推算出k+1 時(shí)刻位置狀態(tài)向量Xk+1與觀測向量zk+1的互協(xié)方差矩陣。 最終得到觀測函數(shù)h計(jì)算k+ 1 時(shí)刻傳播容積點(diǎn)(i= 1,2,…,m):

k+ 1 時(shí)刻卡爾曼增益為

式中,Kk+1表示為k+1 時(shí)刻卡爾曼增益;Pxz,k+1|k為k時(shí)刻位置狀態(tài)向量Xk與觀測向量zk的互協(xié)方差矩陣。

k+ 1 時(shí)刻的位置狀態(tài)估計(jì)值為

k+ 1 時(shí)刻的位置狀態(tài)誤差協(xié)方差估計(jì)值為

式中,Pk+1為k+1 時(shí)刻的位置狀態(tài)誤差協(xié)方差估計(jì)值;Pk+1|k為k時(shí)刻的位置狀態(tài)誤差協(xié)方差估計(jì)值;Pzz,k+1|k為k時(shí)刻觀測向量zk的自相關(guān)協(xié)方差矩陣;為k+1 時(shí)刻卡爾曼增益的轉(zhuǎn)置矩陣。

由上述公式可知,CKF 濾波算法通過時(shí)間更新和量測更新得到位置狀態(tài)估計(jì)值Xk+1。 在時(shí)間更新階段,濾波器根據(jù)前一步位置狀態(tài)估計(jì)做出對當(dāng)前位置狀態(tài)值的預(yù)測;在量測更新階段,在時(shí)間更新的基礎(chǔ)上根據(jù)實(shí)時(shí)獲取的當(dāng)前位置狀態(tài)觀測值,從而獲得一個(gè)精確度更高的位置狀態(tài)估計(jì)值。

2.2 井下動(dòng)態(tài)LANDMARC 定位模型求解

為求解井下動(dòng)態(tài)LANDMARC 定位模型,需確定井下觀測函數(shù)h與觀測噪聲vk的關(guān)系。 在井下較為封閉的空間中,射頻信號強(qiáng)度隨著傳播距離衰減,常用的距離—衰減模型[23]為

式中,RSSI(d) 表示待測目標(biāo)位置到閱讀器所在位置的距離為d時(shí)的信號強(qiáng)度值;RSSI0表示距離為d0時(shí)對應(yīng)的信號強(qiáng)度值;d0一般取1 m;η為路徑衰減指數(shù),受井下環(huán)境影響與信號傳輸路徑無關(guān);xσ是由于井下因多徑效應(yīng)引起的信號衰落,表示標(biāo)準(zhǔn)偏差為σ、均值為0 的高斯噪聲。

在量測更新階段,衰落值xσ和系統(tǒng)噪聲,其中M為閱讀器的個(gè)數(shù),均會(huì)對測量值產(chǎn)生影響,這些統(tǒng)稱為觀測噪聲,且vik=xσ+φik,則將第i個(gè)閱讀器在k時(shí)刻所接收的待測目標(biāo)的信號強(qiáng)度值作為觀測值uik,表達(dá)式為

式中,dik為k時(shí)刻第i個(gè)閱讀器到待測目標(biāo)的位置距離。 于是,由k時(shí)刻所有閱讀器的觀測值uik所組成的觀測向量的具體形式可以表示為

聯(lián)立式(1)的位置狀態(tài)轉(zhuǎn)移方程與式(14)觀測方程,即可構(gòu)建出最終的井下非線性動(dòng)態(tài)定位模型。

2.3 自適應(yīng)容積卡爾曼濾波

在確定了井下具體定位模型的基礎(chǔ)上,將CKF濾波算法與BP 神經(jīng)網(wǎng)絡(luò)相融合,利用BP 神經(jīng)網(wǎng)絡(luò)泛化映射的特性解決井下非線性空間的定位結(jié)果優(yōu)化問題,得到更加精準(zhǔn)的待測目標(biāo)位置。 BP 神經(jīng)網(wǎng)絡(luò)由輸入層,一個(gè)或多個(gè)隱含層和輸出層構(gòu)成,能夠使確定約束條件下的一組參數(shù)組合的目標(biāo)函數(shù)最小[24]。 其原理結(jié)構(gòu)如圖2 所示。

圖2 BP 神經(jīng)網(wǎng)絡(luò)模型Fig.2 BP neural network model

為充分發(fā)揮BP 神經(jīng)網(wǎng)絡(luò)高度自學(xué)習(xí)和自適應(yīng)能力,確定合適的輸入神經(jīng)元是關(guān)鍵,由式(10)可得CKF 濾波算法k+ 1 時(shí)刻的位置狀態(tài)估計(jì)值,將其變換形式可得:

由式(15)可知,CKF 濾波算法的濾波結(jié)果與位置狀態(tài)的估計(jì)誤差、觀測誤差zk+1-以及卡爾曼增益Kk+1相關(guān)。 將這三者作為BP神經(jīng)網(wǎng)絡(luò)的輸入,經(jīng)過算法迭代學(xué)習(xí)優(yōu)化,輸出待測目標(biāo)位置的真實(shí)值與預(yù)估值之差,通過自適應(yīng)最小化差值,最終可以得到一個(gè)關(guān)于待測目標(biāo)位置較準(zhǔn)確的真實(shí)值T。 BP 神經(jīng)網(wǎng)絡(luò)優(yōu)化的容積卡爾曼濾波原理如圖3 所示。 應(yīng)用自適應(yīng)CKF 濾波算法對井下非線性動(dòng)態(tài)定位模型進(jìn)行求解,可以得到更加精準(zhǔn)的待測目標(biāo)位置。

圖3 自適應(yīng)CKF 算法原理示意Fig.3 Schematic the principal of adaptive CKF algorithm

在井下定位過程中,單一使用傳統(tǒng)的LANDMARC 定位算法存在目標(biāo)定位結(jié)果不準(zhǔn)、移動(dòng)軌跡波動(dòng)大的現(xiàn)象。 為此,在動(dòng)態(tài)模型定位與濾波算法研究的基礎(chǔ)上結(jié)合BP 神經(jīng)網(wǎng)絡(luò)非線性優(yōu)化的特點(diǎn),提出了一種改進(jìn)的LANDMARC 定位算法,流程如圖4 所示。 首先設(shè)置LANDMARC 定位系統(tǒng)布局形式,通過LANDMRAC 定位算法將閱讀器收集到的信號轉(zhuǎn)化為待測目標(biāo)位置狀態(tài)預(yù)估值;然后結(jié)合自適應(yīng)CKF 算法進(jìn)行濾波處理,并對預(yù)估值進(jìn)行修正優(yōu)化和輸出,提高傳統(tǒng)LANDMARC 算法的定位精度及魯棒性。

圖4 LANDMARC 改進(jìn)定位算法流程Fig.4 Flow of LANDMARC positioning improvement algorithm

3 試驗(yàn)與性能對比

3.1 建立實(shí)驗(yàn)環(huán)境

本研究以國內(nèi)某金屬礦區(qū)9 ～11 中段內(nèi)部分區(qū)域的巷道位置信息為基礎(chǔ),由于礦山井下巷道不在同一水平面,針對井下信號的傳播特點(diǎn),模擬了50 m×50 m×50 m 的三維空間進(jìn)行試驗(yàn),真實(shí)、立體地反映了井下的工作環(huán)境。 定位系統(tǒng)部署情況如圖5 所示。分別在三維空間模型的4 個(gè)頂點(diǎn)上安裝了1 個(gè)閱讀器,80 個(gè)參考標(biāo)簽均勻被分布該空間中,間距為10 m。 為確保定位的準(zhǔn)確性和可靠性,所有參考標(biāo)簽均在4 個(gè)頂置閱讀器的檢測范圍內(nèi)。

圖5 定位系統(tǒng)部署Fig.5 Layout of the positioning system

BP 神經(jīng)網(wǎng)絡(luò)由3 層結(jié)構(gòu)構(gòu)成,輸入神經(jīng)元的個(gè)數(shù)為3,分別為估計(jì)誤差、觀測誤差以及卡爾曼增益Kk+1,結(jié)合試驗(yàn)效果設(shè)置隱含神經(jīng)元個(gè)數(shù)為8,輸出為真實(shí)值與預(yù)估值的差值,迭代次數(shù)為1 000 次,期望誤差為0.01。 整個(gè)觀測過程的參數(shù)設(shè)置見表1。

表1 觀測過程參數(shù)設(shè)置Table 1 Parameter settings for observation process

待測目標(biāo)真實(shí)運(yùn)動(dòng)軌跡以及傳統(tǒng)LANDMARC定位算法的定位軌跡如圖6 所示。

圖6 LANDMARC 定位軌跡對比Fig.6 Comparison of LANDMARC positioning traj ectory

由圖6 可知:傳統(tǒng)LANDMARC 定位算法得到的目標(biāo)標(biāo)簽的位置與真實(shí)值之間的距離偏差較大,并有個(gè)別預(yù)測點(diǎn)偏移真實(shí)軌跡極遠(yuǎn)。 反映出該算法能夠粗略地反映出目標(biāo)標(biāo)簽的運(yùn)動(dòng)軌跡,但是定位精度不高,定位性能不穩(wěn)定。

為了評價(jià)本研究算法的定位性能,采用均方根誤差來衡量其定位效果。該值能直觀地反映出待測目標(biāo)預(yù)估坐標(biāo)與真實(shí)坐標(biāo)(x,y,z) 之間的偏離程度,公式為

式中,RMSE為均方根誤差;n′為預(yù)估坐標(biāo)的真實(shí)個(gè)數(shù)。

3.2 定位結(jié)果與性能分析

由于KF 算法在定位環(huán)境處于非線性情況下,其性能會(huì)大幅度下降不適用于井下定位,故在圖5 所示環(huán)境中僅將本研究算法與經(jīng)由HIF 濾波的LANDMARC 定位算法及單一LANDMARC 定位算法進(jìn)行性能比較。

圖7(a)所示為3 種算法的定位軌跡,可以直觀地看出,本研究算法和經(jīng)過HIF 濾波得到的目標(biāo)標(biāo)簽軌跡與真實(shí)軌跡更加貼近。 圖7(b)所示為3 種定位算法的均方根誤差。 分析可得:本研究算法和經(jīng)由HIF 濾波的定位偏差都遠(yuǎn)小于傳統(tǒng)LANDMARC 定位算法。 但是經(jīng)HIF 算法濾波后的定位結(jié)果偏差波動(dòng)較大,呈現(xiàn)逐漸發(fā)散現(xiàn)象。 相較于HIF 濾波算法,本研究算法得到的定位偏差分布較為平穩(wěn)。

圖7 算法軌跡預(yù)測與均方根誤差對比Fig.7 Comparison of the trajectory prediction and root mean square error of the algorithms

為了更加精準(zhǔn)地反映算法的定位性能,將3 種函數(shù)的均方根誤差采用概率密度函數(shù)與累計(jì)分布函數(shù)方式,從定位偏差分布概率以及累計(jì)分布概率上分別進(jìn)行比較,結(jié)果如圖8 所示。 由圖8(a)偏差概率密度分布圖可以看出,本研究算法的定位偏差分布最為集中,小于1 m 占90%以上,且偏差的期望值最小。由圖8(b)偏差經(jīng)驗(yàn)累計(jì)分布圖可知,本研究算法定位偏差小于0.612 m 的概率為60%,HIF 濾波算法的定位偏差小于1.221 m 的概率為60%,LANDMARC算法的定位偏差達(dá)到2.434 m。

圖8 偏差概率密度與偏差累計(jì)分布Fig.8 Deviation probability density and deviation cumulative distribution

綜合上述試驗(yàn)結(jié)果可知:在井下進(jìn)行這種非線性環(huán)境下目標(biāo)定位時(shí),本研究算法相較于經(jīng)由HIF 濾波的LANDMARC 定位算法和單一LANDMARC 定位算法,精度更高,性能更穩(wěn)定。

4 結(jié) 論

針對傳統(tǒng)LANDMARC 算法在井下定位不夠精準(zhǔn)的問題,本研究對其進(jìn)行了改進(jìn),利用BP 神經(jīng)網(wǎng)絡(luò)的泛化映射能力,引入神經(jīng)元參數(shù)對CKF 算法進(jìn)行優(yōu)化,充分結(jié)合BP 神經(jīng)網(wǎng)絡(luò)迭代式學(xué)習(xí)和CKF 算法在強(qiáng)非線性系統(tǒng)中保持穩(wěn)定的特點(diǎn),增強(qiáng)了定位算法的自適應(yīng)能力,提高了井下定位精度。 基于井下真實(shí)場景的仿真試驗(yàn)結(jié)果表明:

(1)本研究所提算法的定位偏差在0.612 m 以下的概率達(dá)到60%,較傳統(tǒng)LANDMARC 定位算法在精度上提高了1.822 m,可滿足井下復(fù)雜動(dòng)態(tài)環(huán)境的高定位精度要求。

(2)引入自適應(yīng)CKF 算法進(jìn)行濾波處理可以大幅提高傳統(tǒng)LANDMARC 定位算法的穩(wěn)定性。 本研究所提算法定位偏差分布更為集中,偏差在1 m 以下的占90%以上,可滿足井下復(fù)雜動(dòng)態(tài)環(huán)境的高穩(wěn)定性要求。

(3)試驗(yàn)中發(fā)現(xiàn),當(dāng)目標(biāo)標(biāo)簽在距離閱讀器較遠(yuǎn)位置移動(dòng)時(shí),本研究算法定位的實(shí)時(shí)性會(huì)較距離近的位置有所下降。 因此,進(jìn)一步提升該算法遠(yuǎn)距離井下定位的實(shí)時(shí)性是后續(xù)研究重點(diǎn)。