基于卡爾曼濾波算法的UWB+IMU組合精確定位系統(tǒng)在選煤廠中的應(yīng)用

白怡明，曾祥玉，李 杰，辛鳳陽(yáng)，郭曉松，朱金龍

(1.中煤科工集團(tuán)沈陽(yáng)設(shè)計(jì)研究院有限公司，遼寧 沈陽(yáng) 110015；2.內(nèi)蒙古錫林河煤化工有限責(zé)任公司，內(nèi)蒙古 錫林郭勒 026324)

煤炭產(chǎn)業(yè)作為傳統(tǒng)行業(yè),多年以來(lái),人員的安全問(wèn)題一直被社會(huì)持續(xù)關(guān)注，而選煤作為提高煤炭高附加值產(chǎn)業(yè)，推進(jìn)其智能化建設(shè)發(fā)展是科技進(jìn)步時(shí)代發(fā)展的必然趨勢(shì)[1-2]。目前,人員精確定位系統(tǒng)在選煤行業(yè)的應(yīng)用尚處于初級(jí)階段[3]。選煤廠人員定位系統(tǒng)最早源于主要機(jī)電設(shè)備離線巡檢系統(tǒng)，即人員在固定的巡檢路線中多個(gè)地點(diǎn)和設(shè)備位置巡查，人員定位系統(tǒng)則大多照搬煤礦井下的模式[4-5]，從最開(kāi)始的Wi-Fi技術(shù)到ZigBee通信技術(shù)再到現(xiàn)在主流的超寬帶定位技術(shù)(Ultra-Wide Band，UWB)，隨著技術(shù)手段的提升[6-7]，定位精準(zhǔn)度逐漸提高。

選煤廠工作環(huán)境較為復(fù)雜，與礦井井下的工作環(huán)境差異較大[8]，存在嚴(yán)重的遮擋、屏蔽、電磁干擾等現(xiàn)象，導(dǎo)致普通的定位技術(shù)在實(shí)際計(jì)算過(guò)程中出現(xiàn)人員定位不準(zhǔn)確[9-10]，會(huì)間斷地出現(xiàn)“超人”、畫(huà)面人員漂移、穿越設(shè)備或墻體等現(xiàn)象，個(gè)別大型設(shè)備密集地方還會(huì)出現(xiàn)人員數(shù)量不準(zhǔn)確等情況[11-13]。一般的無(wú)線定位技術(shù)難以達(dá)到較高的定位精度，而UWB定位技術(shù)具有短脈沖、高多徑分辨率、高定位精度的特點(diǎn)，適合在室內(nèi)定位中應(yīng)用[14-16]。但是UWB定位需要3個(gè)以上的固定基站，因?yàn)樵谀硞€(gè)基站的信號(hào)完全被遮擋的非視距(NLOS)情況下[17]，會(huì)出現(xiàn)可見(jiàn)基站變少不能完成定位或者定位精度較差的情況[18-19]。IMU(Inertial Measurement Uunit)即慣性測(cè)量單元，是一種測(cè)量物體空間三軸姿態(tài)角及運(yùn)動(dòng)的傳感裝置[20]。UWB+IMU組合精確定位系統(tǒng)通過(guò)輔以慣性測(cè)量單元，將UWB的位置和定位標(biāo)簽的速度作為擴(kuò)展卡爾曼濾波算法的觀測(cè)數(shù)據(jù)，來(lái)更新IMU的推算值。

為提高定位系統(tǒng)數(shù)據(jù)的準(zhǔn)確性，開(kāi)發(fā)了基于卡爾曼濾波算法的選煤廠UWB+IMU組合精確定位系統(tǒng)，并在大柳塔選煤廠中測(cè)試應(yīng)用。

1 選煤廠人員精確定位技術(shù)

