張 鵬

（1.中煤科工集團重慶研究院有限公司，重慶 400039；2.瓦斯災害監(jiān)控與應急技術國家重點實驗室，重慶 400037）

在《煤礦安全規(guī)程》和《AQ 1029—2019 煤礦安全監(jiān)控系統(tǒng)及檢測儀器使用管理規(guī)范》等安全生產(chǎn)行業(yè)標準中，煤礦井下各類傳感檢測設備及監(jiān)測監(jiān)控設備在井下安裝位置和方位都有明確要求[1-2]。目前，煤礦井下安全檢測設備種類繁多，安裝位置錯綜復雜。然而在實際安裝過程中，部分工作人員出于某種原因，不按規(guī)定位置安裝或私自移動傳感檢測設備，導致地面顯示結(jié)果和實際井下情況不符?，F(xiàn)有的煤礦安全監(jiān)控系統(tǒng)只能檢測設備工作狀態(tài)和通信狀態(tài)，無法獲取設備安裝的具體位置和方位[3]。針對上述情況，目前國內(nèi)外對于設備的自主位姿監(jiān)測還沒有具體的解決方案。通常采用被動監(jiān)控方式對井下設備監(jiān)測，如視頻監(jiān)控或者人工巡檢。視頻監(jiān)控是在特殊設備旁安裝攝像頭，實時查看設備，但這種方式存在成本高，功耗大和受環(huán)境影響嚴重等缺點；人工巡檢是指派工作人員定期下井檢查設備位置，但這種方式存在人力成本高，檢測能力參差不齊，耗時久、盲區(qū)多、主觀判斷為主、隱性問題多等缺點。為此，設計了一種用于煤礦井下自主監(jiān)測設備位姿的傳感器，將UWB 定位數(shù)據(jù)、超聲波測距數(shù)據(jù)和慣性測量單元的姿態(tài)數(shù)據(jù)相融合，實現(xiàn)對關鍵設備的實時位姿監(jiān)測。

1 位姿監(jiān)測傳感器原理及硬件

傳感器設計原理框圖如圖1。

圖1 傳感器設計原理框圖Fig.1 Block diagram of sensor

位姿自監(jiān)測傳感器由計算控制模塊，多源數(shù)據(jù)模塊和交互接口模塊3 部分組成。計算控制模塊是核心部分，負責采集多源數(shù)據(jù)模塊信息后，進行異構(gòu)數(shù)據(jù)融合定位和姿態(tài)解算；多源數(shù)據(jù)模塊為計算控制模塊提供原始數(shù)據(jù)來源，以UWB 時間數(shù)據(jù)、超聲波測距數(shù)據(jù)、慣性測量單元（IMU）中三軸加速度計和陀螺儀數(shù)據(jù)為主；交互接口模塊用于設置傳感器信息，如配置設備初始位姿和報警閾值，同時也可通過該模塊獲取設備實時位姿信息，該模塊提供串口，網(wǎng)口和WIFI3 種通信方式。

1.1 計算控制模塊

采用意法半導體公司設計的32 位微控制器STM32F103C8T6。這款控制器主頻高達72MHz，內(nèi)置20K 的高速SRAM 和64K Flash，滿足運行過程中的位姿計算實時性要求和數(shù)據(jù)存儲要求[4]。同時，該款處理器包含多種通信接口：2 路IIC，2 路SPI，3 路串口和1 路CAN 總線。STM32F103C8T6 可以根據(jù)工作環(huán)境需求設置不同的低功耗工作模式，符合煤礦井下特殊環(huán)境對低功耗的要求。

1.2 多源數(shù)據(jù)模塊

多源數(shù)據(jù)由3 種數(shù)據(jù)組成：UWB 芯片中獲得的基站與傳感器間的時間戳數(shù)據(jù)，超聲波模塊得到的測距數(shù)據(jù)，慣性測量單元中獲取的三軸加速度和三軸陀螺儀在xyz 方向的原始數(shù)據(jù)。UWB 硬件設計原理圖如圖2。

圖2 UWB 硬件設計原理圖Fig.2 UWB hardware design schematic

UWB定位部分采用DecaWave公司生產(chǎn)的DW1000芯片，符合IEEE802.15.4-2011 超寬帶標準[5]。數(shù)據(jù)傳輸速率有110 kbit/s、850 kbit/s 和6.8 Mbit/s，信號可選擇頻段多，盡可能降低信號間干擾，滿足煤礦井下實時，低功耗，高精度的定位要求，理論上在視距環(huán)境下其通信距離與與信號發(fā)射強度相關，最遠可達600 m，定位精度可達10 cm[6-7]。

為了提高UWB 定位模塊在井下特殊環(huán)境的通信質(zhì)量，在射頻輸出端加入亞德諾公司的MHC326功率放大器，實現(xiàn)遠距離傳輸；為了進一步降低系統(tǒng)功耗，通過引腳控制射頻開關，只有在發(fā)送和接收時才開啟功率放大器。同時，使用1.8 V 低壓為DW1000 芯片供電，當DW1000 芯片測距工作結(jié)束后，進入休眠期時，停用內(nèi)部供電，從而更加節(jié)能，降低功耗。U6 為收發(fā)信號反向器，完成差分信號到單端射頻信號的轉(zhuǎn)換。

