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

康俊瑄

（中國(guó)礦業(yè)大學(xué)（北京），北京 100083）

0 引言

我國(guó)煤炭?jī)?chǔ)量豐富，煤炭資源在我國(guó)能源消費(fèi)結(jié)構(gòu)中占據(jù)主導(dǎo)地位，根據(jù)國(guó)家能源局的統(tǒng)計(jì)數(shù)據(jù)，2020年我國(guó)煤炭消費(fèi)量占能源消費(fèi)總量的56.8%。煤礦開采環(huán)境錯(cuò)綜復(fù)雜，嚴(yán)重制約著煤礦的安全生產(chǎn)[1]。近年來，國(guó)家對(duì)煤礦挖掘的生產(chǎn)安全提出了更嚴(yán)格的要求[2]，經(jīng)過多年整治改善，我國(guó)煤礦的事故死亡人數(shù)從2010年的1 973 人下降至2020年的225 人。然而，目前我國(guó)與國(guó)外發(fā)達(dá)國(guó)家在煤礦智能化領(lǐng)域的研究仍然存在著較大差距，重大安全隱患的智能監(jiān)測(cè)、預(yù)測(cè)、預(yù)警等技術(shù)相對(duì)落后。為了提高煤礦智能化水平，在煤礦安全事故發(fā)生后能夠快速有效地對(duì)失聯(lián)工作人員進(jìn)行定位搜尋，本文圍繞煤礦井下人員定位系統(tǒng)開展研究。

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

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

1 系統(tǒng)架構(gòu)

1.1 系統(tǒng)硬件架構(gòu)

為了實(shí)現(xiàn)本文提出的慣性軌跡測(cè)量算法，本文設(shè)計(jì)一個(gè)基于微傳感器的慣性軌跡測(cè)量標(biāo)簽，如圖1所示，其大小為3 cm（寬） ×13 cm（長(zhǎng)），具有較強(qiáng)的便攜性。

圖1 人員定位系統(tǒng)軌跡測(cè)量標(biāo)簽Fig.1 Trajectory measurement label of personnel positioning system

軌跡測(cè)量標(biāo)簽的系統(tǒng)硬件框圖如圖2所示，采用的實(shí)驗(yàn)標(biāo)簽由微控制器、慣性傳感器、無線傳輸模塊和電池組成。

圖2 軌跡測(cè)量標(biāo)簽硬件框圖Fig.2 Hardware block diagr am of trajector y measurement label

軌跡測(cè)量標(biāo)簽的慣性傳感器采用的是ADI公司的10 自由度MEMS 慣性傳感器ADIS16448，此傳感器包含1 個(gè)三軸陀螺儀、1 個(gè)三軸加速度計(jì)、1 個(gè)三軸磁力計(jì)以及1 個(gè)壓力傳感器。此傳感器能夠提供較為精確的姿態(tài)和速度信息，與復(fù)雜且昂貴的分離設(shè)計(jì)方案相比，ADIS16448 能夠?yàn)榫_的多軸慣性檢測(cè)提供簡(jiǎn)單且高效的方法。其在工廠生產(chǎn)環(huán)節(jié)就做過必要的運(yùn)動(dòng)測(cè)試及校準(zhǔn)，可以有效縮短系統(tǒng)集成時(shí)間。ADIS16448 的陀螺儀最大測(cè)量范圍是±1 000°/s，加速度計(jì)測(cè)量范圍最大為±18 g，可以感受的人體運(yùn)動(dòng)和靜止?fàn)顟B(tài)時(shí)的加速度。磁力計(jì)最大測(cè)量范圍為±1.9 gauss。

本標(biāo)簽采用1 顆STM32L051 芯片，可滿足2 路SPI通信，且具有較低功耗，較低的功耗能夠保證人員長(zhǎng)時(shí)間井下工作時(shí)仍能夠保持系統(tǒng)的運(yùn)轉(zhuǎn)。標(biāo)簽還配備了1個(gè)nRF24L01 模塊作為2.4G通信的模塊，以便于標(biāo)簽可以遠(yuǎn)程傳輸數(shù)據(jù)。

1.2 系統(tǒng)軟件架構(gòu)

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

圖3 人員定位系統(tǒng)框圖Fig.3 Framework of personnel positioning system

1）零速檢測(cè)：使用低成本的MEMS 慣性傳感器進(jìn)行軌跡測(cè)量的一個(gè)非常關(guān)鍵的技術(shù)就是零速檢測(cè)技術(shù)。當(dāng)前最主要的檢測(cè)方法即為閾值測(cè)量法，此算法通過測(cè)量IMU的陀螺儀和加速度計(jì)的輸出，當(dāng)此輸出值大于靜止?fàn)顟B(tài)的閾值時(shí)，將此時(shí)看作運(yùn)動(dòng)狀態(tài)，否則認(rèn)為當(dāng)前狀態(tài)為靜止?fàn)顟B(tài)[13]。