當(dāng)前我國(guó)智能化選煤廠的建設(shè)，對(duì)人員定位的安全和精準(zhǔn)度要求越來(lái)越高[21]，大多數(shù)定位系統(tǒng)的功能已不能滿足選煤廠智能化建設(shè)發(fā)展的需求[22-23]，因此需要加大對(duì)選煤廠室內(nèi)定位功能的開(kāi)發(fā)和設(shè)計(jì)，不斷完善人員定位管理系統(tǒng)的監(jiān)控功能，以及完善人員監(jiān)管的有效性、準(zhǔn)確性，從而降低事故發(fā)生率與人員傷亡率[24-25]。選煤廠現(xiàn)階段應(yīng)用最多的定位技術(shù)有藍(lán)牙、Wi-Fi、精準(zhǔn)ZigBee定位技術(shù)、RFID定位技術(shù)、UWB定位技術(shù)[26]等。定位技術(shù)對(duì)比見(jiàn)表1。

表1 定位技術(shù)對(duì)比Table 1 Comparison of positioning techniques currently available

2 UWB+IMU組合精確定位系統(tǒng)

UWB+IMU組合精確定位系統(tǒng)是一種針對(duì)室內(nèi)定位NLOS現(xiàn)象，提出減弱其影響誤差的一種算法模型，主要是利用UWB做定位測(cè)距定位技術(shù)，通過(guò)輔以慣性測(cè)量單元(Inertial Measurement Unit)做平滑處理，預(yù)測(cè)并修正NLOS誤差概率曲線，最后利用卡爾曼濾波算法處理NLOS誤差干擾對(duì)UWB系統(tǒng)精確定位計(jì)算的影響。

2.1 模型建立

卡爾曼濾波算法是一種典型濾波算法，是一種積累歷史數(shù)據(jù)，更新數(shù)據(jù)預(yù)測(cè)值的迭代過(guò)程。濾波的作用就是減少噪聲誤差與干擾對(duì)數(shù)據(jù)測(cè)量的影響。

卡爾曼濾波模型是可用于時(shí)變線性系統(tǒng)的算法機(jī)理模型，這種算法模型將過(guò)去大量的實(shí)際數(shù)據(jù)和誤差數(shù)據(jù)匯總，合并到現(xiàn)在的誤差數(shù)據(jù)上，以預(yù)測(cè)未來(lái)的誤差數(shù)據(jù)。目前基于卡爾曼濾波算法的UWB精確定位系統(tǒng)研究仍處于初級(jí)階段。從影響準(zhǔn)確度的參數(shù)入手，利用卡爾曼濾波算法建立過(guò)程模型，對(duì)其進(jìn)行各種響應(yīng)測(cè)試。該算法是在變化的初始數(shù)據(jù)中去除噪聲后對(duì)未來(lái)進(jìn)行數(shù)據(jù)預(yù)測(cè)的算法，是基于大數(shù)據(jù)統(tǒng)計(jì)原理的分析算法。任何一個(gè)卡爾曼濾波輸入值都是可測(cè)量的，僅需要知道可測(cè)量的精度是多少，通過(guò)卡爾曼濾波算法模型不斷進(jìn)行數(shù)據(jù)積累，對(duì)系統(tǒng)工程的不斷測(cè)量，然后不斷估計(jì)，持續(xù)一段時(shí)間就能模擬出一個(gè)相對(duì)準(zhǔn)確的預(yù)測(cè)值。通過(guò)卡爾曼濾波算法原理，可以在測(cè)量值可能不準(zhǔn)確的前提下，通過(guò)不斷估算，迭代更新，估計(jì)出一個(gè)相對(duì)準(zhǔn)確的系統(tǒng)輸出值。模型建立的基礎(chǔ)是采用卡爾曼濾波算法通過(guò)simulink仿真平臺(tái)，建立UWB+IMU組合精確定位系統(tǒng)?？柭鼮V波算法模型原理如圖1所示。

圖1 卡爾曼濾波算法模型原理Fig. 1 Principle of the Kalman filtering algorithm-based positioning model

