改進(jìn)卡爾曼濾波的井下組合導(dǎo)航定位算法

蘆寶娟 趙大磊 徐廣允

(貴州工業(yè)職業(yè)技術(shù)學(xué)院電子與信息工程學(xué)院 貴州 貴陽 551400)

0 引 言

目前，煤礦井下定位系統(tǒng)大多采用RFID區(qū)域位置監(jiān)測技術(shù)，也有少部分采用超帶寬技術(shù)。其中超帶寬技術(shù)[1]定位精度高，但建造成本相對較高，因此造成超帶寬普及率不高，在煤礦井下使用較少。RFID技術(shù)[2]只能將井下作業(yè)人員和設(shè)備定位到一個區(qū)域范圍內(nèi)，定位精度不高，在井下惡劣的環(huán)境中故障率較高，無法滿足煤礦智能化對人員或設(shè)備定位的需求。

國有大中型煤礦都在建設(shè)煤礦井下有線無線一體化網(wǎng)絡(luò)系統(tǒng)，可在此基礎(chǔ)上無須增加額外設(shè)備即可建立井下無線定位系統(tǒng)，用于井下人員定位服務(wù)。能實現(xiàn)井下無線定位方法[3-4]較多，其中指紋定位[5-9]必須在實驗區(qū)采集大量指紋信息，采集數(shù)據(jù)工作量較大，當(dāng)電磁環(huán)境產(chǎn)生改變時，定位精度不穩(wěn)定。多邊定位[10-11]采用最小二乘算法進(jìn)行多邊定位，但定位效果不理想。加權(quán)質(zhì)心定位算法[12]相對簡單，利用權(quán)值權(quán)衡每個參考點對目標(biāo)點的影響，但定位精度較低。然而單一無線定位方法定位效果始終不夠理想，近年來，學(xué)者們提出組合導(dǎo)航進(jìn)行定位，也取得一些成果，其中柏思琪等[13]提出步行慣性導(dǎo)航與無線測距定位相融合的定位算法，但在實際應(yīng)用中定位精度不高。朱亞萍等[14]采用慣性導(dǎo)航定位和無線信號進(jìn)行融合定位，先用位置指紋識別算法進(jìn)行定位，再結(jié)合慣性傳感單元提供的慣性數(shù)據(jù),對定位結(jié)果進(jìn)行融合修正，但實驗條件為室內(nèi)環(huán)境。吳靜然等[15]提出了煤礦井下人員融合定位算法，將慣性導(dǎo)航定位和指紋定位進(jìn)行融合實現(xiàn)井下人員定位，定位效果較好，但融合算法未考慮到擴(kuò)展卡爾曼濾波優(yōu)化問題。

綜上所述，為能夠滿足煤礦井下安全生產(chǎn)對人員和設(shè)備的定位要求，以及煤礦井下智能化對位置定位精度的需求，本文提出并設(shè)計一種井下組合導(dǎo)航定位算法。實驗結(jié)果表明，該定位算法比傳統(tǒng)加權(quán)質(zhì)心定位算法定位精度提高55.47%。

1 井下組合導(dǎo)航定位算法

井下組合導(dǎo)航定位算法是基于微慣性導(dǎo)航系統(tǒng)行人航跡推算算法與無線傳感器網(wǎng)絡(luò)改進(jìn)加權(quán)質(zhì)心定位算法，利用正弦余弦蝙蝠融合算法優(yōu)化的卡爾曼濾波模型進(jìn)行組合導(dǎo)航的定位算法。進(jìn)行組合導(dǎo)航算法運行流程如圖1所示。

圖1 井下組合導(dǎo)航定位算法流程

可以看出，井下組合導(dǎo)航算法是由兩種定位算法進(jìn)行組合得到，具體算法分析如下。

1.1 微慣性導(dǎo)航系統(tǒng)定位算法

微慣性導(dǎo)航系統(tǒng)(Micro-Inertial-Navigation System,Micro-INS)是一種基于微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System,MEMS)傳感器技術(shù)的微型慣性導(dǎo)航系統(tǒng)。微慣性導(dǎo)航系統(tǒng)是一種不依賴于外部信息的自主式導(dǎo)航系統(tǒng)，可適應(yīng)煤礦井下特殊的工作環(huán)境。