2）慣性軌跡測(cè)量算法：慣性軌跡測(cè)量算法主要包括2 部分：①其一為設(shè)備姿態(tài)估計(jì)，對(duì)行人軌跡跟蹤的性能很大程度上取決于準(zhǔn)確跟蹤移動(dòng)設(shè)備方向姿態(tài)能力。在嵌入低成本IMU傳感器的無約束設(shè)備環(huán)境中，這項(xiàng)任務(wù)尤其具有挑戰(zhàn)性。通常，陀螺儀漂移和磁場(chǎng)干擾，特別是在地下煤礦環(huán)境中，是準(zhǔn)確估計(jì)方向的主要障礙。解決這個(gè)問題至關(guān)重要，因?yàn)閼T性軌跡測(cè)量算法的幾乎所有任務(wù)都使用方向信息。②其二是對(duì)設(shè)備的速度更新算法。

3）零速狀態(tài)下軌跡誤差校正算法：在檢測(cè)到標(biāo)簽處于零速狀態(tài)下后，本文使用零速校正算法修正因?yàn)橥勇輧x和加速度計(jì)漂移引起的速度位移誤差。當(dāng)零速檢測(cè)判定當(dāng)前時(shí)刻的標(biāo)簽速度為0 時(shí)，將通過慣性軌跡更新算法計(jì)算得到的速度當(dāng)作觀測(cè)量送入觀測(cè)方程，使用卡爾曼濾波方程估計(jì)位移誤差。

2 軌跡測(cè)量算法

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

通過對(duì)姿態(tài)轉(zhuǎn)換矩陣進(jìn)行更新，即可得到行人此時(shí)的方向，進(jìn)而計(jì)算人員的位移軌跡。為了能夠進(jìn)行更精確的軌跡測(cè)量，需要計(jì)算初始姿態(tài)，初始姿態(tài)的計(jì)算方法與靜態(tài)位姿計(jì)算一樣。

當(dāng)通過零速狀態(tài)檢測(cè)器檢測(cè)到當(dāng)前運(yùn)動(dòng)狀態(tài)為靜止時(shí)，此時(shí)IMU測(cè)量得到的加速度全部由重力加速度提供，因?yàn)榈厍蛑亓铀俣鹊姆较虿浑S時(shí)間而改變。同樣具有此特性的還有地球的磁場(chǎng)，其磁力方向不隨時(shí)間變化，所以利用這2 個(gè)物理量，通過加速度計(jì)和磁力計(jì)檢測(cè)并計(jì)算，可以計(jì)算得到當(dāng)前的位姿，如式（2）所示。

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

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

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

當(dāng)人員開始移動(dòng)時(shí)，通過陀螺儀采集到角速度數(shù)據(jù)計(jì)算位姿，并通過加速度計(jì)采集加速度數(shù)據(jù)計(jì)算人員移動(dòng)速度，進(jìn)而求解人員軌跡。本文采用四元數(shù)法計(jì)算位姿更新，四元數(shù)法具有計(jì)算量小，奇異性小的特點(diǎn)，每1個(gè)位姿對(duì)應(yīng)1 個(gè)四元數(shù)，所以大量應(yīng)用于飛行器的位姿計(jì)算算法中。

四元數(shù)定義為式（5）～（6）：

將姿態(tài)角轉(zhuǎn)換為四元數(shù)為式（7）：

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

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

3 零速校正

當(dāng)檢測(cè)到標(biāo)簽處于零速狀態(tài)時(shí)，可以采用零速校正算法對(duì)軌跡漂移誤差進(jìn)行校正。標(biāo)簽通過檢測(cè)慣性器件輸出的加速度和角速度數(shù)值的矢量和，將此矢量和與靜止?fàn)顟B(tài)下的輸出值做對(duì)比，設(shè)定1 個(gè)閾值，若當(dāng)前狀態(tài)的矢量和與靜止?fàn)顟B(tài)下的矢量和之間的差距超過了這個(gè)閾值，則認(rèn)為當(dāng)前處于運(yùn)動(dòng)狀態(tài)，不對(duì)當(dāng)前狀態(tài)的標(biāo)簽進(jìn)行零速校正；相反，若通過對(duì)比2 個(gè)矢量和，發(fā)現(xiàn)其插值小于閾值，則可以認(rèn)為當(dāng)前狀態(tài)為零速狀態(tài)。系統(tǒng)處于零速狀態(tài)時(shí)，可以認(rèn)為當(dāng)前的真實(shí)速度為0，以這個(gè)真實(shí)值作為參考可以建立卡爾曼濾波方程，進(jìn)而估計(jì)出當(dāng)前的位移誤差，通過修正即可得到較為精確的軌跡信息?？柭鼮V波算法如下：

