楊 剛,李 強(qiáng),韓 路,張 鑫
(西安郵電大學(xué) 通信與信息工程學(xué)院,西安 710121)
常年以來,建筑物火災(zāi)事故頻有發(fā)生,常需要消防員進(jìn)入火場(chǎng)進(jìn)行滅火和救援?;饒?chǎng)環(huán)境煙霧彌漫,能見度低,消防員難以確定自己的具體位置,給滅火和救援帶來了很大的困難。由于缺乏成熟的消防員室內(nèi)定位系統(tǒng),不僅影響了消防員的救援效率,也極大威脅到消防員的生命安全,造成更大悲劇的發(fā)生。
目前關(guān)于消防員室內(nèi)定位算法的研究成果比較少,主要以UWB、RFID、ZigBee、WIFI、慣性導(dǎo)航等方面的定位算法研究為主。文獻(xiàn)[1]使用UWB技術(shù)實(shí)現(xiàn)消防員火場(chǎng)定位,UWB雖然具有較高的定位精度,但是需要提前部署定位基站,并且在室內(nèi)環(huán)境中易受遮擋造成較大的非視距誤差。文獻(xiàn)[2]使用RFID技術(shù)定位,這種方式雖然成本低,但是其抗干擾能力較差。文獻(xiàn)[3]使用基于RSSI算法的ZigBee技術(shù)實(shí)現(xiàn)消防員室內(nèi)定位,這種定位方式雖然功耗小,但是易受多徑效應(yīng)的影響。上述定位技術(shù)具有一定的局限性,在真實(shí)火場(chǎng)環(huán)境中很難滿足消防員精確實(shí)時(shí)定位的需求。
慣性導(dǎo)航定位技術(shù)是一種不依賴任何外部設(shè)備或先驗(yàn)數(shù)據(jù)庫(kù),自主性強(qiáng)的室內(nèi)定位技術(shù)[4],比較適合于消防員在火場(chǎng)中的定位。隨著MEMS技術(shù)的發(fā)展,慣性傳感器的體積變得越來越小,成本逐漸降低,被大量用于行人導(dǎo)航系統(tǒng)中[5]。9軸MARG(Magnetic,Angular Rate,and Gravity)系統(tǒng),是一種包含3軸加速度計(jì),陀螺儀和磁力計(jì)的傳感器陣列,能夠提供相對(duì)于重力方向和地球磁場(chǎng)方向的完整測(cè)量[6]。行人航跡推算(Pedestrian Dead Reckoning,PDR)算法基于行人步態(tài)的運(yùn)動(dòng)生理學(xué)特征,利用慣性傳感器計(jì)算行走時(shí)的步數(shù)、步長(zhǎng)和航向信息,由初始位置推算出行人的位置坐標(biāo)[7-8]。
本文使用MARG系統(tǒng)作為定位數(shù)據(jù)采集的終端,根據(jù)消防員行走過程中所采集的3軸加速度值、3軸角速度值和3軸磁強(qiáng)度值,計(jì)算出步數(shù)、步長(zhǎng)和航向角信息,使用PDR算法結(jié)合擴(kuò)展卡爾曼濾波器(Extended Kalman Filter,EKF)估計(jì)出消防員的位置坐標(biāo)。對(duì)于步數(shù)檢測(cè)問題,設(shè)計(jì)了一種根據(jù)加速度幅值波形使用峰值探測(cè)法和零點(diǎn)交叉法相結(jié)合的方式進(jìn)行步數(shù)檢測(cè);對(duì)于航向角誤差的累積問題,在改進(jìn)的啟發(fā)式漂移消除(improve Heuristic Drift Elimination,iHDE)算法[9]的基礎(chǔ)上設(shè)計(jì)了一種航向反饋修正算法,通過直行判定和主導(dǎo)方向判定將消防員的行走狀態(tài)進(jìn)行嚴(yán)格分類,對(duì)每種狀態(tài)采取不同的航向修正措施。最后,將EKF和PDR算法融合,推算出消防員的行走軌跡。通過上述方法,可以有效地提高消防員在室內(nèi)環(huán)境中的定位精度。
定位系統(tǒng)的算法框架如圖1所示。首先通過消防員胸前佩戴的傳感器獲取行走過程 MARG系統(tǒng)的原始數(shù)據(jù),對(duì)原始數(shù)據(jù)進(jìn)行濾波和校準(zhǔn)預(yù)處理。對(duì)預(yù)處理后的數(shù)據(jù)分別進(jìn)行以下處理:使用Madgwick算法根據(jù)3軸加速度、角速度和磁場(chǎng)強(qiáng)度求出航向角;通過加速度幅值來獲取行走步數(shù)和步長(zhǎng)信息;對(duì)獲取的原始航向角進(jìn)行濾波預(yù)處理,再經(jīng)過反饋修正算法修正航向;最后通過PDR算法根據(jù)步數(shù)、步長(zhǎng)和航向數(shù)據(jù)使用EKF估計(jì)出消防員的位置坐標(biāo)。
圖1 系統(tǒng)框架Fig.1 System framework
本文采用的導(dǎo)航坐標(biāo)系(n系)是東北天(East North Up,ENU)坐標(biāo)系,X、Y、Z軸分別為正東、正北和法線方向,構(gòu)成右手坐標(biāo)系。載體坐標(biāo)系(b系)是用戶自身所定義的坐標(biāo)系,原點(diǎn)是導(dǎo)航設(shè)備的中心,消防員行走中水平前進(jìn)方向?yàn)閅軸,設(shè)備的右側(cè)為Y軸,Z軸垂直向上。
在 ENU坐標(biāo)系下,通過下文介紹的步數(shù)檢測(cè)、步長(zhǎng)計(jì)算和航向求解及修正使用PDR算法結(jié)合EKF計(jì)算出消防員的位置坐標(biāo)。
傳統(tǒng)的步頻檢測(cè)法有峰值檢測(cè)法[10]、零點(diǎn)交叉法、自相關(guān)法、快速傅里葉變換法等[11]??紤]到消防員在火場(chǎng)行走的復(fù)雜情況,本文通過對(duì)加速度幅值的波形分析,使用峰值探測(cè)法和零點(diǎn)交叉法結(jié)合的方式進(jìn)行步數(shù)檢測(cè),具體過程如下所述。
其次將傳感器靜止放置一定的時(shí)間進(jìn)行加速度數(shù)據(jù)采樣,計(jì)算出本地的重力加速度值。將減去的加速度幅值進(jìn)行Kalman濾波:
狀態(tài)方程為:
式中,ak為k時(shí)刻Kalman濾波的狀態(tài)變量,預(yù)測(cè)噪聲滿足由具體實(shí)驗(yàn)環(huán)境決定。
觀測(cè)方程為:
再對(duì)經(jīng)過 Kalman濾波后的結(jié)果進(jìn)行平滑濾波,經(jīng)過處理后的加速度幅值波形變得更加平滑,如圖2所示,這時(shí)可以進(jìn)行步數(shù)檢測(cè)。預(yù)設(shè)加速度閾值和相鄰波峰時(shí)間閾值來檢測(cè)加速度波峰,根據(jù)波峰數(shù)來確定消防員行走步數(shù)M;通過加速度幅值波形零點(diǎn)特征,判斷加速度幅值前一點(diǎn)小于 0,當(dāng)前點(diǎn)大于或等于 0,并且預(yù)設(shè)相鄰零點(diǎn)時(shí)間閾值來檢測(cè)零點(diǎn),準(zhǔn)確標(biāo)記一步的開始時(shí)刻和結(jié)束時(shí)刻,用于獲取一步之內(nèi)加速度的最大值和最小值來計(jì)算步長(zhǎng)。根據(jù)零點(diǎn)交叉算法標(biāo)定的每一步開始與結(jié)束時(shí)刻,求出每一步的航向角平均值,作為下文航向角修正的原始航向角。圖2中紅色五角星標(biāo)記為檢測(cè)到的波峰,綠色圓圈標(biāo)記為檢測(cè)到的零點(diǎn)。
圖2 加速度幅值波形Fig.2 Acceleration amplitude waveform
本文根據(jù)加速度幅值信息使用非線性步長(zhǎng)模型[12]來計(jì)算消防員行走過程中每一步的步長(zhǎng):
式中,K是模型參數(shù),Amax(m)和Amin(m)代表第m步加速度的最大值和最小值,S(m)表示第m步的步長(zhǎng)。
本文將9軸MARG傳感器輸出的3軸加速度值、3軸角速率值和3軸磁力值通過Madgwick算法[6]更新四元數(shù)來求解航向角。
Madgwick算法的目標(biāo)是通過加權(quán)融合k時(shí)刻的陀螺儀算出的四元數(shù)和加速度計(jì)與磁力計(jì)共同計(jì)算出的四元數(shù),進(jìn)而得到最優(yōu)的四元數(shù)融合加權(quán)的公式如下所示:
本文提出的航向反饋修正算法通過直行判定和主導(dǎo)方向判定將消防員的行走狀態(tài)進(jìn)行嚴(yán)格分類,對(duì)于每種狀態(tài)采取不同的航向修正措施,避免出現(xiàn)錯(cuò)誤矯正情況的發(fā)生,增加了算法的健壯性。此外,用于航向修正的卡爾曼濾波器狀態(tài)變量只有一維,減少了算法的復(fù)雜度和運(yùn)算成本。
本算法將消防員在室內(nèi)行走路線分為兩類,分別是曲線行走和直線行走。曲線行走時(shí)算法不直接修正航向角,由于算法框架融合的磁力計(jì)保證了原始航向角具有一定的精確度,所以可以直接使用原始航向角進(jìn)行坐標(biāo)計(jì)算。直線行走又分為沿著主導(dǎo)方向直線行走和沿著非主導(dǎo)方向直線行走。在主導(dǎo)方向直線行走時(shí)使用主導(dǎo)方向值作為航向角參考值,在非主導(dǎo)方向直線行走時(shí)使用鄰近三步航向角的均值作為航向角參考值,將當(dāng)前航向角與航向角參考值的差值作為卡爾曼濾波器的觀測(cè)量,求出最優(yōu)航向誤差估計(jì)值,使用當(dāng)前航向減去最優(yōu)航向誤差估計(jì)值得到修正后的航向角。
算法的具體流程如圖3所示。首先對(duì)原始航向角進(jìn)行濾波預(yù)處理,然后進(jìn)行直行判斷和主導(dǎo)方向判斷,再計(jì)算航向角偏差,最后對(duì)航向角偏差進(jìn)行卡爾曼濾波處理,進(jìn)而修正航向角。算法詳細(xì)步驟如下所述:
到了24周,寶寶體重約650克,4周時(shí)間里從20周的300克又增重了350克,平均每周增重90克左右。這時(shí)會(huì)發(fā)現(xiàn)寶寶增重加快了,從16~20周每周的45克到每周90克。相對(duì)而言孕媽媽也應(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ù)建筑物大多坐南朝北的特點(diǎn),本文設(shè)置了東、西、南、北4個(gè)主導(dǎo)方向,如圖4所示,主導(dǎo)方向之間的間隔Δ是90°。本文設(shè)定航向角的范圍是 0°到±180°。
圖4 主導(dǎo)方向Fig.4 Dominant direction
2)直行判定
分別用連續(xù) 3步航向值減去這三步航向的平均值,并獲取最大航向差值和預(yù)設(shè)的閾值進(jìn)行比較,如式(10)所示:
式中,m為當(dāng)前時(shí)刻的步數(shù)。
當(dāng)SLP為1時(shí)判定為直線行走,可以進(jìn)入下一項(xiàng)判斷;當(dāng)SLP為0時(shí)判定為非直線行走,不進(jìn)入下一項(xiàng)判定。Thθ為直線行走判定預(yù)設(shè)的角度閾值。
3)主導(dǎo)方向判斷
在判定為直線行走的條件下,使用MOD函數(shù)來判斷消防員是否在主導(dǎo)方向直行:
當(dāng)E的絕對(duì)值大于35°時(shí),判定為主導(dǎo)方向行走;當(dāng)E>0時(shí),行人在鄰近主導(dǎo)方向的右側(cè),當(dāng)E<0時(shí),行人在鄰近主導(dǎo)方向的左側(cè)。E的閾值設(shè)定是參考相關(guān)文獻(xiàn)[9,13-14],并經(jīng)過大量實(shí)驗(yàn)確定的。
4)航向角誤差計(jì)算
在主導(dǎo)方向直行時(shí),航向角誤差值δψ(m)等于當(dāng)前航向角ψs減去鄰近主導(dǎo)方向的值;在非主導(dǎo)方向直行時(shí),航向角誤差值δψ(m)等于當(dāng)前航向角減去鄰近三步航向角的均值,即:
式中:ψb為與當(dāng)前航向角ψs最鄰近的主導(dǎo)方向的航向值;mean(ψs(j))為鄰近三步航向角的均值,j=m,m-1,m-2;δψ(m)作為卡爾曼濾波器的觀測(cè)量用來修正航向角。
5)航向角誤差置信度
式中,σHDE=0.1 rad[9]。在對(duì)航向角誤差δψ(m)進(jìn)行Kalman濾波時(shí),將σδψ(m)作為觀測(cè)噪聲大小的參考值。
6)卡爾曼濾波處理
本文使用航向角誤差值δψ(m)作為Kalman的觀測(cè)量來估計(jì)出最優(yōu)航向角誤差X(m|m)??柭鼮V波算法的詳細(xì)過程可以參考文獻(xiàn)[13],本文不再贅述。
7)航向角修正
根據(jù) Kalman濾波器求出的最優(yōu)航向角誤差估計(jì)X(m|m),使用當(dāng)前航向值ψs(m)減去最優(yōu)航向誤差估計(jì)值X(m|m)得到修正后的航向值(m),即:
式中,ψs(m)為修正前的航向角,(m)為修正后的航向角。
本文將擴(kuò)展卡爾曼濾波器(EKF)和PDR算法融合,把前文獲取到的步數(shù)m、步長(zhǎng)S(m)、和修正后的航向角(m)代入到EKF中推算出消防員的最優(yōu)行走軌跡。具體過程如下所述:
系統(tǒng)狀態(tài)變量為:
式中,E和N為消防員在ENU坐標(biāo)系中的東向和北向坐標(biāo),S為每一步的步長(zhǎng),ψ為每一步的航向角。
根據(jù)PDR算法,狀態(tài)方程為:
由于狀態(tài)方程(16)是非線性方程,根據(jù)EKF的原理,用狀態(tài)方程計(jì)算的雅各比矩陣近似表示狀態(tài)轉(zhuǎn)移矩陣:
動(dòng)態(tài)預(yù)測(cè)噪聲為:
觀測(cè)量為:
觀測(cè)方程為:
觀測(cè)方程是線性方程,觀測(cè)矩陣為:
觀測(cè)噪聲矩陣為:
通過EKF的預(yù)測(cè)更新和測(cè)量更新動(dòng)態(tài)方程,可以得到狀態(tài)變量的最優(yōu)估計(jì)值:
本次實(shí)驗(yàn)使用集成MPU9250傳感器的JY901作為實(shí)驗(yàn)數(shù)據(jù)采集平臺(tái)。JY901的主控部分STM32F051是一款基于ARM 32-bit的Cortex?-M0 CPU,具有32k到64k字節(jié)的Flash Memory,工作頻率高,功耗低,外設(shè)豐富且價(jià)格低。MPU9250是一款9軸MARG運(yùn)動(dòng)跟蹤傳感器,由3軸加速度計(jì)、3軸陀螺儀和3軸磁力計(jì)組成。加速度計(jì)誤差為±6mg,陀螺儀誤差為±5 (°)/s。經(jīng)過測(cè)試,根據(jù)MPU9250傳感器采集的慣性數(shù)據(jù),使用 Madgwick算法求取的原始航向角誤差在±10°范圍內(nèi)波動(dòng)。在實(shí)驗(yàn)中,設(shè)定傳感器的數(shù)據(jù)采集速率是50 Hz,工作電壓是3.3 V,波特率為115 200 Hz。實(shí)驗(yàn)場(chǎng)地是西安郵電大學(xué)東區(qū)逸夫樓八樓自習(xí)室,實(shí)驗(yàn)中慣性傳感器佩戴在測(cè)試人員胸前,傳感器獲取到的數(shù)據(jù)通過串口傳輸?shù)絇C端,由Matlab軟件來處理數(shù)據(jù),如圖5所示。
圖5 數(shù)據(jù)采集Fig.5 Data acquisition
為了驗(yàn)證本文提出的消防員室內(nèi)定位算法的性能,設(shè)計(jì)了3組實(shí)驗(yàn),測(cè)試人員胸前佩戴著傳感器,分別沿著矩形路線、三角形路線和半圓形路線進(jìn)行實(shí)地測(cè)試。
1)矩形路線實(shí)驗(yàn)
圖6 矩形實(shí)驗(yàn)航向角變化Fig.6 Variations of heading angle inrectangular experiment
矩形路線實(shí)驗(yàn)是沿著東、南、西、北四個(gè)主導(dǎo)方向行走一圈,以驗(yàn)證在這四個(gè)主導(dǎo)方向上直行時(shí)航向反饋修正算法的性能。圖6是測(cè)試人員行走過程中航向角的變化,通過對(duì)比分析,未經(jīng)修正的原始航向角波形不平整且偏離真實(shí)值,經(jīng)過航向反饋修正算法處理的航向角更加平滑并且較為接近真實(shí)航向值。圖7為矩形實(shí)驗(yàn)的行走軌跡,無(wú)航向修正的PDR算法軌跡圖在短時(shí)間嚴(yán)重偏離真實(shí)軌跡,加了航向反饋修正算法的軌跡圖始終接近真實(shí)軌跡。
如表1所示,使用的航向反饋修正算法的返回原位置誤差(Return Position Error,RPE)為0.9048 m,與未使用相比提升了46.84%,位置RMSE提升了77.90%,航向 RMSE提升了 65.03%。本次實(shí)驗(yàn)驗(yàn)證了在主導(dǎo)方向直行時(shí)航向反饋修正算法可以很好地修正航向。
圖7 矩形實(shí)驗(yàn)行走軌跡Fig.7 Rectangular experimental walking track
表1 矩形實(shí)驗(yàn)精度分析Tab.1 Accuracy analysis of rectangular experiment
2)三角形路線實(shí)驗(yàn)
在三角形路線實(shí)驗(yàn)中,行走軌跡為一個(gè)直角三角形,兩條直角邊在主導(dǎo)方向上,一條斜邊在非主導(dǎo)方向上。該實(shí)驗(yàn)是為了驗(yàn)證在非主導(dǎo)方向直行時(shí),航向反饋修正算法的性能。圖8為航向角變化情況,其中前兩段為在主導(dǎo)方向直行時(shí)的航向角變化,最后一段為在非主導(dǎo)方向直行時(shí)的航向角變化。圖9為三角形實(shí)驗(yàn)的行走軌跡,相比單純使用PDR算法,添加了航向反饋修正算法生成的軌跡更加接近真實(shí)軌跡。
圖8 三角形實(shí)驗(yàn)航向角變化Fig.8 Variations of heading angle in triangular experiment
圖9 三角形實(shí)驗(yàn)行走軌跡Fig.9 Triangle experimental walking track
如表2所示,添加的航向反饋修正算法 RPE為1.10m,與未使用航向修正算法相比提升了 45.54%,位置 RMSE提升了 76.81%,航向 RMSE提升了68.21%。三角形實(shí)驗(yàn)的結(jié)果表明在非主導(dǎo)方向直行時(shí)航向反饋修正算法具有良好的精度。
表2 三角形實(shí)驗(yàn)精度分析Tab.2 Accuracy analysis of triangle experiment
3)半圓形路線實(shí)驗(yàn)
半圓形路線實(shí)驗(yàn)設(shè)定為直徑在主導(dǎo)方向上,圓弧在非主導(dǎo)方向上,這是對(duì)航向反饋修正算法設(shè)計(jì)的挑戰(zhàn)性實(shí)驗(yàn)。半圓形路線實(shí)驗(yàn)的航向角變化如圖10所示:第一段是沿著直徑在主導(dǎo)方向上直行,航向反饋修正算法會(huì)修正航向角;第二段是沿著圓弧曲線行走,由于實(shí)際航向角在不斷變化,相比于直線行走,沒有一個(gè)固定的航向角觀測(cè)值,導(dǎo)致難以計(jì)算出航向角偏差,無(wú)法對(duì)航向角進(jìn)行直接修正,航向反饋修正算法性能會(huì)暫時(shí)下降,軌跡推算直接使用原始航向角。實(shí)驗(yàn)軌跡如圖11所示,由于真實(shí)行走軌跡的半圓不是規(guī)則的半圓,是一個(gè)近似的半圓,所以在本實(shí)驗(yàn)的軌跡圖中并沒有標(biāo)出真實(shí)的軌跡。本文提出的航向反饋修正算法框架融合了磁力計(jì),求出的原始航向角具有一定的精確度,在曲線行走航向修正算法性能下降的情況下,如圖11所示,所依據(jù)的原始航向信息依然具備一定的準(zhǔn)確性??紤]到在多數(shù)情況下人的行走是直線行走,所以在曲線行走時(shí)導(dǎo)致短暫的算法性能下降不會(huì)對(duì)最終定位精度產(chǎn)生較大的影響。
圖10 半圓形實(shí)驗(yàn)航向角變化Fig.10 Variations of heading angle in semicircular experiment
圖11 半圓形實(shí)驗(yàn)行走軌跡Fig.11 Semicircular experimental walking track
本文以MARG傳感器為基礎(chǔ),針對(duì)消防員室內(nèi)火場(chǎng)定位的應(yīng)用背景,設(shè)計(jì)了一種導(dǎo)航定位方法。針對(duì)PDR算法中的步數(shù)檢測(cè)問題,設(shè)計(jì)了一種根據(jù)加速度幅值波形使用峰值探測(cè)法和零點(diǎn)交叉法相結(jié)合的方法來準(zhǔn)確獲取消防員的行走步數(shù)。針對(duì)慣性導(dǎo)航中航向角誤差隨時(shí)間和行走距離增大的問題,提出了一種航向反饋修正算法來修正航向角。當(dāng)消防員沿著直線路徑行走時(shí),算法會(huì)對(duì)航向角進(jìn)行修正,修正后的航向角結(jié)合步數(shù)和步長(zhǎng)信息通過EKF和PDR算法來估計(jì)出消防員的位置坐標(biāo)。算法框架所融合的磁力計(jì)增加了航向信息的準(zhǔn)確度,即使消防員在非理想路徑行走時(shí)也可以獲得良好的定位精度。綜合3組實(shí)驗(yàn),本文所提算法將 RPE提升了 45%以上,航向角和位置的RMSE分別提升65%和76%以上,具有較好的穩(wěn)定性和可靠性,能適應(yīng)較為復(fù)雜的應(yīng)用場(chǎng)景。
本文所述的定位方法為消防員在室內(nèi)火場(chǎng)環(huán)境中的位置信息獲取提供了一種有效的解決方案。在后續(xù)的研究中,將考慮如何解決周圍磁場(chǎng)環(huán)境對(duì)磁力計(jì)測(cè)量精度的影響。磁力計(jì)擾動(dòng)的改善將有助于提高定位精度,進(jìn)而保障消防員的自身安全和提高救援效率。