本文中的微慣性導(dǎo)航系統(tǒng)定位算法采用行人航跡推算算法(Pedestrian Dead Reckoning，PDR)。該算法對步行者行走的步數(shù)、步長、方向進(jìn)行測量和統(tǒng)計，推算出步行者行走軌跡和位置等信息。

PDR算法中的步態(tài)檢測是行人在行走過程中邁步的一個周期的重復(fù)動作。本文慣性定位終端放置于作業(yè)人員腰間，因此采用過零點檢測法來檢測行人行走的步數(shù)。

PDR算法通過慣性傳感器的數(shù)據(jù)和已知初始位置，計算出步頻、步長和方向角，并利用計算出來的值估算任意時刻的位置坐標(biāo)，可表示為：

(1)

1.2 改進(jìn)加權(quán)質(zhì)心定位算法

圖2 質(zhì)心模型

由圖2可知，DEF組成一個三角形，計算其質(zhì)心O的坐標(biāo)公式為：

(2)

加權(quán)質(zhì)心定位利用半徑距離ri計算加權(quán)值，定位誤差較大，在井下特殊環(huán)境中不適用。為此提出改進(jìn)加權(quán)質(zhì)心定位算法，采用AP節(jié)點之間半徑距離ri與RSSI值之比的平方和計算加權(quán)值，能很好反映AP節(jié)點間的內(nèi)在關(guān)系。

(3)

式中：{ω1,ω2,ω3}為質(zhì)心加權(quán)值。由此可得到改進(jìn)加權(quán)質(zhì)心定位公式為：

(4)

1.3 基于組合導(dǎo)航的卡爾曼濾波算法

本文提出的井下組合導(dǎo)航定位算法，采用微慣性導(dǎo)航系統(tǒng)定位的位置坐標(biāo)和速度與改進(jìn)加權(quán)質(zhì)心定位的位置坐標(biāo)和速度之差作為量測信息。

因井下只考慮二維平面，所以假設(shè)目標(biāo)點的真實坐標(biāo)為(x，y)，速度為(vx，vy)；微慣性導(dǎo)航系統(tǒng)定位的位置坐標(biāo)為(xMi，yMi),速度為(vMx，vMy)；改進(jìn)加權(quán)質(zhì)心定位的位置坐標(biāo)為(xRi，yRi),速度為(vRx，vRy)；坐標(biāo)量測信息為地理坐標(biāo)位置微慣性導(dǎo)航系統(tǒng)定位與改進(jìn)加權(quán)質(zhì)心定位之差，可表示為：

(5)

式中：δx為x軸的差值；ρx為x軸的白噪聲；δy為y軸的差值；ρy為y軸的白噪聲。

速度量測信息為地理坐標(biāo)位置微慣性導(dǎo)航系統(tǒng)速度與改進(jìn)加權(quán)質(zhì)心定位速度之差，可表示為：

(6)

式中：δvx為x軸的速度差值；ρvx為x軸的速度白噪聲；δvy為y軸的速度差值；ρvy為y軸的速度白噪聲。

將式(5)和式(6)合并，可以得到組合導(dǎo)航算法的卡爾曼濾波方程[18]，可表示為：

Zi=HiXi+Vi

(7)

式中：Zi為第i時刻的觀測向量；Hi為第i時刻的量測靈敏度矩陣；Xi為第i時刻的估計誤差向量；Vi為第i時刻的量測白噪聲向量。

根據(jù)卡爾曼濾波(Kalman Filter，KF)求解卡爾曼增益矩陣、協(xié)方差矩陣[19]，可表示為：

(8)

(9)

Pi(+)=(I-KiHi)Pi(-)

(10)

(11)

(12)

按照傳統(tǒng)卡爾曼濾波，零均值不相關(guān)設(shè)備噪聲和零均值白噪聲依照井下定位系統(tǒng)所處的環(huán)境取近似量，不能實現(xiàn)濾波的準(zhǔn)確估計，為此需對這兩個參數(shù)進(jìn)行實時優(yōu)化以提高組合定位算法的定位精度，所以本文采用改進(jìn)蝙蝠算法實時優(yōu)化這兩個參數(shù)。

1.4 蝙蝠算法

