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

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

0 引 言

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

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

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

1 位置信息確定方案

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

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

1.2 零速度更新法

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

圖1 零速度修正效果

1.3 運(yùn)動拆分法減小累積誤差

1.3.1 單步拆分算法

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

圖2 運(yùn)動拆分約束速度效果

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

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

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

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

1.4 位置處理

1.4.1 整體位置計(jì)算過程

1)求解四元數(shù)坐標(biāo)轉(zhuǎn)換矩陣,將載體坐標(biāo)系采集到的加速度值轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中。但由于姿態(tài)傳感器精度不足,且室內(nèi)存在較大的磁場干擾,其采集到的航向角的絕對值遠(yuǎn)不能達(dá)到定位的精度。因而此方案采用與GPS結(jié)合的方式,在進(jìn)入室內(nèi)之前先校準(zhǔn)載體系與正北方向的航偏角。并且將指南針模塊放在運(yùn)動劇烈程度較小的腰部來減小俯仰運(yù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ù)角速度特點(diǎn)判斷運(yùn)動和靜止?fàn)顟B(tài)。保留單個(gè)運(yùn)動狀態(tài)由始至末的有效加速度值進(jìn)行下一步處理。對單個(gè)靜止態(tài)的加速度值取平均處理,用來進(jìn)行零偏校正

(1)

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

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

圖4 方案流程圖

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

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

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

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

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

2.1 直線和往返運(yùn)動實(shí)驗(yàn)結(jié)果

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

表1 直線行走實(shí)驗(yàn)結(jié)果

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

表2 往返運(yùn)動實(shí)驗(yàn)結(jié)果

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

2.2 進(jìn)行矩形運(yùn)動的定位效果

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

圖5 矩形軌跡定位效果

3 結(jié) 論

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

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