礦用智能傳感器自主定位研究

柏思忠

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

隨著工業4.0、工業互聯網、人工智能系統、物聯網等相關技術的發展，對于各類傳感器的應用需求愈發增大，例如我國提出的《中國制造2025））和德國政府推出的《Hightech-Strategie 2020 für Deutschland》都將智能傳感器系統（Smart Sensors System）列入戰略發展領域[1]。GB/T 34069—2017《物聯網總體技術 智能傳感器特性與分類》[2]明確了智能傳感器定義和特性，其中智能傳感器的物聯網特性要求能自動向外部發出信息描述自己的位置，在物聯網中能自動識別自身在網絡中的位置。GB/T 33900—2017《工業物聯網儀表應用屬性協議》[3]規定工業物聯網儀表位置屬性是指工業物聯網儀表的安裝位置，是經緯度確定的絕對位置、海拔高度以及相對位置信息。

礦用智能傳感器工作在井下巷道，無法接收地面衛星信號，位置信息必須依靠室內定位技術。張勝利等[4]在室內定位方法和技術綜述中闡述了WiFi、地磁、慣導、Zigbee、藍牙、RFID、紅外、超聲波、UWB等主流室內定位技術的優缺點；胡青松等[5]在礦井動目標定位：挑戰、現狀與趨勢中重點從原理、方法和算法上闡述了WiFi、Zigbee、藍牙、RFID、紅外、超聲波、UWB 等主流無線定位技術需要外圍預裝設備配合才能實現定位，預裝設備增加了成本和不確定性；另外采用地磁數據庫比對方式的地磁定位易受設備布局和環境因素變化影響；最后單一慣導存在累積誤差的影響，隨著時間和距離的增加會增大。因此，現有的單一定位技術無法滿足智能傳感器定位的要求。為此，針礦用智能傳感器地面和井下定位的問題，采用地磁輔助慣導/北斗（BDS）的組合定位模式[6]，全部國產化芯片方案，研制出低功耗的自主定位模塊，并將自主定位模塊帶載到現有高低體積分數甲烷傳感器實現傳感器自主定位功能。

1 地磁輔助慣導/北斗組合定位

1.1 地磁輔助的慣導/北斗組合定位原理

地磁輔助慣導/北斗組合定位原理框圖如圖1。

圖1 組合定位原理框圖Fig.1 Block diagram of combined positioning principle

組合定位在地面以北斗定位為主，沒有衛星信號的地方慣導[7]輔助定位，煤礦井下以慣導為主，地磁輔助定位[8]。慣導系統是利用MEMS 慣性測量單元（MEMS IMU）測量載體的加速度，并在給定運動初始條件下，由MCU 算出傳感器的速度、距離和相對位置，由陀螺儀測量傳感器的角速率，并經轉換、處理，輸出傳感器的姿態。由于慣性器件存在著累計誤差，解算出來的姿態信息并不準確，所以利用地磁對其進行輔助，在特定地點進行標校消除系統累計誤差[9]。

1.2 慣導姿態更新算法

根據慣性器件的輸出實時計算出傳感器的姿態矩陣，姿態計算采用四元數法[10-11]，根據一段時間（k時刻到k+1 時刻）內的角度變化量來解算出最新時刻的四元數，如式（1）：

式中：Q 為姿態四元數，由1 個實部 和3 個虛部構成，Q＝q0＋q1i q1＋q2i q2＋q3i q3；tk為k 時刻；tk+1為（k＋1）時刻；ωx為陀螺儀x 軸角速率；ωy為陀螺儀y軸角速率；ωz為陀螺儀z 軸角速率；t 為時間。

對于固定輸出頻率的陀螺儀而言，計算精度受到陀螺儀輸出頻率的影響，輸出頻率越高，計算精度越高，考慮實際硬件計算能力和需求情況，陀螺儀數據更新頻率選取0.01 s。當得知姿態四元數時，根據四元數與姿態角之間的關系計算出姿態角[12]，根據陀螺儀實時輸出不斷更新姿態。

式中：θ 為俯仰角；γ 為橫滾角；ψ 為航向角。

1.3 慣導速度更新算法

取當地地理坐標系（東北天-ENU）作為導航坐標系，在從kT 時刻到（k+1）T 時刻的1 個周期T 內，由東北天方向的速度微分方程[1]可以得到東向、北向和天向速度的更新方程：

式中：VE為東向速度；VN為北向速度；VU為天向速度；fE為東向加速度；fN為北向加速度；fU為天向加速度；ωie為位置速率；L 為經度；RN為卯酉圈的曲率半徑；RM為子午圈的曲率半徑；H 為高度；g0為重力加速度；T 為更新時間周期。

1.4 慣導位置更新算法

在從kT 時刻到（k+1）T 時刻的1 個周期T 內，傳感器的經度L 遞推更新方程、緯度B 遞推更新方程、高度H 遞推更新方程如式（6）～式（8）：

1.5 地磁輔助定位算法

在載體靜態或者勻速運動的狀態下，單獨利用加速度計只能得到俯仰角和橫滾角，航向角信息需要通過三軸磁強計輔助定位計算[13]，通過預測礦井定位區域內巷道分叉和轉折點的磁場強度作為慣導信息的標定點，分段調整方向余弦陣，消除慣導累計誤差。

式中：ψ 為航向角；θ 為俯仰角；M 為本地磁場強度；η 為磁傾角；mx為磁強強度x 軸分量。

1.6 北斗定位算法

2 自主定位模塊構成及電路