蝙蝠算法(Bat Algorithm，BA)是由Yang[20]提出的生物啟發(fā)式智能優(yōu)化算法。它是利用蝙蝠在覓食過程中脈沖發(fā)射頻率與脈沖響度的變化規(guī)律，通過可調(diào)節(jié)頻率擴(kuò)大空間搜索范圍，最終尋找到最優(yōu)解。

(13)

式中：xbbest為當(dāng)前種群中最優(yōu)位置；fθ為脈沖頻率。

位置更新公式為：

(14)

蝙蝠算法能有效控制全局搜索和局部搜索[21]，在迭代過程中引入自適應(yīng)的脈沖發(fā)射響度Cθ和脈沖發(fā)射速率jθ。蝙蝠找食物過程中，Cθ減小，jθ增大。當(dāng)脈沖發(fā)射響度為最小時，Cmin=0，表示蝙蝠已經(jīng)找到食物并停止發(fā)射脈沖。

在局部搜索時，蝙蝠擾動位置更新公式為：

(15)

1.5 正弦余弦算法

正弦余弦算法(Sine Cosine Algorithm，SCA)是由Mirjalili[22]提出的新型智能優(yōu)化算法，先利用正弦函數(shù)值或余弦函數(shù)值大于1或小于-1時進(jìn)行全局探索，再通過正弦函數(shù)值或余弦函數(shù)值在-1到1之間時算法進(jìn)行局部開發(fā)。該算法特點是基于正弦函數(shù)和余弦函數(shù)的性質(zhì)，通過迭代尋找出全局最優(yōu)解。

(16)

1.6 生成定位算法

在實現(xiàn)傳統(tǒng)的卡爾曼濾波時，零均值不相關(guān)設(shè)備噪聲和零均值白噪聲應(yīng)當(dāng)根據(jù)井下定位系統(tǒng)所處的環(huán)境取近似量，但實際應(yīng)用時是根據(jù)經(jīng)驗和算法特征估計這兩個參數(shù)，不能確保參數(shù)的值達(dá)到井下定位系統(tǒng)所處的環(huán)境取近似量，從而會影響濾波的準(zhǔn)確估計，進(jìn)而降低組合定位算法的定位精度。為此需對這兩個參數(shù)進(jìn)行實時優(yōu)化，以提高組合定位算法的定位精度。

為解決上述問題，本文提出的井下組合導(dǎo)航定位算法(以下簡稱為組合導(dǎo)航算法)，將正弦余弦算法和蝙蝠算法進(jìn)行融合，延伸出正弦余弦蝙蝠融合算法(Sine Cosine Algorithm and Bat Algorithm，SCABA)，并用其對卡爾曼濾波進(jìn)行優(yōu)化。SCABA具有強大的局部和全局搜索能力，同時能提高搜索效率[24]。

卡爾曼濾波中零均值不相關(guān)設(shè)備噪聲和零均值白噪聲與算法的定位精度有關(guān)，通過SCABA搜索出這兩個參數(shù)全局最優(yōu)值，從而提高算法定位精度。

零均值不相關(guān)設(shè)備噪聲的協(xié)方差Qi，所對應(yīng)的最優(yōu)零均值不相關(guān)設(shè)備噪聲誤差為qbest。利用qbest對零均值不相關(guān)設(shè)備噪聲的協(xié)方差進(jìn)行估算[25]，可以表示為：

(17)

式中：Q0為零均值不相關(guān)設(shè)備噪聲的協(xié)方差初始值。

零均值白噪聲的協(xié)方差Bi，所對應(yīng)的最優(yōu)零均值白噪聲誤差為bbest。利用bbest對零均值白噪聲的協(xié)方差進(jìn)行估算，可以表示為：

(18)

式中：B0為零均值白噪聲的協(xié)方差初始值。

為使組合導(dǎo)航定位更精確，提高卡爾曼濾波定位精度，構(gòu)造適應(yīng)度函數(shù)為輸出的平方誤差的倒數(shù)：

(19)

利用正弦余弦蝙蝠融合算法優(yōu)化卡爾曼濾波模型,從而生成組合導(dǎo)航算法模型。正弦余弦蝙蝠融合算法優(yōu)化算法主要用于對最優(yōu)零均值不相關(guān)設(shè)備噪聲誤差qbest和最優(yōu)零均值白噪聲誤差bbest進(jìn)行尋優(yōu)，利用最優(yōu)的qbest和bbest，再計算出零均值不相關(guān)設(shè)備噪聲的協(xié)方差Qi和零均值白噪聲的協(xié)方差Bi。從而確保這兩個參數(shù)的值達(dá)到井下定位系統(tǒng)所處的環(huán)境取近似量，提高濾波的準(zhǔn)確估計，提升組合定位算法的定位精度。

