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

白怡明，曾祥玉，李 杰，辛鳳陽，郭曉松，朱金龍

(1.中煤科工集團沈陽設計研究院有限公司，遼寧 沈陽 110015；2.內蒙古錫林河煤化工有限責任公司，內蒙古 錫林郭勒 026324)

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

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

為提高定位系統數據的準確性，開發了基于卡爾曼濾波算法的選煤廠UWB+IMU組合精確定位系統，并在大柳塔選煤廠中測試應用。

1 選煤廠人員精確定位技術

當前我國智能化選煤廠的建設，對人員定位的安全和精準度要求越來越高[21]，大多數定位系統的功能已不能滿足選煤廠智能化建設發展的需求[22-23]，因此需要加大對選煤廠室內定位功能的開發和設計，不斷完善人員定位管理系統的監控功能，以及完善人員監管的有效性、準確性，從而降低事故發生率與人員傷亡率[24-25]。選煤廠現階段應用最多的定位技術有藍牙、Wi-Fi、精準ZigBee定位技術、RFID定位技術、UWB定位技術[26]等。定位技術對比見表1。

表1 定位技術對比Table 1 Comparison of positioning techniques currently available

2 UWB+IMU組合精確定位系統

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

2.1 模型建立

卡爾曼濾波算法是一種典型濾波算法，是一種積累歷史數據，更新數據預測值的迭代過程。濾波的作用就是減少噪聲誤差與干擾對數據測量的影響。

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

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

建立卡爾曼濾波算法模型分為兩部分：第一部分就是卡爾曼濾波算法的狀態方程和觀測方程，它是一個預測數據實時更新的過程；第二部分是卡爾曼濾波算法分析值迭代過程，每個節點默認僅有自身實際的變量指標，沒有其他控制變量存在，這個算法模型可以幫助UWB精確定位系統提高整個系統的響應時間。在該處的初始控制量是0，初始測量值1，將初始測量值分為實際值0.5和預測值0.5，也就是用卡爾曼濾波算法來預測UWB精確定位系統中的距離變化時，將默認為不變化，預測數據實時更新的變化值就是1，由于距離變化是可以直接測量的，因此測量矩陣變化值也是1。

卡爾曼濾波算法模型的基本公式參數較多，但在UWB精確定位系統中有大量參數都在自我不斷地迭代更新，整個UWB精確定位系統真正受外界調整的參數只有三個，而系統開始循環的初始值，需要根據選煤廠的實際情況人為定義，其他影響UWB精確定位系統精準度的兩個參數是過程誤差和測量誤差的協方差。在卡爾曼濾波算法模型測試中，給定一個真實的選煤廠UWB精確定位系統數據，通過對真實的系統數據進行迭代更新，再加上測量誤差噪聲，可同時檢測系統實際值、測量值還有卡爾曼預測估值。卡爾曼濾波算法迭代模型如圖2所示。

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

在卡爾曼濾波算法模型的實際測試中，得到了大量過程誤差和測量誤差，這些誤差的協方差是不可直接測量的，是存在于UWB精確定位系統本身的，但誤差數據仍存在一定關聯性，因此在實際模擬過程中發現，卡爾曼濾波算法模型中即便存在一個對誤差的協方差有不準確的判斷時，也能對系統的真實輸出數據有一個較好的預測。

2.2 精確定位

定位標簽行走路線為已固定坐標為圓心、半徑為2.5 m的圓，在無遮擋的定位區域中心，UWB精確定位精度基本上比較可靠，而在非視距環境下的定位試驗中，混凝土柱子、較大的金屬物體對定位精度影響較大。此外，人體對UWB信號的阻擋也十分嚴重，單UWB精確定位系統位置誤差較大并且部分數據丟失，而在無遮擋區域定位精度相對較高且比較穩定。UWB+IMU組合精確定位系統通過更新擴展卡爾曼濾波算法可更新NLOS誤差值并獲得更準確的估算位置，從而實現在NLOS影響下的高精度定位。在選煤廠復雜環境的定位系統中，普通UWB基站測出的距離數據，可能因遮擋、多徑干擾導致定位誤差較大，有的地方達到100 cm左右，而采用基于卡爾曼濾波算法進行數據預處理后，數據得到較大的優化，定位精度可以提升至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 系統組成