自主定位模塊包括中央微處理器（MCU）、慣性導航、北斗導航、數據存儲、數據接口、電平轉換、電源轉化和外部接口等電路。在地面北斗衛星接收信號正常時以北斗定位為主，沒有衛星信號的地方以慣導輔助定位；煤礦井下以慣導為主，地磁輔助定位，以此實現智能傳感器地磁輔助慣導/北斗組合定位功能。自主定位模塊構成框圖如圖2，自主定位模塊電路圖如圖3。

圖2 自主定位模塊構成框圖Fig.2 Principle block diagram of autonomous positioning module

圖3 自主定位模塊電路圖Fig.3 Circuit diagram of autonomous positioning module

3 自主定位模塊關鍵技術

整個模塊硬件采用低功耗和國產化設計，軟件采用了零速修正算法、四元數法、地磁輔助修正算法，實現了智能傳感器移動軌跡存儲和反演。

3.1 模塊硬件低功耗和國產化

MCU 選擇超低功耗HC32L130，0.9 μA @3 V深度休眠模式+ RTC 工作；130 μA/MHz@ 3V@24 MHz 工作模式：CPU 和外設運行，從Flash 運行程序；4 μs 超低功耗喚醒時間，使模式切換更加靈活高效。

MEMS IMU 采用9 軸芯片ICM-20948，3 軸加速度、3 軸陀螺儀和3 軸磁強計，1.8 V 供電，僅需2.5 mW，是目前世界上功耗最低的9 軸慣導芯片。

北斗定位采用低功耗一體化模組TAU1103，工作時3.3 V 供電35 mA，待機模式電流僅需14 μA。

數據存儲Flash 選用低功耗MX25L128，3.3 V供電最大電流25 mA，休眠狀態電流不超過20 μA。

自主定位模塊3.3 V 供電時，工作電流小于80 mA，休眠狀態小于2 mA；外接USB 采用5 V 供電時，工作電流小于100 mA，休眠狀態小于3 mA。

所有芯片選擇都遵循國產化原則，全部國產化，避免“卡脖子”問題出現。自主定位模塊所有芯片和廠家見表1。

表1 模塊芯片生產廠家統計表Table 1 Total table of module chip manufacturers

3.2 關鍵算法及軟件設計

關鍵算法包括開機零速修正、四元數法移動過程姿態角和位置更新、地磁輔助修正轉彎點航向角3 部分。

零速修正是指智能傳感器上電開機后，靜止一段時間后將速度計算結果清零，達到修正速度誤差的目的。為充分利用開機靜止檢測結果，要求智能傳感器在移動前的起始點靜止30 s 以上，并且將該點建立為平面內相對移動的坐標原點O（0，0）。

四元數法時陀螺儀輸出的3 軸角速度數據經過卡爾曼濾波后代入式（1）不斷更新四元數，更新后的四元數代入式（2）得到實時姿態角；慣性導航輸出的3 軸加速度數據經過式（3）～式（8）轉換為速度更新再到位置更新。

地磁輔助修正是根據實時輸出的3 軸地磁數據，當智能傳感器在直線運動或有一定弧度的巷道移動時，單位長度內航向角變化不超過閾值（＜15°/m）時，地磁數據不參與導航參數航向角修正，智能傳感器移動到礦井定位區域內巷道分叉和轉折點等這些地磁標定點時，航向角變化超過閾值，該點的航向角如式（9）以地磁數據為準進行修正，實現地磁輔助定位功能。

結合3 部分算法，智能傳感器自主定位軟件流程圖如圖4。

圖4 軟件流程圖Fig.4 Software flow chart

當系統開機時，首先將煤礦井下巷道標定點地磁位置數據下載到存儲單元中，再配置慣性導航參數，讀取3 軸陀螺儀、3 軸加速度和3 軸地磁數據，完成初始化；然后將傳感器上電開機，在起始點靜止30s 以上，完成零速修正。實時判斷衛星信號有效性，能有效接收北斗衛星信號，直接將北斗定位數據作為當前坐標和位置更新數據；不能有效接收衛星數據時，以慣性導航為主、地磁校準點比對輔助，更新姿態角和位置信息，更新后的姿態角和位置信息一邊輸出一邊存儲，單次處理完畢后重復上述主循環操作，實現智能傳感器實時更新自主定位信息。

4 試驗驗證

將自主定位模塊搭載到高低體積分數甲烷傳感器內部，并備有蓄電池可供持續工作8 h 以上。在地面全長376 m 某環形道路上，將環形道的地圖對應彎道處設置標定點A（0，43）、B（－115，46）、C（－116，－24）、D（0，－25）。生成的地磁矩陣數據如式（11），將其下載到傳感器。

提著傳感器開機后靜止30 s，出發移動1 圈，回到起點后靜止5 s，測試結果移動軌跡和實際環道吻合程度較高，終點和起點相差約2 m，航向偏差約2°，整個移動距離375.01 m，與參考距離378 m 相差2.99 m，相對誤差0.8%。

5 結 語

針對智能傳感器物聯網特性要求，采用地磁輔助的慣導/北斗組合定位算法，研制了智能傳感器自主定位模塊。智能傳感器搭載自主定位模塊可完成移動軌跡存儲和反演，在網傳感器可實時更新位置、速度和姿態信息，離線傳感器可在恢復網絡后補傳定位信息反演移動軌跡，實現了智能傳感器物聯網特性要求能自動向外部發出或自動識別自己的位置信息，智能傳感器自主定位具有以下4 個方面優點：①依靠自身測量信息進行連續定位；②不需接收外部信息，不受外界干擾；③不向外部輻射能量，具有隱蔽性；④同時確定傳感器位置、速度和姿態信息。但在實際應用中，智能傳感器還需要解決移動中供電和通信問題，才能進一步實用。