建立卡爾曼濾波算法模型分為兩部分：第一部分就是卡爾曼濾波算法的狀態(tài)方程和觀測(cè)方程，它是一個(gè)預(yù)測(cè)數(shù)據(jù)實(shí)時(shí)更新的過(guò)程；第二部分是卡爾曼濾波算法分析值迭代過(guò)程，每個(gè)節(jié)點(diǎn)默認(rèn)僅有自身實(shí)際的變量指標(biāo)，沒(méi)有其他控制變量存在，這個(gè)算法模型可以幫助UWB精確定位系統(tǒng)提高整個(gè)系統(tǒng)的響應(yīng)時(shí)間。在該處的初始控制量是0，初始測(cè)量值1，將初始測(cè)量值分為實(shí)際值0.5和預(yù)測(cè)值0.5，也就是用卡爾曼濾波算法來(lái)預(yù)測(cè)UWB精確定位系統(tǒng)中的距離變化時(shí)，將默認(rèn)為不變化，預(yù)測(cè)數(shù)據(jù)實(shí)時(shí)更新的變化值就是1，由于距離變化是可以直接測(cè)量的，因此測(cè)量矩陣變化值也是1。

卡爾曼濾波算法模型的基本公式參數(shù)較多，但在UWB精確定位系統(tǒng)中有大量參數(shù)都在自我不斷地迭代更新，整個(gè)UWB精確定位系統(tǒng)真正受外界調(diào)整的參數(shù)只有三個(gè)，而系統(tǒng)開(kāi)始循環(huán)的初始值，需要根據(jù)選煤廠的實(shí)際情況人為定義，其他影響UWB精確定位系統(tǒng)精準(zhǔn)度的兩個(gè)參數(shù)是過(guò)程誤差和測(cè)量誤差的協(xié)方差。在卡爾曼濾波算法模型測(cè)試中，給定一個(gè)真實(shí)的選煤廠UWB精確定位系統(tǒng)數(shù)據(jù)，通過(guò)對(duì)真實(shí)的系統(tǒng)數(shù)據(jù)進(jìn)行迭代更新，再加上測(cè)量誤差噪聲，可同時(shí)檢測(cè)系統(tǒng)實(shí)際值、測(cè)量值還有卡爾曼預(yù)測(cè)估值?？柭鼮V波算法迭代模型如圖2所示。

圖2 卡爾曼濾波算法迭代模型Fig. 2 The Kalman filtering algorithm-based iterative model

在卡爾曼濾波算法模型的實(shí)際測(cè)試中，得到了大量過(guò)程誤差和測(cè)量誤差，這些誤差的協(xié)方差是不可直接測(cè)量的，是存在于UWB精確定位系統(tǒng)本身的，但誤差數(shù)據(jù)仍存在一定關(guān)聯(lián)性，因此在實(shí)際模擬過(guò)程中發(fā)現(xiàn)，卡爾曼濾波算法模型中即便存在一個(gè)對(duì)誤差的協(xié)方差有不準(zhǔn)確的判斷時(shí)，也能對(duì)系統(tǒng)的真實(shí)輸出數(shù)據(jù)有一個(gè)較好的預(yù)測(cè)。

2.2 精確定位

定位標(biāo)簽行走路線為已固定坐標(biāo)為圓心、半徑為2.5 m的圓，在無(wú)遮擋的定位區(qū)域中心，UWB精確定位精度基本上比較可靠，而在非視距環(huán)境下的定位試驗(yàn)中，混凝土柱子、較大的金屬物體對(duì)定位精度影響較大。此外，人體對(duì)UWB信號(hào)的阻擋也十分嚴(yán)重，單UWB精確定位系統(tǒng)位置誤差較大并且部分?jǐn)?shù)據(jù)丟失，而在無(wú)遮擋區(qū)域定位精度相對(duì)較高且比較穩(wěn)定。UWB+IMU組合精確定位系統(tǒng)通過(guò)更新擴(kuò)展卡爾曼濾波算法可更新NLOS誤差值并獲得更準(zhǔn)確的估算位置，從而實(shí)現(xiàn)在NLOS影響下的高精度定位。在選煤廠復(fù)雜環(huán)境的定位系統(tǒng)中，普通UWB基站測(cè)出的距離數(shù)據(jù)，可能因遮擋、多徑干擾導(dǎo)致定位誤差較大，有的地方達(dá)到100 cm左右，而采用基于卡爾曼濾波算法進(jìn)行數(shù)據(jù)預(yù)處理后，數(shù)據(jù)得到較大的優(yōu)化，定位精度可以提升至30 cm左右。 UWB+IMU組合精確定位融合算法流程如圖3所示。

圖3 UWB+IMU組合精確定位融合算法流程Fig. 3 Flowchart of the fusion algorithm process of the UWB+IMU integrated accurate positioning system

