楊 剛，李 強，韓 路，張 鑫

（西安郵電大學(xué) 通信與信息工程學(xué)院，西安 710121）

常年以來，建筑物火災(zāi)事故頻有發(fā)生，常需要消防員進入火場進行滅火和救援?；饒霏h(huán)境煙霧彌漫，能見度低，消防員難以確定自己的具體位置，給滅火和救援帶來了很大的困難。由于缺乏成熟的消防員室內(nèi)定位系統(tǒng)，不僅影響了消防員的救援效率，也極大威脅到消防員的生命安全，造成更大悲劇的發(fā)生。

目前關(guān)于消防員室內(nèi)定位算法的研究成果比較少，主要以UWB、RFID、ZigBee、WIFI、慣性導(dǎo)航等方面的定位算法研究為主。文獻[1]使用UWB技術(shù)實現(xiàn)消防員火場定位，UWB雖然具有較高的定位精度，但是需要提前部署定位基站，并且在室內(nèi)環(huán)境中易受遮擋造成較大的非視距誤差。文獻[2]使用RFID技術(shù)定位，這種方式雖然成本低，但是其抗干擾能力較差。文獻[3]使用基于RSSI算法的ZigBee技術(shù)實現(xiàn)消防員室內(nèi)定位，這種定位方式雖然功耗小，但是易受多徑效應(yīng)的影響。上述定位技術(shù)具有一定的局限性，在真實火場環(huán)境中很難滿足消防員精確實時定位的需求。

慣性導(dǎo)航定位技術(shù)是一種不依賴任何外部設(shè)備或先驗數(shù)據(jù)庫，自主性強的室內(nèi)定位技術(shù)[4]，比較適合于消防員在火場中的定位。隨著MEMS技術(shù)的發(fā)展，慣性傳感器的體積變得越來越小，成本逐漸降低，被大量用于行人導(dǎo)航系統(tǒng)中[5]。9軸MARG（Magnetic,Angular Rate,and Gravity）系統(tǒng)，是一種包含3軸加速度計，陀螺儀和磁力計的傳感器陣列，能夠提供相對于重力方向和地球磁場方向的完整測量[6]。行人航跡推算（Pedestrian Dead Reckoning，PDR）算法基于行人步態(tài)的運動生理學(xué)特征，利用慣性傳感器計算行走時的步數(shù)、步長和航向信息，由初始位置推算出行人的位置坐標(biāo)[7-8]。

本文使用MARG系統(tǒng)作為定位數(shù)據(jù)采集的終端，根據(jù)消防員行走過程中所采集的3軸加速度值、3軸角速度值和3軸磁強度值，計算出步數(shù)、步長和航向角信息，使用PDR算法結(jié)合擴展卡爾曼濾波器（Extended Kalman Filter,EKF）估計出消防員的位置坐標(biāo)。對于步數(shù)檢測問題，設(shè)計了一種根據(jù)加速度幅值波形使用峰值探測法和零點交叉法相結(jié)合的方式進行步數(shù)檢測；對于航向角誤差的累積問題，在改進的啟發(fā)式漂移消除（improve Heuristic Drift Elimination,iHDE）算法[9]的基礎(chǔ)上設(shè)計了一種航向反饋修正算法，通過直行判定和主導(dǎo)方向判定將消防員的行走狀態(tài)進行嚴(yán)格分類，對每種狀態(tài)采取不同的航向修正措施。最后，將EKF和PDR算法融合，推算出消防員的行走軌跡。通過上述方法，可以有效地提高消防員在室內(nèi)環(huán)境中的定位精度。

1 系統(tǒng)介紹

定位系統(tǒng)的算法框架如圖1所示。首先通過消防員胸前佩戴的傳感器獲取行走過程 MARG系統(tǒng)的原始數(shù)據(jù)，對原始數(shù)據(jù)進行濾波和校準(zhǔn)預(yù)處理。對預(yù)處理后的數(shù)據(jù)分別進行以下處理：使用Madgwick算法根據(jù)3軸加速度、角速度和磁場強度求出航向角；通過加速度幅值來獲取行走步數(shù)和步長信息；對獲取的原始航向角進行濾波預(yù)處理，再經(jīng)過反饋修正算法修正航向；最后通過PDR算法根據(jù)步數(shù)、步長和航向數(shù)據(jù)使用EKF估計出消防員的位置坐標(biāo)。