測(cè)量量如式（10）所示：

式中：Ve，Vn，Vu分別為通過算法計(jì)算得到的東北天3 個(gè)方向上的速度，單位為m/s；Vebase，Vnbase，Vubase為東北天3 個(gè)方向上的參考速度，當(dāng)處于零速狀態(tài)時(shí)，觀測(cè)量為式（11）所示：

考慮線性離散系統(tǒng)，如式（12）所示：

式中：xk為k時(shí)刻的狀態(tài)向量；Φk，k-1為狀態(tài)轉(zhuǎn)移矩陣；Γk-1為系統(tǒng)噪聲驅(qū)動(dòng)矩陣；zk為量測(cè)向量；Hk為量測(cè)矩陣；wk-1和vk分別為系統(tǒng)噪聲和量測(cè)噪聲。量測(cè)矩陣如式（13）所示：

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)運(yùn)行結(jié)果

本文實(shí)驗(yàn)將此標(biāo)簽固定于足部，在室內(nèi)的環(huán)境中定向移動(dòng)，為了驗(yàn)證純慣導(dǎo)情況下的長(zhǎng)時(shí)間長(zhǎng)距離運(yùn)動(dòng)軌跡的精確性，本文實(shí)驗(yàn)選擇在礦井實(shí)地進(jìn)行實(shí)驗(yàn)驗(yàn)證，相關(guān)數(shù)據(jù)在兗州礦業(yè)集團(tuán)興隆莊煤礦采集。首先選擇1個(gè)環(huán)形平臺(tái)，繞行1 圈后返回原地，然后沿著巷道行進(jìn)，在行進(jìn)過程中多次停止運(yùn)動(dòng)，并處于靜止?fàn)顟B(tài)休息工作，然后再次運(yùn)動(dòng)。

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

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

圖5 井下測(cè)量結(jié)果Fig.5 Undergr ound measurement results

4.2 實(shí)驗(yàn)結(jié)果分析與對(duì)比

從上文的實(shí)驗(yàn)分析中，對(duì)長(zhǎng)度為52 m，寬度為7.2 m的巷道空間進(jìn)行運(yùn)動(dòng)軌跡測(cè)量。為了對(duì)比本文提出方法的實(shí)用性與可行性，將本文實(shí)驗(yàn)與文獻(xiàn)[14]和文獻(xiàn)[15]中的實(shí)驗(yàn)結(jié)果進(jìn)行對(duì)比，本文方法與上述文獻(xiàn)的關(guān)鍵參數(shù)對(duì)比如表1所示。

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

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

通過與WIFI定位和UWB定位的實(shí)驗(yàn)對(duì)比分析可以看到：采用微型慣性傳感器并應(yīng)用零速檢測(cè)算法的人員軌跡跟蹤系統(tǒng)的定位精度比起WIFI定位更高，且在應(yīng)用時(shí)不需要提前架設(shè)基站，僅需要1 個(gè)慣性軌跡測(cè)量標(biāo)簽即可實(shí)現(xiàn)軌跡測(cè)量，實(shí)用性較強(qiáng)；與UWB定位技術(shù)進(jìn)行對(duì)比可以發(fā)現(xiàn)，本文提出方法無需架設(shè)基站，所以相比于UWB的應(yīng)用空間，慣性標(biāo)簽可以應(yīng)用于更大的地下空間，更適用于地下礦井環(huán)境。

5 結(jié)論

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

2）零速檢測(cè)的結(jié)果在很大程度上直接影響足式軌跡測(cè)量的精度，在不能準(zhǔn)確判斷當(dāng)前零速狀態(tài)的工作時(shí)間段，系統(tǒng)的軌跡測(cè)量將產(chǎn)生較大的誤差。

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

4）總結(jié)當(dāng)前的實(shí)驗(yàn)結(jié)果，下一步的研究工作將主要集中于零速檢測(cè)算法，目前，大量基于機(jī)器學(xué)習(xí)的技術(shù)應(yīng)用于煤礦領(lǐng)域，后續(xù)可研究基于機(jī)器學(xué)習(xí)的零速檢測(cè)算法，通過對(duì)零速狀態(tài)采取更好的判斷，提高足式人員定位系統(tǒng)的精度。