基于MEMS的便攜式消防員室內(nèi)定位研究*

王羿帆, 蘇彥莽, 張 琳, 王靜宜

(河北工業(yè)大學(xué) 電子信息工程學(xué)院,天津 300401)

0 引 言

日益復(fù)雜的建筑結(jié)構(gòu),給消防員的滅火工作帶來了很大困難。在復(fù)雜的火場中消防員難以準(zhǔn)確判斷自身位置,無法找到撤離路線,從而錯失最佳自救和營救時機。因而設(shè)計一個良好的消防員室內(nèi)定位系統(tǒng),對提高消防員的救援效率,保證消防員的生命安全具有重要意義。

目前存在的室內(nèi)定位方式如紅外線技術(shù)、超寬帶技術(shù)、Wi-Fi定位技術(shù)、計算機視覺定位技術(shù)等,都需事先在環(huán)境中安裝相關(guān)節(jié)點、設(shè)備,對外部條件要求較高[1]。由于消防工作的特殊性,需要考慮現(xiàn)場復(fù)雜度對定位結(jié)果的影響,再加上全球定位系統(tǒng)(GPS)等衛(wèi)星信號在室內(nèi)無法發(fā)揮很好的作用[2],因而對于大多數(shù)作業(yè)場景來說只有慣性導(dǎo)航方式比較合適[3]。但單純靠慣性導(dǎo)航一種方式在定位過程中會存在很大的漂移誤差,而且慣性導(dǎo)航定位方式不可避免會隨時間產(chǎn)生累積誤差[4],因而大多數(shù)慣性定位方案都會結(jié)合無線定位方法來修正誤差以提高定位精度。因此目前消防定位首先解決的問題是提高慣性導(dǎo)航的精度或者采用抗干擾能力較強且安裝便捷的無線定位方式來消除慣性定位產(chǎn)生的累計誤差。

一般慣性導(dǎo)航主要采用積分法和航位推算法[5]:1)利用積分加速度來計算位移,再結(jié)合實時方位實現(xiàn)定位。但是由于傳感器精度不足和外部環(huán)境的影響,實際應(yīng)用中加速度兩次積分后求得的位移值會隨時間持續(xù)產(chǎn)生累積誤差,幾秒內(nèi)誤差可達幾十米,甚至幾百米,幾乎不能達到定位所需要的精度[6]。2)利用步數(shù)和步長計算位置,一般可以根據(jù)目標(biāo)運動的加速度或角速度特點得到比較精確的步數(shù)。通過行走過程中步頻或加速度與步長之間的關(guān)系來求出每一步的步長值[7]。但這類算法對個體差異的變化比較敏感,尤其是很難準(zhǔn)確計算原地踏步等類似運動的位移結(jié)果。

1 位置信息確定方案

1.1 誤差產(chǎn)生原因

產(chǎn)生誤差的首要原因是傳感器的精度問題,高精度傳感器往往體積較大且價格昂貴。 微機電系統(tǒng)(micro-electro-mechanical system,MEMS)傳感器具有尺寸小、價格低的優(yōu)點,但其采集數(shù)據(jù)的誤差相對較大[8]。且最終的定位結(jié)果,還要進行坐標(biāo)系轉(zhuǎn)換、位移計算等多個階段,誤差會進一步增加。再加上穿戴式設(shè)備所處環(huán)境和運動狀態(tài)相對復(fù)雜,想得到準(zhǔn)確的測量結(jié)果就更加困難。這些問題導(dǎo)致慣性定位方式本身存在著不可避免的累積誤差,且誤差會隨著時間距離的不斷累積而越來越大[7]。研究顯示,人位移計算結(jié)果誤差的標(biāo)準(zhǔn)差與運動持續(xù)時間的三次方呈正相關(guān)[9],以上為慣性導(dǎo)航定位不能得到廣泛應(yīng)用的根本原因。

1.2 零速度更新法

理論上,根據(jù)傳感器采集到的加速度、角速度數(shù)據(jù)解算出運動距離和方向便可以算出目標(biāo)位置。但在實際應(yīng)用中,由于姿態(tài)傳感器精度有限,尤其對于低成本MEMS慣性傳感器而言,幾乎不能在一次積分后檢測到速度為零的結(jié)果[9],再次積分之后誤差被進一步擴大,在長期累積誤差會更大。零速度更新法可以解決運動結(jié)束后的速度漂移誤差問題,使積分方式帶來的誤差大大減小。零速度更新法通過實時檢測目標(biāo)的靜止?fàn)顟B(tài),將靜止態(tài)的速度強制置零,來解決積分誤差帶來的靜止時速度不為零,從而使目標(biāo)位移仍不斷變化的問題,避免誤差累積到下一運動過程中[1]。效果如圖1所示。