圖1 系統(tǒng)框架Fig.1 System framework

2 消防員室內(nèi)定位算法

2.1 坐標(biāo)系

本文采用的導(dǎo)航坐標(biāo)系（n系）是東北天（East North Up，ENU）坐標(biāo)系，X、Y、Z軸分別為正東、正北和法線方向，構(gòu)成右手坐標(biāo)系。載體坐標(biāo)系（b系）是用戶自身所定義的坐標(biāo)系，原點是導(dǎo)航設(shè)備的中心，消防員行走中水平前進方向為Y軸，設(shè)備的右側(cè)為Y軸，Z軸垂直向上。

在 ENU坐標(biāo)系下，通過下文介紹的步數(shù)檢測、步長計算和航向求解及修正使用PDR算法結(jié)合EKF計算出消防員的位置坐標(biāo)。

2.2 步數(shù)檢測

傳統(tǒng)的步頻檢測法有峰值檢測法[10]、零點交叉法、自相關(guān)法、快速傅里葉變換法等[11]?？紤]到消防員在火場行走的復(fù)雜情況，本文通過對加速度幅值的波形分析，使用峰值探測法和零點交叉法結(jié)合的方式進行步數(shù)檢測，具體過程如下所述。

其次將傳感器靜止放置一定的時間進行加速度數(shù)據(jù)采樣，計算出本地的重力加速度值。將減去的加速度幅值進行Kalman濾波：

狀態(tài)方程為：

式中，ak為k時刻Kalman濾波的狀態(tài)變量，預(yù)測噪聲滿足由具體實驗環(huán)境決定。

觀測方程為：

再對經(jīng)過 Kalman濾波后的結(jié)果進行平滑濾波，經(jīng)過處理后的加速度幅值波形變得更加平滑，如圖2所示，這時可以進行步數(shù)檢測。預(yù)設(shè)加速度閾值和相鄰波峰時間閾值來檢測加速度波峰，根據(jù)波峰數(shù)來確定消防員行走步數(shù)M；通過加速度幅值波形零點特征，判斷加速度幅值前一點小于 0，當(dāng)前點大于或等于 0，并且預(yù)設(shè)相鄰零點時間閾值來檢測零點，準(zhǔn)確標(biāo)記一步的開始時刻和結(jié)束時刻，用于獲取一步之內(nèi)加速度的最大值和最小值來計算步長。根據(jù)零點交叉算法標(biāo)定的每一步開始與結(jié)束時刻，求出每一步的航向角平均值，作為下文航向角修正的原始航向角。圖2中紅色五角星標(biāo)記為檢測到的波峰，綠色圓圈標(biāo)記為檢測到的零點。

圖2 加速度幅值波形Fig.2 Acceleration amplitude waveform

2.3 步長估計

本文根據(jù)加速度幅值信息使用非線性步長模型[12]來計算消防員行走過程中每一步的步長：

式中，K是模型參數(shù)，Amax(m)和Amin(m)代表第m步加速度的最大值和最小值，S(m)表示第m步的步長。

2.4 航向角求解

本文將9軸MARG傳感器輸出的3軸加速度值、3軸角速率值和3軸磁力值通過Madgwick算法[6]更新四元數(shù)來求解航向角。

Madgwick算法的目標(biāo)是通過加權(quán)融合k時刻的陀螺儀算出的四元數(shù)和加速度計與磁力計共同計算出的四元數(shù)，進而得到最優(yōu)的四元數(shù)融合加權(quán)的公式如下所示：

2.5 航向反饋修正算法

本文提出的航向反饋修正算法通過直行判定和主導(dǎo)方向判定將消防員的行走狀態(tài)進行嚴(yán)格分類，對于每種狀態(tài)采取不同的航向修正措施，避免出現(xiàn)錯誤矯正情況的發(fā)生，增加了算法的健壯性。此外，用于航向修正的卡爾曼濾波器狀態(tài)變量只有一維，減少了算法的復(fù)雜度和運算成本。