2.3 系統(tǒng)組成

基于卡爾曼濾波算法的UWB+IMU組合精確定位系統(tǒng)由人員定位卡、UWB定位基站、定位引擎、系統(tǒng)軟件等組成。人員定位卡與現(xiàn)場(chǎng)的UWB定位基站之間分別測(cè)距，基站將與定位卡之間的定位距離傳輸給定位引擎，定位引擎內(nèi)運(yùn)行定位算法，將計(jì)算出的位置數(shù)據(jù)輸送給平臺(tái)軟件進(jìn)行位置展示?？柭鼮V波算法運(yùn)行在定位引擎內(nèi)部，定位基站傳輸?shù)脑紨?shù)據(jù)先經(jīng)過(guò)濾波算法處理后，再進(jìn)行定位算法計(jì)算，能顯著提升定位系統(tǒng)精度。

3 UWB+IMU組合精確定位系統(tǒng)的應(yīng)用

3.1 系統(tǒng)網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)

大柳塔選煤廠UWB+IMU組合精確定位系統(tǒng)網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)如圖4所示。該系統(tǒng)由服務(wù)器、路由器、交換機(jī)、光纖環(huán)網(wǎng)、集中控制器、智能定位模塊、定位標(biāo)簽卡組成。服務(wù)器配有定位精度解析軟件及Web發(fā)布軟件，交換機(jī)用于連接各個(gè)集中控制器，智能定位模塊、定位標(biāo)簽卡均通過(guò)無(wú)線網(wǎng)絡(luò)全覆蓋來(lái)實(shí)現(xiàn)通信。

圖4 UWB+IMU組合精確定位系統(tǒng)網(wǎng)絡(luò)拓?fù)鋱DFig. 4 Network topology structure of the UWB+IMU integrated accurate personnel positioning system

根據(jù)UWB信號(hào)傳輸?shù)奶攸c(diǎn)，一般情況下，智能定位模塊天線覆蓋的定位范圍在30 m左右，由于選煤廠復(fù)雜的現(xiàn)場(chǎng)作業(yè)環(huán)境，在大柳塔選煤廠內(nèi)智能定位模塊距離考慮在15～20 m左右(安裝在智能照明燈具上)。與定位相關(guān)的主要決定因素是UWB定位卡與UWB定位基站之間距離的準(zhǔn)確程度，以及定位數(shù)據(jù)的連續(xù)性。當(dāng)人員攜帶定位標(biāo)簽卡出現(xiàn)在智能定位模塊(智能燈具)周?chē)鷷r(shí)，多個(gè)智能定位模塊可分別測(cè)算出該定位標(biāo)簽卡與自身的距離，并將原始數(shù)據(jù)進(jìn)行噪聲誤差剔除、初步測(cè)距算法等處理，然后將人員定位標(biāo)簽卡信息及距離信息上傳至服務(wù)器端；在服務(wù)器端，通過(guò)卡爾曼濾波算法模型過(guò)濾，并結(jié)合選煤廠各燈具的具體位置及高度，構(gòu)建參與運(yùn)算的坐標(biāo)系統(tǒng)，結(jié)合燈具的空間坐標(biāo)位置，即可測(cè)算出人員定位標(biāo)簽卡的位置范圍。

3.2 應(yīng)用效果

在大柳塔選煤廠某空間場(chǎng)景布置8臺(tái)UWB基站，UWB定位采用3D定位方式，定位基站選用2主站和6從站，以主基站為坐標(biāo)原點(diǎn)建立X，Y，Z坐標(biāo)軸，當(dāng)人員攜帶定位標(biāo)簽卡進(jìn)入基站覆蓋范圍內(nèi)，定位標(biāo)簽卡將發(fā)送數(shù)據(jù)通過(guò)基站傳輸至上位機(jī)解算定位標(biāo)簽卡實(shí)際位置。圖5為大柳塔選煤廠基于卡爾曼濾波算法的UWB+IMU組合精確定位系統(tǒng)的精確度圖譜。