基于卡爾曼濾波算法的UWB+IMU組合精確定位系統由人員定位卡、UWB定位基站、定位引擎、系統軟件等組成。人員定位卡與現場的UWB定位基站之間分別測距，基站將與定位卡之間的定位距離傳輸給定位引擎，定位引擎內運行定位算法，將計算出的位置數據輸送給平臺軟件進行位置展示。卡爾曼濾波算法運行在定位引擎內部，定位基站傳輸的原始數據先經過濾波算法處理后，再進行定位算法計算，能顯著提升定位系統精度。

3 UWB+IMU組合精確定位系統的應用

3.1 系統網絡拓撲結構

大柳塔選煤廠UWB+IMU組合精確定位系統網絡拓撲結構如圖4所示。該系統由服務器、路由器、交換機、光纖環網、集中控制器、智能定位模塊、定位標簽卡組成。服務器配有定位精度解析軟件及Web發布軟件，交換機用于連接各個集中控制器，智能定位模塊、定位標簽卡均通過無線網絡全覆蓋來實現通信。

圖4 UWB+IMU組合精確定位系統網絡拓撲圖Fig. 4 Network topology structure of the UWB+IMU integrated accurate personnel positioning system

根據UWB信號傳輸的特點，一般情況下，智能定位模塊天線覆蓋的定位范圍在30 m左右，由于選煤廠復雜的現場作業環境，在大柳塔選煤廠內智能定位模塊距離考慮在15～20 m左右(安裝在智能照明燈具上)。與定位相關的主要決定因素是UWB定位卡與UWB定位基站之間距離的準確程度，以及定位數據的連續性。當人員攜帶定位標簽卡出現在智能定位模塊(智能燈具)周圍時，多個智能定位模塊可分別測算出該定位標簽卡與自身的距離，并將原始數據進行噪聲誤差剔除、初步測距算法等處理，然后將人員定位標簽卡信息及距離信息上傳至服務器端；在服務器端，通過卡爾曼濾波算法模型過濾，并結合選煤廠各燈具的具體位置及高度，構建參與運算的坐標系統，結合燈具的空間坐標位置，即可測算出人員定位標簽卡的位置范圍。

3.2 應用效果

在大柳塔選煤廠某空間場景布置8臺UWB基站，UWB定位采用3D定位方式，定位基站選用2主站和6從站，以主基站為坐標原點建立X，Y，Z坐標軸，當人員攜帶定位標簽卡進入基站覆蓋范圍內，定位標簽卡將發送數據通過基站傳輸至上位機解算定位標簽卡實際位置。圖5為大柳塔選煤廠基于卡爾曼濾波算法的UWB+IMU組合精確定位系統的精確度圖譜。

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

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

3.3 系統優勢

(1)定位準確。選煤廠內定位人員攜帶定位卡出現在燈具或者定位設備周圍時，多個燈具及定位設備可分別測算出該定位標簽卡與自身的距離，并將原始數據進行噪聲剔除、卡爾曼濾波算法、初步測距算法等融合處理，將人員定位卡信息及距離信息上傳至后臺服務器端，甚至僅需要較少的基站(≥2)即可實現定位。

(2)高效響應。利用AI算法并結合選煤廠廠區內各燈具的具體位置及高度，構建三維可視化模型并生成坐標系統，同時考慮各種數據的濾波及篩選結果，以達到更高效的計算響應時間。

(3)可與其他設備實現聯動。服務器后臺通過空間位置的核心算法，結合燈具的空間坐標，測算出人員定位標簽卡的位置范圍，同時可以與照明燈具、各類環境監測傳感器、主要機電設備保護裝置互聯互通，實現選煤廠內“人來燈亮，人走燈滅”。

4 結語

基于卡爾曼濾波算法的UWB+IMU組合精確定位系統，可以解決目前由于生產環境遮擋、屏蔽、電磁干擾等現象影響人員位置實時計算過程中位置不斷變化、定位不準確的問題，基本不會出現較大數據誤差，還原了人員精準定位的誤差軌跡。同時由于智能定位模塊的適當增加，基本解決了人員數量計算總數與選煤廠內實際人員數量不一致的現象，系統數據真實可靠。

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