張 鵬

（1.中煤科工集團(tuán)重慶研究院有限公司，重慶 400039；2.瓦斯災(zāi)害監(jiān)控與應(yīng)急技術(shù)國家重點(diǎn)實(shí)驗(yàn)室，重慶 400037）

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

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

傳感器設(shè)計(jì)原理框圖如圖1。

圖1 傳感器設(shè)計(jì)原理框圖Fig.1 Block diagram of sensor

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

1.1 計(jì)算控制模塊

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

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

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

圖2 UWB 硬件設(shè)計(jì)原理圖Fig.2 UWB hardware design schematic

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

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

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

圖3 IMU 硬件設(shè)計(jì)原理圖Fig.3 IMU hardware design schematic

1.3 交互接口模塊

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

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

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

2.1 初始化任務(wù)

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

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

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

2.2 多源融合定位任務(wù)

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

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

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

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

2.3 姿態(tài)解算

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

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

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

3 結(jié) 語

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