基于微型慣性傳感器的井下人員跟蹤定位系統*

康俊瑄

（中國礦業大學（北京），北京 100083）

0 引言

我國煤炭儲量豐富，煤炭資源在我國能源消費結構中占據主導地位，根據國家能源局的統計數據，2020年我國煤炭消費量占能源消費總量的56.8%。煤礦開采環境錯綜復雜，嚴重制約著煤礦的安全生產[1]。近年來，國家對煤礦挖掘的生產安全提出了更嚴格的要求[2]，經過多年整治改善，我國煤礦的事故死亡人數從2010年的1 973 人下降至2020年的225 人。然而，目前我國與國外發達國家在煤礦智能化領域的研究仍然存在著較大差距，重大安全隱患的智能監測、預測、預警等技術相對落后。為了提高煤礦智能化水平，在煤礦安全事故發生后能夠快速有效地對失聯工作人員進行定位搜尋，本文圍繞煤礦井下人員定位系統開展研究。

大多數現代行人跟蹤系統都采用全球定位系統（global positioning system，GPS），然而，典型的行人環境往往會扭曲和阻擋衛星信號，從而使GPS 通信系統變得不精確[3]。對于室內和地下礦井的應用場景尤其如此，當行人軌跡跟蹤系統在煤礦井下工作時，接收不到GPS信號，造成GPS 導航系統無法應用于地下環境。目前室內空間應用主要包括WIFI定位技術和UWB（ultra wide band，超寬帶）定位技術，其中WIFI定位技術具有傳輸距離遠、使用方便等優點，然而WIFI定位覆蓋范圍有限，只能在小空間下應用使用，而且會受到其他信號干擾，影響精度[4-5]。根據文獻[6]，采用單獨的WIFI定位技術進行室內定位，會造成1～2 m的運動誤差，而且場景環境較簡單，實際地下應用中環境復雜，定位精度可能會進一步下降。同時WIFI定位需要依賴較多的AP（access point，無線訪問接入點），在地下礦井搭建WIFI定位系統較為不便；Wang等[7]使用攝像機和深度學習方法對靜止和運動中的行人進行跟蹤，實現多行人跟蹤器。近些年來，MEMS 慣性傳感器開始被大量應用于導航系統，李博文等[8]使用低成本微機電系統（microelectro-mechanical system，MEMS）慣性測量單元（inertal measure unit，IMU）來應對GPS 衛星信號受到遮擋，無法使用GPS 時的場景，同時通過使用Kalman 濾波算法將定位精度大幅提高；劉維等[9]研究了MEMS 慣性傳感器的系統誤差和環境干擾，研究了一種基于自適應無跡卡爾曼濾波的姿態算法，抑制MEMS 陀螺的姿態角發散；Zheng等[10]分析了3 種慣性傳感器的性能，通過研究慣性元素的典型誤差項和艾倫方差，對IMU傳感器的性能進行分析。

Bahillo等[11]使用腳裝慣性測量模塊作為行人定位系統的基礎，通過藍牙模塊遠程連接智能設備。當前應用于地下礦井的慣性定位算法主要是依賴于預測行人步長的行人航位推算（pedestrian dead reckoning，PDR）算法，采用這種算法需要對行人的補償進行精準預測[12]。本文采用慣性導航算法，通過含有陀螺儀、加速度計和磁力計的九軸傳感器計算行人的位姿，通過加速度計算求解位移，可避免不同的步幅造成的誤差。由于慣性傳感器采集數據存在隨機高斯噪聲，經過積分后獲得的姿態信息和速度位置信息會隨著工作時間增加，一直累積，最終造成大量誤差，因為傳感器噪聲造成的誤差在基于MEMS 傳感器的慣性導航系統中尤其明顯。因此，如何解決慣性軌跡測量算法中的長期誤差成為慣性導航系統的研究重點。對于地面和空中運載體的導航系統，可以使用GPS 定位系統，所以此前的研究是引入其余傳感器，如GPS 或車輪編碼器與慣性傳感器形成組合導航系統。但是應用于地下礦井的行人導航系統很難應用GPS 系統或編碼器，故本文提出并使用零速檢測技術，通過測量傳感器數據判斷行人運動狀態，利用IMU中的加速度計測量加速度具有長期穩定性的特點，在零速狀態下，加速度計測量數據為地球重力加速度的分解來對姿態進行修正，在行人運動過程中通過陀螺儀進行姿態解算。本文在相關的基于慣性系統的行人導航系統研究基礎上，將此類研究成果擴展應用在地下煤礦場景，以此進一步提升煤礦智能化水平。通過實驗證明，采用微型慣性傳感器的井下人員跟蹤定位系統可以滿足地下礦井中人員定位跟蹤的實際需求。