本算法將消防員在室內(nèi)行走路線分為兩類，分別是曲線行走和直線行走。曲線行走時算法不直接修正航向角，由于算法框架融合的磁力計保證了原始航向角具有一定的精確度，所以可以直接使用原始航向角進行坐標(biāo)計算。直線行走又分為沿著主導(dǎo)方向直線行走和沿著非主導(dǎo)方向直線行走。在主導(dǎo)方向直線行走時使用主導(dǎo)方向值作為航向角參考值，在非主導(dǎo)方向直線行走時使用鄰近三步航向角的均值作為航向角參考值，將當(dāng)前航向角與航向角參考值的差值作為卡爾曼濾波器的觀測量，求出最優(yōu)航向誤差估計值，使用當(dāng)前航向減去最優(yōu)航向誤差估計值得到修正后的航向角。

算法的具體流程如圖3所示。首先對原始航向角進行濾波預(yù)處理，然后進行直行判斷和主導(dǎo)方向判斷，再計算航向角偏差，最后對航向角偏差進行卡爾曼濾波處理，進而修正航向角。算法詳細步驟如下所述：

到了24周，寶寶體重約650克，4周時間里從20周的300克又增重了350克，平均每周增重90克左右。這時會發(fā)現(xiàn)寶寶增重加快了，從16～20周每周的45克到每周90克。相對而言孕媽媽也應(yīng)該開始增加增重幅度了，每周應(yīng)該在250克左右，這樣到24周，孕媽媽從孕前開始可以增重4千克左右。其中大約2.5千克是必需增重的，1.5千克是為將來母乳喂養(yǎng)寶寶做準(zhǔn)備的。

圖3 航向反饋修正算法流程圖Fig.3 Flow chart of heading feedback algorithm

1）主導(dǎo)方向設(shè)定

根據(jù)建筑物大多坐南朝北的特點，本文設(shè)置了東、西、南、北4個主導(dǎo)方向，如圖4所示，主導(dǎo)方向之間的間隔Δ是90°。本文設(shè)定航向角的范圍是 0°到±180°。

圖4 主導(dǎo)方向Fig.4 Dominant direction

2）直行判定

分別用連續(xù) 3步航向值減去這三步航向的平均值，并獲取最大航向差值和預(yù)設(shè)的閾值進行比較，如式(10)所示：

式中，m為當(dāng)前時刻的步數(shù)。

當(dāng)SLP為1時判定為直線行走，可以進入下一項判斷；當(dāng)SLP為0時判定為非直線行走，不進入下一項判定。Thθ為直線行走判定預(yù)設(shè)的角度閾值。

3）主導(dǎo)方向判斷

在判定為直線行走的條件下，使用MOD函數(shù)來判斷消防員是否在主導(dǎo)方向直行：

當(dāng)E的絕對值大于35°時，判定為主導(dǎo)方向行走；當(dāng)E＞0時，行人在鄰近主導(dǎo)方向的右側(cè)，當(dāng)E＜0時，行人在鄰近主導(dǎo)方向的左側(cè)。E的閾值設(shè)定是參考相關(guān)文獻[9,13-14]，并經(jīng)過大量實驗確定的。

4）航向角誤差計算

在主導(dǎo)方向直行時，航向角誤差值δψ(m)等于當(dāng)前航向角ψs減去鄰近主導(dǎo)方向的值；在非主導(dǎo)方向直行時，航向角誤差值δψ(m)等于當(dāng)前航向角減去鄰近三步航向角的均值，即：

式中：ψb為與當(dāng)前航向角ψs最鄰近的主導(dǎo)方向的航向值；mean(ψs(j))為鄰近三步航向角的均值，j=m,m-1,m-2；δψ(m)作為卡爾曼濾波器的觀測量用來修正航向角。

5）航向角誤差置信度

式中，σHDE=0.1 rad[9]。在對航向角誤差δψ(m)進行Kalman濾波時，將σδψ(m)作為觀測噪聲大小的參考值。

6）卡爾曼濾波處理

本文使用航向角誤差值δψ(m)作為Kalman的觀測量來估計出最優(yōu)航向角誤差X(m|m)。卡爾曼濾波算法的詳細過程可以參考文獻[13]，本文不再贅述。

7）航向角修正

根據(jù) Kalman濾波器求出的最優(yōu)航向角誤差估計X(m|m)，使用當(dāng)前航向值ψs(m)減去最優(yōu)航向誤差估計值X(m|m)得到修正后的航向值(m)，即：