由圖5可以看出，采用UWB+IMU組合精確定位算法的定位效果比單獨(dú)UWB最小二乘法解算要好。單獨(dú)UWB解算無(wú)論怎么平滑，依然保持較嚴(yán)重的滯后性，出現(xiàn)更多的毛刺和偏離，無(wú)法與融合算法相比。基于卡爾曼濾波算法融合后將初始測(cè)量值分為實(shí)際值和預(yù)測(cè)值，最終繪制出的定位軌跡很好地避免了數(shù)據(jù)毛刺，定位軌跡與真實(shí)軌跡基本吻合，實(shí)現(xiàn)了更為精準(zhǔn)的定位。

圖5 UWB+IMU組合定位系統(tǒng)精確度圖譜Fig. 5 Spectral analysis of the accuracy of the positioning system

3.3 系統(tǒng)優(yōu)勢(shì)

(1)定位準(zhǔn)確。選煤廠內(nèi)定位人員攜帶定位卡出現(xiàn)在燈具或者定位設(shè)備周?chē)鷷r(shí)，多個(gè)燈具及定位設(shè)備可分別測(cè)算出該定位標(biāo)簽卡與自身的距離，并將原始數(shù)據(jù)進(jìn)行噪聲剔除、卡爾曼濾波算法、初步測(cè)距算法等融合處理，將人員定位卡信息及距離信息上傳至后臺(tái)服務(wù)器端，甚至僅需要較少的基站(≥2)即可實(shí)現(xiàn)定位。

(2)高效響應(yīng)。利用AI算法并結(jié)合選煤廠廠區(qū)內(nèi)各燈具的具體位置及高度，構(gòu)建三維可視化模型并生成坐標(biāo)系統(tǒng)，同時(shí)考慮各種數(shù)據(jù)的濾波及篩選結(jié)果，以達(dá)到更高效的計(jì)算響應(yīng)時(shí)間。

(3)可與其他設(shè)備實(shí)現(xiàn)聯(lián)動(dòng)。服務(wù)器后臺(tái)通過(guò)空間位置的核心算法，結(jié)合燈具的空間坐標(biāo)，測(cè)算出人員定位標(biāo)簽卡的位置范圍，同時(shí)可以與照明燈具、各類環(huán)境監(jiān)測(cè)傳感器、主要機(jī)電設(shè)備保護(hù)裝置互聯(lián)互通，實(shí)現(xiàn)選煤廠內(nèi)“人來(lái)燈亮，人走燈滅”。

4 結(jié)語(yǔ)

基于卡爾曼濾波算法的UWB+IMU組合精確定位系統(tǒng)，可以解決目前由于生產(chǎn)環(huán)境遮擋、屏蔽、電磁干擾等現(xiàn)象影響人員位置實(shí)時(shí)計(jì)算過(guò)程中位置不斷變化、定位不準(zhǔn)確的問(wèn)題，基本不會(huì)出現(xiàn)較大數(shù)據(jù)誤差，還原了人員精準(zhǔn)定位的誤差軌跡。同時(shí)由于智能定位模塊的適當(dāng)增加，基本解決了人員數(shù)量計(jì)算總數(shù)與選煤廠內(nèi)實(shí)際人員數(shù)量不一致的現(xiàn)象，系統(tǒng)數(shù)據(jù)真實(shí)可靠。

現(xiàn)階段我國(guó)智能選煤廠建設(shè)起步時(shí)間較短,智能選煤廠中人員精準(zhǔn)定位系統(tǒng)技術(shù)仍有不足，由于主廠房、篩分破碎車(chē)間等建筑內(nèi)的大型設(shè)備立體空間分布復(fù)雜，眾多運(yùn)行的變頻電氣設(shè)備存在大量諧波及電磁干擾，如何有效地提高選煤廠人員定位的定位精度和準(zhǔn)確度的要求十分迫切。選煤廠人員定位系統(tǒng)是保證安全生產(chǎn)的重要環(huán)節(jié)，高效、精準(zhǔn)的人員定位系統(tǒng)將會(huì)給生產(chǎn)管理工作帶來(lái)極大的便利，進(jìn)一步保障現(xiàn)場(chǎng)工作人員的生命安全。精準(zhǔn)的選煤廠人員定位系統(tǒng)將作為智能選煤廠的一個(gè)重要子系統(tǒng)，在安全管理方面起到重要作用。