1 系統架構

1.1 系統硬件架構

為了實現本文提出的慣性軌跡測量算法，本文設計一個基于微傳感器的慣性軌跡測量標簽，如圖1所示，其大小為3 cm（寬） ×13 cm（長），具有較強的便攜性。

圖1 人員定位系統軌跡測量標簽Fig.1 Trajectory measurement label of personnel positioning system

軌跡測量標簽的系統硬件框圖如圖2所示，采用的實驗標簽由微控制器、慣性傳感器、無線傳輸模塊和電池組成。

圖2 軌跡測量標簽硬件框圖Fig.2 Hardware block diagr am of trajector y measurement label

軌跡測量標簽的慣性傳感器采用的是ADI公司的10 自由度MEMS 慣性傳感器ADIS16448，此傳感器包含1 個三軸陀螺儀、1 個三軸加速度計、1 個三軸磁力計以及1 個壓力傳感器。此傳感器能夠提供較為精確的姿態和速度信息，與復雜且昂貴的分離設計方案相比，ADIS16448 能夠為精確的多軸慣性檢測提供簡單且高效的方法。其在工廠生產環節就做過必要的運動測試及校準，可以有效縮短系統集成時間。ADIS16448 的陀螺儀最大測量范圍是±1 000°/s，加速度計測量范圍最大為±18 g，可以感受的人體運動和靜止狀態時的加速度。磁力計最大測量范圍為±1.9 gauss。

本標簽采用1 顆STM32L051 芯片，可滿足2 路SPI通信，且具有較低功耗，較低的功耗能夠保證人員長時間井下工作時仍能夠保持系統的運轉。標簽還配備了1個nRF24L01 模塊作為2.4G通信的模塊，以便于標簽可以遠程傳輸數據。

1.2 系統軟件架構

本文主要討論建立一個地下軌跡跟蹤系統，該系統能夠使用較為便攜的慣性測量單元，由于MEMS 慣性測量元件的自身特性和噪聲影響，在長時間工作狀態下其姿態和速度將會產生較大的誤差，所以本系統要求能夠校正對應的誤差，實現長期工作狀態下的軌跡精度。為了完成這項系統任務，本文設計的整套系統（如圖3所示）包含如下3 個模塊：

圖3 人員定位系統框圖Fig.3 Framework of personnel positioning system

1）零速檢測：使用低成本的MEMS 慣性傳感器進行軌跡測量的一個非常關鍵的技術就是零速檢測技術。當前最主要的檢測方法即為閾值測量法，此算法通過測量IMU的陀螺儀和加速度計的輸出，當此輸出值大于靜止狀態的閾值時，將此時看作運動狀態，否則認為當前狀態為靜止狀態[13]。

2）慣性軌跡測量算法：慣性軌跡測量算法主要包括2 部分：①其一為設備姿態估計，對行人軌跡跟蹤的性能很大程度上取決于準確跟蹤移動設備方向姿態能力。在嵌入低成本IMU傳感器的無約束設備環境中，這項任務尤其具有挑戰性。通常，陀螺儀漂移和磁場干擾，特別是在地下煤礦環境中，是準確估計方向的主要障礙。解決這個問題至關重要，因為慣性軌跡測量算法的幾乎所有任務都使用方向信息。②其二是對設備的速度更新算法。