式中，ψs(m)為修正前的航向角，(m)為修正后的航向角。

2.6 消防員行走軌跡推算

本文將擴展卡爾曼濾波器（EKF）和PDR算法融合，把前文獲取到的步數(shù)m、步長S(m)、和修正后的航向角(m)代入到EKF中推算出消防員的最優(yōu)行走軌跡。具體過程如下所述：

系統(tǒng)狀態(tài)變量為：

式中，E和N為消防員在ENU坐標(biāo)系中的東向和北向坐標(biāo)，S為每一步的步長，ψ為每一步的航向角。

根據(jù)PDR算法，狀態(tài)方程為：

由于狀態(tài)方程(16)是非線性方程，根據(jù)EKF的原理，用狀態(tài)方程計算的雅各比矩陣近似表示狀態(tài)轉(zhuǎn)移矩陣：

動態(tài)預(yù)測噪聲為：

觀測量為：

觀測方程為：

觀測方程是線性方程，觀測矩陣為：

觀測噪聲矩陣為：

通過EKF的預(yù)測更新和測量更新動態(tài)方程，可以得到狀態(tài)變量的最優(yōu)估計值：

3 實驗驗證與分析

3.1 數(shù)據(jù)采集

本次實驗使用集成MPU9250傳感器的JY901作為實驗數(shù)據(jù)采集平臺。JY901的主控部分STM32F051是一款基于ARM 32-bit的Cortex?-M0 CPU，具有32k到64k字節(jié)的Flash Memory，工作頻率高，功耗低，外設(shè)豐富且價格低。MPU9250是一款9軸MARG運動跟蹤傳感器，由3軸加速度計、3軸陀螺儀和3軸磁力計組成。加速度計誤差為±6mg，陀螺儀誤差為±5 (°)/s。經(jīng)過測試，根據(jù)MPU9250傳感器采集的慣性數(shù)據(jù)，使用 Madgwick算法求取的原始航向角誤差在±10°范圍內(nèi)波動。在實驗中，設(shè)定傳感器的數(shù)據(jù)采集速率是50 Hz，工作電壓是3.3 V，波特率為115 200 Hz。實驗場地是西安郵電大學(xué)東區(qū)逸夫樓八樓自習(xí)室，實驗中慣性傳感器佩戴在測試人員胸前，傳感器獲取到的數(shù)據(jù)通過串口傳輸?shù)絇C端，由Matlab軟件來處理數(shù)據(jù)，如圖5所示。

圖5 數(shù)據(jù)采集Fig.5 Data acquisition

3.2 實驗內(nèi)容

為了驗證本文提出的消防員室內(nèi)定位算法的性能，設(shè)計了3組實驗，測試人員胸前佩戴著傳感器，分別沿著矩形路線、三角形路線和半圓形路線進行實地測試。

1）矩形路線實驗

圖6 矩形實驗航向角變化Fig.6 Variations of heading angle inrectangular experiment

矩形路線實驗是沿著東、南、西、北四個主導(dǎo)方向行走一圈，以驗證在這四個主導(dǎo)方向上直行時航向反饋修正算法的性能。圖6是測試人員行走過程中航向角的變化，通過對比分析，未經(jīng)修正的原始航向角波形不平整且偏離真實值，經(jīng)過航向反饋修正算法處理的航向角更加平滑并且較為接近真實航向值。圖7為矩形實驗的行走軌跡，無航向修正的PDR算法軌跡圖在短時間嚴(yán)重偏離真實軌跡，加了航向反饋修正算法的軌跡圖始終接近真實軌跡。

如表1所示，使用的航向反饋修正算法的返回原位置誤差（Return Position Error,RPE）為0.9048 m，與未使用相比提升了46.84%，位置RMSE提升了77.90%，航向 RMSE提升了 65.03%。本次實驗驗證了在主導(dǎo)方向直行時航向反饋修正算法可以很好地修正航向。

圖7 矩形實驗行走軌跡Fig.7 Rectangular experimental walking track

表1 矩形實驗精度分析Tab.1 Accuracy analysis of rectangular experiment

2）三角形路線實驗