圖1 零速度修正效果

1.3 運動拆分法減小累積誤差

1.3.1 單步拆分算法

零速度更新法解決了積分加速度得位移過程中的漂移問題。但在靜止態(tài)將目標(biāo)的運動速度強制置零,只是保證了目標(biāo)開始下一運動狀態(tài)的初始速度為零。在計算單個運動狀態(tài)的速度時,仍然包括了運動狀態(tài)過程中持續(xù)產(chǎn)生的誤差。而且通過實驗分析得知,在一個運動狀態(tài)中,隨著運動的持續(xù)積分誤差是逐漸增大的,且在一個運動狀態(tài)的末尾誤差會達到最大。為了減小這種誤差,本文利用腳部運動先加速后減速的規(guī)律,將單個運動狀態(tài)再次拆分成加速運動和減速運動兩個部分。由于腳部運動總是由靜止到運動再到靜止的過程,因而若分別在加減速兩個部分獨立計算速度,可大幅減小積分的累計誤差。加速部分指速度從零開始增加直至達到最大的過程,減速部分指速度由最大減少到零的過程。把單個運動狀態(tài)拆分成這兩個過程,再將減速過程看作是初速度為零的加速過程的逆過程,進而將單步運動狀態(tài)的開始和結(jié)束速度都約束為零,以達到更有效降低誤差的目的。約束效果如圖2所示。

圖2 運動拆分約束速度效果

1.3.2 運動狀態(tài)拆分算法

要想實現(xiàn)上述處理方式需要從整個運動過程中分離出單步運動狀態(tài),且關(guān)鍵在于運動狀態(tài)的實時監(jiān)測和精確識別。因而本文根據(jù)人整體運動的規(guī)律把整個運動狀態(tài)進行單步分段拆分,利用檢測算法判斷目標(biāo)的運動和靜止?fàn)顟B(tài)分別采集數(shù)據(jù),來保證加減速階段的速度修正。

由于在腳部運動的加速度規(guī)律性不如角速度明顯,且由角速度判斷步數(shù)算法更為簡單,因而選擇根據(jù)腳部角速度的特征來分解運動,這里用5個閾值來加強每一步開始和結(jié)束判斷的準(zhǔn)確效果。第一為開始閾值,當(dāng)角速度大于此值時判斷運動開始,并設(shè)置一個標(biāo)志位,在角速度由正變負再變正的過程中,根據(jù)運動特征再加入3個閾值,下一個閾值是否有效依賴于上一個閾值是否已起作用,不相鄰的閾值間互不影響,達到每個閾值時都會設(shè)立一個標(biāo)志位。最后一個閾值判斷運動過程的結(jié)束,且當(dāng)前面所有標(biāo)志位為1時才有效,最后緩存單步運動開始到結(jié)束的加速度值并將所有標(biāo)志位置零,等待進入新的狀態(tài)檢測。經(jīng)多次試驗其運動狀態(tài)分離的準(zhǔn)確率達99%以上,行走過程中的角速度規(guī)律如圖3所示。

圖3 角速度規(guī)律分析效果

1.4 位置處理

1.4.1 整體位置計算過程

1)求解四元數(shù)坐標(biāo)轉(zhuǎn)換矩陣,將載體坐標(biāo)系采集到的加速度值轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中。但由于姿態(tài)傳感器精度不足,且室內(nèi)存在較大的磁場干擾,其采集到的航向角的絕對值遠不能達到定位的精度。因而此方案采用與GPS結(jié)合的方式,在進入室內(nèi)之前先校準(zhǔn)載體系與正北方向的航偏角。并且將指南針模塊放在運動劇烈程度較小的腰部來減小俯仰運動對模塊正常工作的影響。從而保證定位的準(zhǔn)確性。坐標(biāo)轉(zhuǎn)換矩陣如下

式中an(aE,aN,aU)為變換后導(dǎo)航坐標(biāo)系下的加速度值,ab(ax,ay,az) 為變換前載體坐標(biāo)系下的加速度值,q(q0,q1,q2,q3)為姿態(tài)四元數(shù)。

2)轉(zhuǎn)換到東北天坐標(biāo)中后,根據(jù)角速度特點判斷運動和靜止?fàn)顟B(tài)。保留單個運動狀態(tài)由始至末的有效加速度值進行下一步處理。對單個靜止態(tài)的加速度值取平均處理,用來進行零偏校正