正弦余弦蝙蝠融合算法優(yōu)化卡爾曼濾波具體步驟如下：

輸入：優(yōu)化參數(shù)：qbest和bbest。

輸出：尋優(yōu)后參數(shù)：qbest和bbest。

Step1利用訓(xùn)練數(shù)據(jù)集，初步生成卡爾曼濾波模型,并生成優(yōu)化參數(shù)bbest和qbest。

Step2初始化SCABA各個參數(shù)，在d維空間中隨機(jī)生成蝙蝠的位置xbθ，其中θ=1,2,…,N。將bbest和qbest映射到最優(yōu)位置中，設(shè)置迭代次數(shù)t=1。

Step4根據(jù)式(13)和式(14)更新蝙蝠個體速度和位置，產(chǎn)生新一代個體，計算適應(yīng)度，并對比最優(yōu)解，將最優(yōu)解存放到最優(yōu)解集中。

Step5隨機(jī)產(chǎn)生一個均勻分布數(shù)1∈[0,1]。當(dāng)1>jθ,則從當(dāng)前種群最優(yōu)解集中選擇一個解，用式(15)進(jìn)行擾動，產(chǎn)生新個體xbnew來替代選擇出來的解。

Step6隨機(jī)產(chǎn)生一個均勻分布數(shù)2∈[0,1]。當(dāng)2>Cθ且新個體適應(yīng)度值小于當(dāng)前個體適應(yīng)度值，接收Step 4產(chǎn)生的解為新解，更新脈沖發(fā)射響度和脈沖發(fā)射速率。

Step7將每個蝙蝠的位置為正弦余弦算法初始點，根據(jù)式(16)更新蝙蝠個體位置，計算適應(yīng)度，更新φ1、φ2、φ3、φ4。

Step8按照適應(yīng)度值大小對所有蝙蝠進(jìn)行排序，找出最優(yōu)的蝙蝠位置，并將最優(yōu)解存放到最優(yōu)解集中。

Step9t=t+1，判斷迭代次數(shù)是否到達(dá)最大次數(shù)或適應(yīng)度是否到達(dá)收斂精度。若滿足終止條件，算法結(jié)束輸出最優(yōu)解集；否則跳轉(zhuǎn)到Step 4。

Step10將最優(yōu)解集映射回卡爾曼濾波模型，完成組合導(dǎo)航算法模型。

2 井下組合導(dǎo)航定位算法流程

(1) 在煤礦井下實驗區(qū)域中，利用慣性定位終端采集一定數(shù)量訓(xùn)練數(shù)據(jù)，建立訓(xùn)練數(shù)據(jù)集。生成卡爾曼濾波需要優(yōu)化參數(shù)，將參數(shù)帶到正弦余弦蝙蝠融合算法中進(jìn)行尋優(yōu)。

(2) 將正弦余弦蝙蝠融合算法最優(yōu)解集，帶回到卡爾曼濾波模型中，建立組合導(dǎo)航算法。

(3) 由慣性定位終端掃描目標(biāo)點各個AP節(jié)點的RSSI值、MAC地址和IP地址的無線數(shù)據(jù)。采集目標(biāo)點處加速度計、陀螺儀和磁力計的慣性數(shù)據(jù)，并采集時間數(shù)據(jù)。連續(xù)這樣的3次采樣后，計算出平均值，再傳輸給數(shù)據(jù)庫服務(wù)器。

(4) 將第i-1時刻(即上一個時刻)組合定位坐標(biāo)為初始步，并與采集得到慣性數(shù)據(jù)進(jìn)行行人航跡推算，估算出目標(biāo)點的位置坐標(biāo)和速度，再將位置坐標(biāo)和速度送到組合導(dǎo)航算法中。

(5) 將RSSI數(shù)據(jù)通過改進(jìn)加權(quán)質(zhì)心定位算法，估算出第i時刻質(zhì)心定位位置坐標(biāo)。利用第i-1時刻質(zhì)心定位坐標(biāo)、第i時刻質(zhì)心定位坐標(biāo)和時間計算出速度。然后將位置坐標(biāo)和速度送到組合導(dǎo)航算法中。