3）零速狀態下軌跡誤差校正算法：在檢測到標簽處于零速狀態下后，本文使用零速校正算法修正因為陀螺儀和加速度計漂移引起的速度位移誤差。當零速檢測判定當前時刻的標簽速度為0 時，將通過慣性軌跡更新算法計算得到的速度當作觀測量送入觀測方程，使用卡爾曼濾波方程估計位移誤差。

2 軌跡測量算法

首先，定義坐標系，本文將慣性傳感器的正交坐標系看作載體坐標系（記為b系），將以方位坐標系當作導航坐標系（記為n 系）。由于人體在運動時，標簽會沿著3 個n 系坐標軸旋轉，所以b系和n 系不會是重合的，這中間就會存在姿態角，分別記為俯仰角（Pitch，記為θ，單位為rad/s），橫滾角（Roll，記為γ，單位為rad/s）和方位角（Yaw，記為ψ，單位為rad/s），從b系到n 系的姿態轉換矩陣可以表示為式（1）：

通過對姿態轉換矩陣進行更新，即可得到行人此時的方向，進而計算人員的位移軌跡。為了能夠進行更精確的軌跡測量，需要計算初始姿態，初始姿態的計算方法與靜態位姿計算一樣。

當通過零速狀態檢測器檢測到當前運動狀態為靜止時，此時IMU測量得到的加速度全部由重力加速度提供，因為地球重力加速度的方向不隨時間而改變。同樣具有此特性的還有地球的磁場，其磁力方向不隨時間變化，所以利用這2 個物理量，通過加速度計和磁力計檢測并計算，可以計算得到當前的位姿，如式（2）所示。

式中：abx，aby，abz為加速度計讀取得到的三軸加速度，單位為m2/s，通過式（2）建立了水平2 個姿態角（俯仰角和橫滾角）與加速度計輸出值之間的關系，通過式（2）可以進一步求得水平姿態角[13]，如式（3）所示：

方位角可以由如式（4）求得：

式中：Mbx，Mby，Mbz分別為磁力計三軸輸出值，單位為gauss。

當人員開始移動時，通過陀螺儀采集到角速度數據計算位姿，并通過加速度計采集加速度數據計算人員移動速度，進而求解人員軌跡。本文采用四元數法計算位姿更新，四元數法具有計算量小，奇異性小的特點，每1個位姿對應1 個四元數，所以大量應用于飛行器的位姿計算算法中。

四元數定義為式（5）～（6）：

將姿態角轉換為四元數為式（7）：

由陀螺儀輸出的角速度更新姿態的算法如式（8）～（9）所示：

式中：Qnb（m），Qnb（m-1）分別為當前時刻和上一時刻的四元數；Δθm為陀螺儀輸出角速度積分后得到的角增量，單位為°，通過這種方式完成在運動過程中的姿態更新。

3 零速校正

當檢測到標簽處于零速狀態時，可以采用零速校正算法對軌跡漂移誤差進行校正。標簽通過檢測慣性器件輸出的加速度和角速度數值的矢量和，將此矢量和與靜止狀態下的輸出值做對比，設定1 個閾值，若當前狀態的矢量和與靜止狀態下的矢量和之間的差距超過了這個閾值，則認為當前處于運動狀態，不對當前狀態的標簽進行零速校正；相反，若通過對比2 個矢量和，發現其插值小于閾值，則可以認為當前狀態為零速狀態。系統處于零速狀態時，可以認為當前的真實速度為0，以這個真實值作為參考可以建立卡爾曼濾波方程，進而估計出當前的位移誤差，通過修正即可得到較為精確的軌跡信息。卡爾曼濾波算法如下：

測量量如式（10）所示：

式中：Ve，Vn，Vu分別為通過算法計算得到的東北天3 個方向上的速度，單位為m/s；Vebase，Vnbase，Vubase為東北天3 個方向上的參考速度，當處于零速狀態時，觀測量為式（11）所示：

考慮線性離散系統，如式（12）所示：