(1)

式中a1～an為單個靜止態(tài)中的加速度值,這種方法充分利用了上一步對運動狀態(tài)的分析結(jié)果,大大減小了計算量,且有效剔除了影響運動態(tài)位移計算的加速度漂移誤差。在不影響運動狀態(tài)判斷準(zhǔn)確性的前提下,實現(xiàn)了對有效加速度數(shù)據(jù)的選取。

3)得到有效加速度值后,對單個運動狀態(tài)繼續(xù)進行拆分和速度修正,計算出顯著消除累計誤差的單步位移值,然后結(jié)合每個運動狀態(tài)測得的方向數(shù)據(jù)計算出目標(biāo)最終位置。本文流程圖如圖4所示。

圖4 方案流程圖

1.4.2 首尾雙靜止態(tài)速度修正算法

對整體運動狀態(tài)進行拆分,只計算運動態(tài)位移,不僅能夠有效減小誤差,也為進一步拆分單個運動狀態(tài)打下了基礎(chǔ)。本方案在此基礎(chǔ)上又提出將首尾速度都做歸零修正的方法,即在拆分后單個運動狀態(tài)的加減速階段,分別修正并計算速度。加速階段,從零初始速度開始,以加速度值持續(xù)為正作為處理依據(jù),當(dāng)檢測到加速度由正轉(zhuǎn)變?yōu)樨摃r,停止加速階段速度處理過程。減速階段,為了方便處理,將減速階段看作從零速度開始的加速運動的逆過程。即從末零狀態(tài)開始,從后往前計算速度值,以加速度值持續(xù)為負作為處理依據(jù),當(dāng)檢測到加速度由負轉(zhuǎn)變?yōu)檎龝r,停止處理過程。保留每個階段中計算得到的速度值,最后將得到的速度進行組合,由始至末按順序積分速度得出位移。

2 實驗驗證

采取直接積分加速度求位移方式,相關(guān)文獻和實驗結(jié)果都顯示定位誤差巨大,漂移誤差嚴重,最終無法實現(xiàn)定位[1]。故只對比零速度更新法與本文的位置計算結(jié)果。

本次實驗姿態(tài)傳感器采用維特智能公司的低成本W(wǎng)T931芯片和GY26磁力計,數(shù)據(jù)由STM32F103單片機進行采集和處理。分別通過直線運動、往返運動和軌跡運動三種運動方式對比和驗證本定位效果。

2.1 直線和往返運動實驗結(jié)果

分別計算兩種方式在直線運動28.8,72,100.8 m的結(jié)果,同一距離分別以不同速度運動3次取平均值,兩種方式得出的最終距離解算結(jié)果如表1所示。分別用兩種處理方式在南北距離為7.2 m的室內(nèi)往返運動4,7,14次,得出最終距初始位置的距離解算結(jié)果如表2所示。

表1 直線行走實驗結(jié)果

從表1中可以看出,在100 m內(nèi),此算法對位移計算的精確度可以達到99.3 %以上。而用普通零速度更新算法只能達到80 %左右的精確度。

表2 往返運動實驗結(jié)果

從表2中可以看出在往返運動14次,總位移距離達到近200 m后,本文目標(biāo)距初始位置也僅差1.48 m,準(zhǔn)確度達到了99 %以上,效果顯著好于零速度更新法。

2.2 進行矩形運動的定位效果

采用本方案由2人在長7.2 m,寬12 m的空間內(nèi),進行矩形運動實驗,分別進行3次運動,軌跡結(jié)果如圖5所示。

圖5 矩形軌跡定位效果

3 結(jié) 論

為提高消防員室內(nèi)定位精度,提出了根據(jù)人運動特征對其運動狀態(tài)做兩次分解處理的方案。首先分離出運動狀態(tài)進而除去靜止態(tài)誤差,再將單個運動態(tài)拆分為加減速2個階段,進一步做速度修正,以減小運動態(tài)的誤差累積。實驗證明此方案能顯著降低位移積分過程中的漂移誤差和

累積誤差,百米定位誤差在0.7 %以內(nèi)。由實驗結(jié)果可知,根據(jù)運動特征拆分運動過程,進而在運動中最大程度地加入速度修正,能有效減小定位誤差的累積,提高單純慣性導(dǎo)航方式的定位精度,從而實現(xiàn)實時、便攜且高精度的室內(nèi)定位。