(6) 將微慣性導(dǎo)航算法定位的位置坐標(biāo)和速度，與改進(jìn)加權(quán)質(zhì)心定位的位置坐標(biāo)和速度組合在算法中，并利用卡爾曼濾波對結(jié)果進(jìn)行修正。得出第i時刻組合導(dǎo)航定位坐標(biāo)位置，完成整個組合導(dǎo)航算法定位流程。

3 實 驗

3.1 實驗條件

本實驗條件為貴州某煤礦井下580 m的巷道，巷道中無線局域網(wǎng)已覆蓋。井下巷道工程平面如圖3所示。

圖3 巷道工程平面

可以看出，實驗區(qū)區(qū)域在運輸巷，該巷道中有帶式輸送機(jī)。當(dāng)帶式輸送機(jī)工作時會對電磁環(huán)境產(chǎn)生干擾，為此采用組合導(dǎo)航進(jìn)行定位，減小環(huán)境對定位產(chǎn)生影響。井下巷道中安裝有交接機(jī)和無線AP設(shè)備，采集端由自制慣性定位終端進(jìn)行數(shù)據(jù)的采集工作。

慣性定位終端主控芯片選用STC15W4K56S4芯片，Wi-Fi模塊選用ESP8266EX芯片，加速度計、陀螺儀和電子羅盤選用集成一體9軸慣性傳感器模塊MPU9250芯片。該終端經(jīng)過防爆測試完全滿足井下設(shè)備本質(zhì)安全性的要求[26]。

運輸巷中無線覆蓋，AP與AP間距在50 m到80 m之間不等。測試人員把每個AP節(jié)點的MAC地址、IP地址和物理位置坐標(biāo)記錄下，并組合為每個AP節(jié)點信息集(MACj,IPj,xj,yj)，其中MACj為節(jié)點的MAC地址，IPj為節(jié)點的IP地址,(xj,yj)為節(jié)點的坐標(biāo)地址，將所有AP節(jié)點信息集存放到AP信息數(shù)據(jù)庫，為改進(jìn)加權(quán)質(zhì)心定位算法提供節(jié)點數(shù)據(jù)。

為保證實驗多樣性，測試工作分兩種情況進(jìn)行，分別為帶式輸送機(jī)工作時和帶式輸送機(jī)停止工作時進(jìn)行測試。首先在帶式輸送機(jī)停止工作時，由測試人員將慣性定位智能終端戴在腰間，以2 m/s的速度從起點勻速走到終點，再以3 m/s的速度由終點勻速走到起點。慣性定位智能終端每隔10 s傳輸一次數(shù)據(jù)，每一趟可以采集48組數(shù)據(jù)。測試人員按相同方法完成2趟采集工作，采集到96組數(shù)據(jù)。其次在帶式輸送機(jī)工作時，測試人員按照上述測試方式再次進(jìn)行實驗，完成2趟采集工作，采集到96組數(shù)據(jù)。兩種情況下一共采集到192組信息數(shù)據(jù)，全部存放到訓(xùn)練數(shù)據(jù)集。利用訓(xùn)練數(shù)據(jù)集，采用正弦余弦蝙蝠融合算法對卡爾曼濾波模型進(jìn)行多次訓(xùn)練，最終生成組合導(dǎo)航算法模型。

3.2 算法優(yōu)化性能分析

實時在線測試階段：首先在帶式輸送機(jī)停止工作時，由測試人員將慣性定位智能終端戴在腰間，以1.5 m/s的速度從起點勻速走到終點，再以2 m/s的速度由終點勻速走到起點。慣性定位智能終端每隔5 s傳輸一次數(shù)據(jù)，每一趟可以采集135組數(shù)據(jù)。然后在帶式輸送機(jī)工作時，測試人員按照上述測試方式再次進(jìn)行實驗，完成1趟測試工作，采集到135組數(shù)據(jù)。兩種情況下一共采集到270組信息數(shù)據(jù)，全部存放到在線定位數(shù)據(jù)庫。