式中：xk為k時刻的狀態向量；Φk，k-1為狀態轉移矩陣；Γk-1為系統噪聲驅動矩陣；zk為量測向量；Hk為量測矩陣；wk-1和vk分別為系統噪聲和量測噪聲。量測矩陣如式（13）所示：

4 實驗驗證

4.1 實驗運行結果

本文實驗將此標簽固定于足部，在室內的環境中定向移動，為了驗證純慣導情況下的長時間長距離運動軌跡的精確性，本文實驗選擇在礦井實地進行實驗驗證，相關數據在兗州礦業集團興隆莊煤礦采集。首先選擇1個環形平臺，繞行1 圈后返回原地，然后沿著巷道行進，在行進過程中多次停止運動，并處于靜止狀態休息工作，然后再次運動。

通過對零速狀態下的標簽做零速校正，可以分別計算得到東向、北向和天向的速度。將導航坐標系的速度積分計算即可以求得3 個方向上的位移。如圖4所示，展示了在3 個方向上的位移，由于本次是在平面進行的實驗，所以可以看到在高度方向上的位移整體趨于0，并沒有較大的誤差。在南北方向上的位移由于初期運動是一個環形軌跡，所以最終南北方向位移回歸于0，東西方向的位移首先回歸0，此后沿著巷道沿行走。最終計算得到的軌跡圖如圖5所示，可以看到，本文提出的方法可以有效地測量并繪制出行人在井下行走軌跡，同時不需要應用外部傳感器輔助測量軌跡，可以為復雜井下環境工作人員提供精確定位。

圖4 三軸位移Fig.4 Three-axis displacement

圖5 井下測量結果Fig.5 Undergr ound measurement results

4.2 實驗結果分析與對比

從上文的實驗分析中，對長度為52 m，寬度為7.2 m的巷道空間進行運動軌跡測量。為了對比本文提出方法的實用性與可行性，將本文實驗與文獻[14]和文獻[15]中的實驗結果進行對比，本文方法與上述文獻的關鍵參數對比如表1所示。

表1 常見井下定位方法對比Table 1 Comparison of common underground positioning methods

文獻[14]使用WIFI定位技術，在70 m×14 m的空間內設置了200 個AP，這說明其便攜性很差，需要攜帶大量設備，其最大定位誤差為4.08 m。文獻[15]采用基于UWB的無線定位技術，其實驗環境在5.6 m×8 m的室內空間，采用4 個定位基站，基站和定位標簽的體積較大，便攜性低，最大定位誤差為0.3 m。本文提出的基于微型IMU的慣性軌跡測量算法可以工作在52 m×7.2 m的大空間中，并且無需使用任何基站輔助定位，僅僅依賴1 個慣性定位標簽即可實現軌跡測量，便攜性高，最大定位誤差為0.63 m。

通過與WIFI定位和UWB定位的實驗對比分析可以看到：采用微型慣性傳感器并應用零速檢測算法的人員軌跡跟蹤系統的定位精度比起WIFI定位更高，且在應用時不需要提前架設基站，僅需要1 個慣性軌跡測量標簽即可實現軌跡測量，實用性較強；與UWB定位技術進行對比可以發現，本文提出方法無需架設基站，所以相比于UWB的應用空間，慣性標簽可以應用于更大的地下空間，更適用于地下礦井環境。

5 結論

1）通過使用MEMS 慣性傳感器的井下人員定位系統可以計算得到人員的軌跡，所受到的外界干擾較少。

2）零速檢測的結果在很大程度上直接影響足式軌跡測量的精度，在不能準確判斷當前零速狀態的工作時間段，系統的軌跡測量將產生較大的誤差。

3）由于方位角尚存在較大的計算誤差，后續需要更多地研究修正方位角的誤差。

4）總結當前的實驗結果，下一步的研究工作將主要集中于零速檢測算法，目前，大量基于機器學習的技術應用于煤礦領域，后續可研究基于機器學習的零速檢測算法，通過對零速狀態采取更好的判斷，提高足式人員定位系統的精度。