IMU 硬件設計原理圖如圖3。IMU 采用博世公司的超低功耗智能慣性測量單元BMI270[8]，16 位數(shù)字加速度計有±2g、±4g、±8g、±16g 4 種量程；16 位數(shù)字陀螺儀有±125、±250、±500、±1 000、±2 000 dps 5種范圍[9]，當芯片運行在超低功耗模式下，僅耗費30 μA。BMI270 內(nèi)置陀螺儀自校準技術有靜止組件重新調(diào)整（CRT）功能，可補償MEMS 焊接漂移。

圖3 IMU 硬件設計原理圖Fig.3 IMU hardware design schematic

1.3 交互接口模塊

該模塊主要用于傳感器與外界的數(shù)據(jù)交互，提供多種通信方式?？赏ㄟ^串口、網(wǎng)口和WIFI 可以設置傳感器的初始化參數(shù)、動態(tài)位姿報警閾值、自身靜態(tài)IP、端口和讀取實時位姿等信息，也可以通過這3種接口實時獲取當前設備的位姿信息用于其他功能。顯示部分采用0.96 寸OLED 顯示屏，用于實時顯示位姿數(shù)據(jù)和傳感器參數(shù)。當安裝位置不符合預期設定或者人為私自更改安裝位置，且大于設定閾值時，自動聲光報警提示。

2 位姿監(jiān)測傳感器軟件

軟件上綜合考慮了開發(fā)難度、位姿響應實時性和傳感器低功耗等方面影響因素，底層微控制器端移植了開源的FreeRTOS 嵌入式操作系統(tǒng)，將整個軟件進行多任務劃分。主要有以下核心任務：初始化任務、多源融合定位任務和IMU 姿態(tài)解算任務。

2.1 初始化任務

初始化程序流程圖如圖4。

圖4 初始化程序流程圖Fig.4 Initialization program flow chart

傳感器上電后，首先對微控制器硬件進行初始化，包括定時器、IO 輸入輸出口、O 顯示器、DW1000芯片初始化，BMI270 初始化，然后是操作系統(tǒng)初始化，創(chuàng)建系統(tǒng)子任務并等待運行。完成初始化后，UWB 芯片、IMU 和超聲波模塊都進入休眠狀態(tài)，隨時等待被喚醒。

2.2 多源融合定位任務

微控制器使用定時器，每隔10 s 喚醒DW1000芯片進行定位。煤礦井下巷道大部分都為狹窄且修長的空間，基于TOF 的一維定位算法完全滿足實際定位需求。但TOF 定位只能得到距離信息，無法獲得傳感器所在空間的高度信息，因此利用朝下的超聲波模塊，實現(xiàn)高度位置獲取，UWB 定位數(shù)據(jù)和超聲波定高數(shù)據(jù)結(jié)合，進而實現(xiàn)空間定位。一維定位算法采用雙向雙邊測距算法，經(jīng)過測量3 次無線電信號在空間中的發(fā)送與接收時間戳進行距離計算。此測距方法不需要進行2 個節(jié)點的時間同步，便可消除2 個節(jié)點間的時基不統(tǒng)一誤差，達到厘米級測距精度。雙向雙邊測距法流程如圖5。

圖5 雙向雙邊測距流程圖Fig.5 Schematic diagram of SD-TWR

UWB 節(jié)點A 和UWB 節(jié)點B 間的距離D 為：

式中：TSP為請求測距節(jié)點主動發(fā)送無線信號時的時間戳；TRP為接收節(jié)點接收到無線信號時的時間戳；TSR為當前接收節(jié)點的響應時間戳；TRR即為對應的接收當前響應信號的時間戳；TSF、TRF分別為數(shù)據(jù)幀的發(fā)送和接收的時間戳；c 為光速。

2.3 姿態(tài)解算

姿態(tài)解算程序流程圖如圖6。

圖6 姿態(tài)解算程序流程圖Fig.6 Flow chart of attitude solution program

定時器同時觸發(fā)UWB 測距和姿態(tài)解算。首先，通過IIC 采集IMU 的3 軸加速度計和3 軸陀螺儀原始數(shù)據(jù)，由于陀螺儀固有特性和溫度的影響，工作時間越長，產(chǎn)生的累計誤差越大，而加速度計靜態(tài)穩(wěn)定性好，但動態(tài)響應數(shù)據(jù)不可靠，因此通過低通濾波器過濾加速度計高頻噪聲和高通濾波器過濾陀螺儀低頻噪聲，抑制漂移。然后將陀螺儀積分所得的角度和加速度計算的角度進行融合，形成互補濾波算法[10]。互補濾波算法是根據(jù)不同傳感器的特性，通過高低通濾波器形成互補，對不同的數(shù)據(jù)來源進行權(quán)重加權(quán)求和計算融合數(shù)據(jù)。隨后利用四元素微分方程對慣性測量單元進行初步位姿求解。最后再利用卡爾曼濾波算法抑制測量噪點，對初始位姿再進一步進行數(shù)據(jù)精確化，提高姿態(tài)角精度，得到最終IMU 的姿態(tài)角[11]。

3 結(jié) 語

針對煤礦井下對關鍵設備位姿自監(jiān)測的需求，設計了一款多源數(shù)據(jù)融合的位姿傳感器。借助UWB技術完成一維定位，再結(jié)合超聲波測高數(shù)據(jù)和IMU姿態(tài)解算數(shù)據(jù)進而計算出井下設備空間位姿。該傳感器與現(xiàn)有設備解耦，體積小巧，方便安裝，當井下機電設備被隨意移動或者安裝不符合規(guī)程時，能及時向上反饋，避免因設備性能降低或功能缺失造成的安全隱患問題。