為檢測組合導(dǎo)航算法的優(yōu)化性能，將蝙蝠算法、正弦余弦算法和本文算法分別對卡爾曼濾波模型進(jìn)行優(yōu)化，待優(yōu)化完成后進(jìn)行定位測試。采用相同訓(xùn)練數(shù)據(jù)集和在線定位數(shù)據(jù)庫進(jìn)行模型優(yōu)化和井下定位，其定位誤差的累計概率分布如圖4所示。

圖4 定位誤差的累計概率分布

由圖4可知，蝙蝠算法優(yōu)化卡爾曼濾波模型的組合導(dǎo)航算法定位精度較低，其定位誤差范圍小于2 m的概率為52.75%。利用正弦余弦算法優(yōu)化卡爾曼濾波模型的組合導(dǎo)航算法定位精度稍高，其定位誤差范圍小于2 m的概率為55.31%。本文算法優(yōu)化卡爾曼濾波模型的組合導(dǎo)航算法定位精度最高，其定位誤差范圍小于2 m的概率為61.82%。

3.3 算法定位性能分析

為檢測組合導(dǎo)航算法的定位性能，利用在線定位數(shù)據(jù)庫中的信息數(shù)據(jù)分別將傳統(tǒng)加權(quán)質(zhì)心定位算法、行人航跡推算算法和本文算法進(jìn)行定位，其結(jié)果如圖5所示。

圖5 算法定位誤差

由圖5可知，行人航跡推算算法初始定位時精度較高，但隨著時間增加定位累計誤差也隨之增加，當(dāng)采樣1 min后，定位誤差到達(dá)6 m以上。傳統(tǒng)加權(quán)質(zhì)心定位算法定位精度不高，平均定位誤差在3.84 m。本文算法定位精度明顯高于兩種定位算法，其平均定位誤差在1.71 m，其定位精度明顯高于前兩種定位算法。

為檢測組合導(dǎo)航算法的定位性能，從起點處開始采集RSSI指紋庫。采樣點之間的平均距離為1 m，運輸巷中采集100 m的范圍，一共采集300個采樣點，組成指紋數(shù)據(jù)庫，為文獻(xiàn)[15]提供指紋定位算法數(shù)據(jù)。將傳統(tǒng)加權(quán)質(zhì)心定位算法、文獻(xiàn)[15]融合定位算法和本文算法進(jìn)行定位誤差比較，具體如表1所示。

表1 算法定位誤差比較表 單位：m

從表1可看出，傳統(tǒng)加權(quán)質(zhì)心定位算法平均定位誤差為3.84 m；文獻(xiàn)[15]算法平均定位誤差為1.98 m；組合導(dǎo)航算法最大定位誤差為2.49 m，平均定位誤差為1.71 m。由此可見本文算法定位精度最高，比傳統(tǒng)加權(quán)質(zhì)心定位算法定位精度提高55.47%，比文獻(xiàn)[15]算法定位精度提高13.64%。

4 結(jié) 語

因煤礦井下特定的地理和通信環(huán)境，傳統(tǒng)單一方式定位易受井下環(huán)境影響。在充分利用井下現(xiàn)有無線網(wǎng)絡(luò)資源情況下，提出基于行人航跡推算算法和改進(jìn)加權(quán)質(zhì)心定位算法的組合導(dǎo)航算法。

本文提出將正弦余弦算法和蝙蝠算法進(jìn)行融合，并用其對卡爾曼濾波參數(shù)進(jìn)行優(yōu)化，使卡爾曼濾波中參數(shù)最大限度達(dá)到井下定位系統(tǒng)所處的環(huán)境取近似量，提升卡爾曼濾波的準(zhǔn)確度，提高算法的定位精度。探索出智能算法優(yōu)化卡爾曼濾波應(yīng)用到無線定位算法的解決方法。

本文提出的井下組合導(dǎo)航算法不僅加強卡爾曼濾波的組合度，還進(jìn)一步提升組合導(dǎo)航算法的定位精度。其定位精度比傳統(tǒng)加權(quán)質(zhì)心定位算法定位精度提高55.47%。

接下來的研究工作重點是如何改進(jìn)行人航跡推算算法和加權(quán)質(zhì)心定位算法，進(jìn)一步提升正弦余弦蝙蝠融合算法優(yōu)化卡爾曼濾波性能，從而提高組合導(dǎo)航算法的定位精度。