在三角形路線實驗中，行走軌跡為一個直角三角形，兩條直角邊在主導(dǎo)方向上，一條斜邊在非主導(dǎo)方向上。該實驗是為了驗證在非主導(dǎo)方向直行時，航向反饋修正算法的性能。圖8為航向角變化情況，其中前兩段為在主導(dǎo)方向直行時的航向角變化，最后一段為在非主導(dǎo)方向直行時的航向角變化。圖9為三角形實驗的行走軌跡，相比單純使用PDR算法，添加了航向反饋修正算法生成的軌跡更加接近真實軌跡。

圖8 三角形實驗航向角變化Fig.8 Variations of heading angle in triangular experiment

圖9 三角形實驗行走軌跡Fig.9 Triangle experimental walking track

如表2所示，添加的航向反饋修正算法 RPE為1.10m，與未使用航向修正算法相比提升了 45.54%，位置 RMSE提升了 76.81%，航向 RMSE提升了68.21%。三角形實驗的結(jié)果表明在非主導(dǎo)方向直行時航向反饋修正算法具有良好的精度。

表2 三角形實驗精度分析Tab.2 Accuracy analysis of triangle experiment

3）半圓形路線實驗

半圓形路線實驗設(shè)定為直徑在主導(dǎo)方向上，圓弧在非主導(dǎo)方向上，這是對航向反饋修正算法設(shè)計的挑戰(zhàn)性實驗。半圓形路線實驗的航向角變化如圖10所示：第一段是沿著直徑在主導(dǎo)方向上直行，航向反饋修正算法會修正航向角；第二段是沿著圓弧曲線行走，由于實際航向角在不斷變化，相比于直線行走，沒有一個固定的航向角觀測值，導(dǎo)致難以計算出航向角偏差，無法對航向角進行直接修正，航向反饋修正算法性能會暫時下降，軌跡推算直接使用原始航向角。實驗軌跡如圖11所示，由于真實行走軌跡的半圓不是規(guī)則的半圓，是一個近似的半圓，所以在本實驗的軌跡圖中并沒有標(biāo)出真實的軌跡。本文提出的航向反饋修正算法框架融合了磁力計，求出的原始航向角具有一定的精確度，在曲線行走航向修正算法性能下降的情況下，如圖11所示，所依據(jù)的原始航向信息依然具備一定的準(zhǔn)確性?？紤]到在多數(shù)情況下人的行走是直線行走，所以在曲線行走時導(dǎo)致短暫的算法性能下降不會對最終定位精度產(chǎn)生較大的影響。

圖10 半圓形實驗航向角變化Fig.10 Variations of heading angle in semicircular experiment

圖11 半圓形實驗行走軌跡Fig.11 Semicircular experimental walking track

4 結(jié) 論

本文以MARG傳感器為基礎(chǔ)，針對消防員室內(nèi)火場定位的應(yīng)用背景，設(shè)計了一種導(dǎo)航定位方法。針對PDR算法中的步數(shù)檢測問題，設(shè)計了一種根據(jù)加速度幅值波形使用峰值探測法和零點交叉法相結(jié)合的方法來準(zhǔn)確獲取消防員的行走步數(shù)。針對慣性導(dǎo)航中航向角誤差隨時間和行走距離增大的問題，提出了一種航向反饋修正算法來修正航向角。當(dāng)消防員沿著直線路徑行走時，算法會對航向角進行修正，修正后的航向角結(jié)合步數(shù)和步長信息通過EKF和PDR算法來估計出消防員的位置坐標(biāo)。算法框架所融合的磁力計增加了航向信息的準(zhǔn)確度，即使消防員在非理想路徑行走時也可以獲得良好的定位精度。綜合3組實驗，本文所提算法將 RPE提升了 45%以上，航向角和位置的RMSE分別提升65%和76%以上，具有較好的穩(wěn)定性和可靠性，能適應(yīng)較為復(fù)雜的應(yīng)用場景。

本文所述的定位方法為消防員在室內(nèi)火場環(huán)境中的位置信息獲取提供了一種有效的解決方案。在后續(xù)的研究中，將考慮如何解決周圍磁場環(huán)境對磁力計測量精度的影響。磁力計擾動的改善將有助于提高定位精度，進而保障消防員的自身安全和提高救援效率。