鄧 平，朱飛翔，趙榮鑫

（西南交通大學(xué)信息編碼與傳輸重點(diǎn)實(shí)驗(yàn)室，成都 610097）

當(dāng)今城市建筑物內(nèi)部結(jié)構(gòu)錯(cuò)綜復(fù)雜，火災(zāi)現(xiàn)場(chǎng)危險(xiǎn)性極大，對(duì)消防員的人身安全構(gòu)成了極大的威脅。救災(zāi)時(shí)如果不能準(zhǔn)確確定消防員的位置所在，在消防員自身遇險(xiǎn)時(shí)就難以實(shí)施救援。GPS/北斗等全球衛(wèi)星定位導(dǎo)航系統(tǒng)雖然在室外定位精度比較高，但是在室內(nèi)環(huán)境一般缺乏有效信號(hào)覆蓋，很難進(jìn)行準(zhǔn)確、有效的定位[1]。基于WiFi、UWB、ZigBee、RFID 等設(shè)備的室內(nèi)定位技術(shù)一般都需要提前在建筑物內(nèi)部署信號(hào)源等基礎(chǔ)設(shè)施，難以在沒(méi)有基礎(chǔ)設(shè)施的一般建筑物內(nèi)實(shí)施室內(nèi)實(shí)時(shí)精準(zhǔn)定位[2-4]。慣性導(dǎo)航定位技術(shù)是一種可以全天候、全時(shí)段、實(shí)時(shí)工作，且工作時(shí)不依賴事先部署基礎(chǔ)設(shè)施，不易受到外界干擾的一種自主式定位導(dǎo)航技術(shù)。近年來(lái)隨著微電機(jī)（MEMS）技術(shù)的發(fā)展，基于MEMS 的慣性測(cè)量單元（IMU）的體積越來(lái)越小，測(cè)量精度越來(lái)越高，性能愈來(lái)愈強(qiáng)，使得基于IMU 的室內(nèi)定位技術(shù)的研究及應(yīng)用受到了人們的廣泛關(guān)注[5,6]。

基于IMU 進(jìn)行三維室內(nèi)定位，需要實(shí)時(shí)采集行人行走過(guò)程中的3 軸加速度、3 軸角速度、高度數(shù)據(jù)，進(jìn)而計(jì)算出步頻、步長(zhǎng)以及航向角，用于行人航位推算（PDR）定位。因此步頻檢測(cè)、步長(zhǎng)估計(jì)和航向估計(jì)的精確度決定了PDR 定位的性能優(yōu)劣。目前步頻檢測(cè)主要采用的方法有峰值檢測(cè)法[7]，零點(diǎn)交叉法[8]等，但研究中發(fā)現(xiàn)這類(lèi)方法在上下樓梯時(shí)會(huì)導(dǎo)致合加速度波形即使去掉重力分量，仍會(huì)關(guān)于X 軸不對(duì)稱(chēng)，造成使用峰值閾值的步頻檢測(cè)出現(xiàn)錯(cuò)漏判，產(chǎn)生一定的步頻檢測(cè)誤差。為此，本文在峰值檢測(cè)法的基礎(chǔ)上提出一種峰峰值探測(cè)法，即在三窗式滑動(dòng)窗口中先進(jìn)行無(wú)差別的波峰波谷識(shí)別，再進(jìn)行針對(duì)性的橫向時(shí)間閾值和縱向峰峰值閾值判決來(lái)完成步頻檢測(cè)。

在航向角的估計(jì)中由于陀螺儀自身不可避免會(huì)出現(xiàn)數(shù)據(jù)漂移現(xiàn)象，航向角勢(shì)必會(huì)隨著時(shí)間累計(jì)而產(chǎn)生不可逆的誤差[9,10]。為了降低僅靠陀螺儀數(shù)據(jù)進(jìn)行航向判斷的誤差，文獻(xiàn)[11]采用陀螺儀、磁力計(jì)等數(shù)據(jù)，利用互補(bǔ)濾波結(jié)合PI 調(diào)節(jié)控制來(lái)校正陀螺儀零偏，但磁力計(jì)受室內(nèi)環(huán)境中的鋼結(jié)構(gòu)、金屬等干擾嚴(yán)重，其測(cè)量值會(huì)出現(xiàn)嚴(yán)重的偏差。文獻(xiàn)[12]采用粒子濾波和地圖匹配技術(shù)得到最佳航向估計(jì)，但由于室內(nèi)環(huán)境復(fù)雜多變，地圖匹配工作量大不易實(shí)現(xiàn)。文獻(xiàn)[13]提出了一種啟發(fā)式偏移消除算法(Heuristic Drift Elimination，HDE)，利用建筑物90 °主方向作為約束條件對(duì)角速度進(jìn)行反饋修正，取得了較好效果。但是隨著現(xiàn)代建筑設(shè)計(jì)風(fēng)格的多樣化，室內(nèi)建筑主方向不僅僅是橫平豎直，諸如環(huán)形、弧形等多種不規(guī)則轉(zhuǎn)彎方向使得室內(nèi)行人行走路徑轉(zhuǎn)向逐漸增多，這類(lèi)場(chǎng)景下HDE 算法的航向角估計(jì)精度已不能滿足室內(nèi)精確定位導(dǎo)航的需求。為此，本文在HDE 算法的基礎(chǔ)上，細(xì)化設(shè)定了新的建筑物主方向角度，設(shè)計(jì)了不同方向行走路徑判別規(guī)則，對(duì)沿著建筑主方向直行、非主方向直行以及轉(zhuǎn)彎時(shí)，分別用不同的反饋系數(shù)直接去準(zhǔn)確修正航向角，有效提高了航向角的估計(jì)精度。

1 步頻探測(cè)與步長(zhǎng)估計(jì)

基于捷聯(lián)式慣性導(dǎo)航器件的室內(nèi)航跡推算，需要在一定的坐標(biāo)系下開(kāi)展研究。其中定義運(yùn)載體坐標(biāo)系是慣性導(dǎo)航器件自身定義的坐標(biāo)系（b 系），向右為X軸，向上為Y 軸，垂直模塊向外為Z 軸；選擇大地地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系（n 系），向東為X 軸，向北為Y 軸，垂直向天空為Z 軸。本文算法利用IMU 采集的加速度、角速度、高度、四元素等數(shù)據(jù)信息進(jìn)行PDR 推算實(shí)時(shí)確定消防員的位置。

當(dāng)消防員處于行走或者奔跑時(shí)會(huì)產(chǎn)生3 個(gè)方向的加速度值。由于本文設(shè)計(jì)的單人慣導(dǎo)定位裝置穿戴在腰間，消防員移動(dòng)時(shí)由于其運(yùn)動(dòng)方向的不確定性，導(dǎo)致定位裝置不可能完全水平或者豎直，故沒(méi)有一個(gè)方向軸能夠保持不動(dòng)，因此我們采用合加速度，即三軸加速度之和來(lái)檢測(cè)加速度的周期性變化：

式中，α t為t時(shí)刻的三軸合加速度幅值，分別為t時(shí)刻加速度計(jì)的三軸加速度值。步頻探測(cè)時(shí)合加速度的峰值檢測(cè)至關(guān)重要，若采用單個(gè)固定時(shí)間的滑動(dòng)窗口提取時(shí)域峰值，對(duì)不同行進(jìn)速度的適應(yīng)性較差，而且對(duì)單個(gè)滑動(dòng)窗口的數(shù)據(jù)進(jìn)行濾波時(shí)，一方面加速度幅值下降會(huì)給步長(zhǎng)估計(jì)帶來(lái)誤差，另一方面波形失真會(huì)導(dǎo)致峰值提取丟失。采用峰值探測(cè)法[7]時(shí)，通常采用固定門(mén)限閾值來(lái)判決波峰波谷，則可能會(huì)因?yàn)樯舷聵翘莸葟?fù)雜行走姿態(tài)，導(dǎo)致合加速度波形不是關(guān)于X 軸對(duì)稱(chēng)而出現(xiàn)錯(cuò)漏判現(xiàn)象。

針對(duì)上述問(wèn)題，本文將采集的合加速度數(shù)據(jù)輸入一個(gè)雙端隊(duì)列，并將隊(duì)列分解成三個(gè)窗口，選取中間窗口進(jìn)行峰值提取，而前后兩個(gè)窗口僅參與數(shù)據(jù)濾波、峰值保存過(guò)程，目的是防止滑動(dòng)窗口中遭截?cái)嗟淖笥疫吔缣幉ǚ宀ü葦?shù)據(jù)因?yàn)V波而產(chǎn)生失真，導(dǎo)致當(dāng)前峰值低于所設(shè)的峰值門(mén)限閾值進(jìn)而造成峰值漏檢。

圖1 單窗和三窗峰值檢測(cè)Fig.1 Single window and three window peak detection

如圖1 所示，分別是單個(gè)固定時(shí)間的滑動(dòng)窗口和三窗式滑動(dòng)窗口在綠色虛線框中對(duì)同一個(gè)采樣周期內(nèi)的數(shù)據(jù)進(jìn)行時(shí)域峰值提取，從圖1 中紅色虛線框中可以看出，單個(gè)固定時(shí)間的滑動(dòng)窗口在邊界處即采樣點(diǎn)20 附近，因?yàn)V波造成加速度波谷幅值減小進(jìn)而造成波谷值提取丟失，而對(duì)應(yīng)的三窗式滑動(dòng)窗口由于前后兩窗口參與了濾波，在采樣點(diǎn)210 附近未發(fā)生波谷提取丟失現(xiàn)象。

對(duì)中間窗口進(jìn)行時(shí)域峰值提取，采用的是本文提出的一種峰峰值探測(cè)法，即首先經(jīng)過(guò)低通濾波濾除抖動(dòng)，然后進(jìn)行波峰波谷探測(cè)無(wú)差別粗檢，再進(jìn)行縱橫經(jīng)驗(yàn)閾值針對(duì)性細(xì)檢。無(wú)差別粗檢是指進(jìn)行峰值探測(cè)時(shí)無(wú)需設(shè)置經(jīng)驗(yàn)閾值進(jìn)行判別，探明為波峰或者波谷即存入相應(yīng)的數(shù)據(jù)隊(duì)列中暫存；針對(duì)性細(xì)檢是指利用橫向的時(shí)間經(jīng)驗(yàn)閾值和縱向的幅值經(jīng)驗(yàn)閾值進(jìn)行判決，其中縱向的閾值我們?cè)O(shè)置為波峰、波谷隊(duì)列中對(duì)應(yīng)的波峰值和波谷值差值的絕對(duì)值。測(cè)試結(jié)果表明（采樣頻率200 Hz），正常行走時(shí)，橫向時(shí)間上，一步大概在50-60 個(gè)采樣點(diǎn)之間，典型值為55；縱向幅值上，一步的峰峰值應(yīng)大于2.3。如圖2 所示，是使用普通峰值檢測(cè)法和本文峰峰值探測(cè)法，分別對(duì)樓梯行走過(guò)程中的合加速度波形進(jìn)行有效峰值探測(cè)，其中向上箭頭為有效波峰值，向下箭頭為有效波谷值。從圖2 紅色虛線矩形框中對(duì)比可看出當(dāng)加速度波形不是關(guān)于X 軸對(duì)稱(chēng)時(shí)，峰值檢測(cè)法中會(huì)產(chǎn)生明顯錯(cuò)漏判，而峰峰值探測(cè)法則不會(huì)產(chǎn)生。

圖2 峰值檢測(cè)與峰峰值探測(cè)Fig.2 Peak and peak-to-peak detection

本文采用Harvey Weinderg 等人提出非線性步長(zhǎng)模型[14]來(lái)實(shí)時(shí)估計(jì)每一步步長(zhǎng)，經(jīng)驗(yàn)公式如下：

式中，k為跟身高、步頻等參數(shù)有關(guān)的常數(shù)，可通過(guò)訓(xùn)練來(lái)獲得；Amax、Amin分別是波峰、波谷隊(duì)列中存儲(chǔ)的每一步的加速度的最大值和最小值。

綜上，本文設(shè)計(jì)的步頻探測(cè)與步長(zhǎng)估計(jì)算法描述如下：

1、給定一個(gè)固定長(zhǎng)度為N的隊(duì)列Q；

2、先填充N(xiāo)/3 冗余數(shù)據(jù)進(jìn)隊(duì)首，為后續(xù)濾波做準(zhǔn)備；

3、隊(duì)列N個(gè)數(shù)據(jù)填滿后做低通濾波、去重力等數(shù)據(jù)處理；

4、進(jìn)行峰峰值探測(cè)，獲取N/3－2N/3 區(qū)間探測(cè)的步頻和步長(zhǎng)等數(shù)據(jù)；

5、隊(duì)首N/3 個(gè)數(shù)據(jù)出隊(duì)列，等待隊(duì)尾進(jìn)N/3 個(gè)數(shù)據(jù)后，重復(fù)第4 步操作。

2 航向角準(zhǔn)確估計(jì)

本文采用的JY901B 慣導(dǎo)模塊能夠直接輸出四元數(shù)實(shí)時(shí)信息并更新，故可先對(duì)運(yùn)載體坐標(biāo)下的三軸矢量角速度用四元數(shù)表示的姿態(tài)變換矩陣進(jìn)行坐標(biāo)系變換，變換成導(dǎo)航坐標(biāo)系下對(duì)應(yīng)的角速度，再進(jìn)行積分即可得到原始的航向角ψ。運(yùn)載體坐標(biāo)系下的三軸矢量角速度可以表示為導(dǎo)航坐標(biāo)系下的三軸矢量角速度表示為載體坐標(biāo)系到導(dǎo)航坐標(biāo)系下的姿態(tài)旋轉(zhuǎn)矩陣為：

其中，q0、q1、q2、q3表示四元數(shù)。然后可按式(4)把轉(zhuǎn)換為：

此時(shí)只需要對(duì)分量進(jìn)行積分即可以得到角度增量，加上前一個(gè)時(shí)刻的航向角ψt1，可得到當(dāng)前時(shí)刻的航向角ψt2：

式中t1～t2的時(shí)間間隔為一個(gè)采樣周期。為單軸陀螺在t時(shí)刻的角速度數(shù)值。

2.1 HDE 航向角估計(jì)算法

啟發(fā)式偏移消除（Heuristic Drift Elimination，HDE）算法[13]框圖如圖3 所示。

圖3 HDE 算法框圖Fig.3 Framework of HDE algorithm

圖中，ψ為航向角、Ii為反饋?zhàn)兞?、ic為反饋系數(shù)、sgn(Ei)為符號(hào)函數(shù)，定義為：

MOD(ψi-1, Δ )是用來(lái)計(jì)算前一時(shí)刻航向角相對(duì)于當(dāng)前主方向的偏移角，定義為：

其中，INT(k) 為取整函數(shù)，結(jié)果為不大于k值的最大整數(shù)。所以MOD函數(shù)的結(jié)果在區(qū)間[0,m]內(nèi)。

2.2 改進(jìn)的航向角準(zhǔn)確修正算法

HDE 算法將航向角的主方向設(shè)置為4 個(gè)或8 個(gè)，角度間隔Δ=90 °或45 °，此時(shí)HDE 算法會(huì)將當(dāng)前航向角朝著假設(shè)的建筑主方向拉攏，即通過(guò)減小當(dāng)前航向與主方向之間的差值進(jìn)行修正。但是在室內(nèi)救災(zāi)時(shí)，消防員除沿著建筑通常的主方向行走以外，還可能存在非建筑主方向的直行以及連續(xù)大、小角度轉(zhuǎn)彎的情況[15]，這些情況可能導(dǎo)致HDE 算法對(duì)航向角的過(guò)度修正甚至修正失效[16]。為此本文首先細(xì)化設(shè)定16 個(gè)主方向來(lái)描述現(xiàn)代建筑內(nèi)樓道轉(zhuǎn)彎可能出現(xiàn)的各種情況，主方向間的角度間隔Δ=22.5 °；然后通過(guò)區(qū)分直線行走和轉(zhuǎn)彎的不同行走狀態(tài)，以及在直線行走的前提下，區(qū)分沿著建筑主方向行走和非建筑主方向行走兩種不同狀態(tài)[17]。區(qū)分了行走狀態(tài)后，對(duì)于沿著非建筑主方向和轉(zhuǎn)彎的行走路徑，減小HDE 算法的反饋系數(shù)ic，以減緩航向角方向修正回當(dāng)前的偽主方向，從而降低系統(tǒng)對(duì)用來(lái)修正航向角的反饋?zhàn)兞縄i的敏感性，達(dá)到航向角準(zhǔn)確修正目的。改進(jìn)的航向角準(zhǔn)確修正算法（IHDE）原理如圖4 所示。

圖4 改進(jìn)的航向角準(zhǔn)確修正算法Fig.4 Improved algorithm for accurate correction of heading

1）主方向的設(shè)定

如圖5 所示，為本文設(shè)定的16 個(gè)主方向，主方向的角度間隔Δ=22.5 °，由于未使用磁力計(jì)，設(shè)置初始航向角ψ0=0 °。將主導(dǎo)方向細(xì)分為16 個(gè)方向是為了應(yīng)對(duì)長(zhǎng)時(shí)間沿著非主方向或者持續(xù)小角度轉(zhuǎn)彎行走的情況，以減緩HDE 算法對(duì)角度的過(guò)度修正，增強(qiáng)算法適應(yīng)各種不同室內(nèi)道路環(huán)境的能力。

圖5 16 個(gè)主方向Fig.5 16 main directions

2）直行的判決

兩年來(lái)，酒店共計(jì)接待顧客15萬(wàn)余人次。同時(shí)，酒店的高品質(zhì)服務(wù)工作得到當(dāng)?shù)卣某浞终J(rèn)可，兩年來(lái)共接待國(guó)家、省、市領(lǐng)導(dǎo)30余次。各種國(guó)際化會(huì)議和民間友好活動(dòng)選擇在這里舉辦，越來(lái)越多的外國(guó)賓客也紛至沓來(lái)。通過(guò)優(yōu)化服務(wù)工作，提高管理水平和服務(wù)質(zhì)量，讓酒店有了更高、更快的發(fā)展。憑借和諧團(tuán)結(jié)的隊(duì)伍，完美高效的服務(wù)，酒店將延續(xù)這份業(yè)績(jī)，在今后實(shí)現(xiàn)新突破。

由于IMU 模塊采用腰部佩戴的方式，航向角曲線有著明顯的特征。圖6 是算法測(cè)試時(shí)行走的兩段轉(zhuǎn)彎和三段直行的航向角曲線圖，其中紅色點(diǎn)為直行每一步的航向角，綠色點(diǎn)為轉(zhuǎn)彎每一步的航向角。

圖6 直行和轉(zhuǎn)彎時(shí)的航向角Fig.6 Heading for turning and going straight

對(duì)行走中的航向角進(jìn)行時(shí)域特征提取并分類(lèi)。假定連續(xù)3 步的航向角數(shù)值分別為ψi-2、ψi1-、ψi，如上圖6 中所示，可分為4 類(lèi)：第1 類(lèi)為T(mén)1 直行類(lèi)，可采用3 步航向數(shù)值分別進(jìn)行差分相乘后，得到判決條件C1= (ψi-ψi-1) × (ψi-1-ψi-2)，當(dāng)C1< 0 時(shí)，則判決當(dāng)前在直行；第2 類(lèi)為T(mén)2 直行變轉(zhuǎn)彎類(lèi)，航向角ψ i仍滿足條件C1< 0，但是此時(shí)已經(jīng)處于轉(zhuǎn)彎狀態(tài)，顯然發(fā)生了誤判。此時(shí)可對(duì)相鄰的航向角數(shù)值進(jìn)行差分求絕對(duì)值后，得到判決條件C2= |ψi-ψi-1|，當(dāng)同時(shí)滿足C1< 0 和C2<θ時(shí)，則可以防止誤判發(fā)生；第3 類(lèi)為T(mén)3 轉(zhuǎn)彎類(lèi)，可用連續(xù)的3 步航向數(shù)值兩兩差分取絕對(duì)值后 ， 得判決條件C3= (|ψ i-1-ψi-2|＜β and|ψ i-ψi-1|＜β)，當(dāng)C3為true 成立時(shí)，可判決當(dāng)前在走直線，反之則在轉(zhuǎn)彎；第4 類(lèi)為T(mén)4 轉(zhuǎn)彎變直行類(lèi)，當(dāng)航向角ψ i滿足C1< 0 時(shí)，即可判斷當(dāng)前在直行。

綜上，是否直行可按式(8)進(jìn)行判斷。

當(dāng)S1為1 的時(shí)候判定為直線行走，可以進(jìn)入下一項(xiàng)主方向判決；當(dāng)S1為0 時(shí)為轉(zhuǎn)彎，需修正反饋系數(shù)ic。θ、β為相鄰兩航向角度差值的經(jīng)驗(yàn)門(mén)限閾值，一般取15 °、10 °。

3）主方向的判決

對(duì)于直行前提下判斷是否沿著建筑主方向行走，先選擇4 個(gè)主方向，其角度間隔Δ=90 °。原因是一方面22.5 °的倍數(shù)中雖包含了4 個(gè)主方向，但還有很多非主方向；另一方面正常行走過(guò)程中航向值波動(dòng)在正負(fù)10 °左右，Δ=22.5 °的主方向判決條件比較苛刻，難以實(shí)現(xiàn)。

故先對(duì)連續(xù)的3 步航向角數(shù)值求出其均值后除以360 °取余數(shù)得到的一個(gè)角度值為μ，再運(yùn)用式(7)令n=μ、m= 90°把μ映射到第一象限中去，最后選擇與象限軸夾角最小的角度σ=min(μ, 90° -μ)，如圖7所示。主方向判決公式如式9 所示。

圖7 主方向的偏移角度Fig.7 Offset angle of the dominant direction

3 PDR 航跡推算算法

將上述改進(jìn)的航向角修正算法解算出來(lái)的航向角ψ i，與探測(cè)的步長(zhǎng)d以及高度h相結(jié)合，便可以在已知上一時(shí)刻的坐標(biāo)位置基礎(chǔ)上，按式(10)PDR 推算出當(dāng)前的位置坐標(biāo)：

式中，Ek、Nk、Hk分別為相對(duì)位置坐標(biāo)系中的X、Y、Z 軸距離分量，其中（E0、N0、H0）是三維坐標(biāo)系中的初始點(diǎn)。

推算定位時(shí)本文首先對(duì)采集的三軸合加速度進(jìn)行去重力、濾波，角速度進(jìn)行坐標(biāo)系轉(zhuǎn)換并積分，高度數(shù)據(jù)進(jìn)行卡爾曼濾波，然后對(duì)三軸合加速度進(jìn)行峰峰值探測(cè)獲取步頻、步長(zhǎng)、高度等信息，對(duì)角速度坐標(biāo)轉(zhuǎn)換、積分得到的角度進(jìn)行航向角修正，最后由式(10)計(jì)算出消防員的當(dāng)前相對(duì)位置坐標(biāo)。算法架構(gòu)如圖8所示。

圖8 算法架構(gòu)Fig.8 Algorithm architecture

4 實(shí)驗(yàn)結(jié)果及分析

本文采用 JY901B慣導(dǎo)模塊以及STM32F103C8T6 單片機(jī)設(shè)計(jì)了定位數(shù)據(jù)采集裝置，裝置系于腰間中間部位，如圖9 所示。定位裝置采集的數(shù)據(jù)通過(guò)串口傳輸?shù)絇C，由Python 軟件進(jìn)行PDR等后續(xù)處理。傳感器的數(shù)據(jù)采集速率為200 Hz，波特率為460800 Hz，工作電壓為3.3 V。實(shí)驗(yàn)場(chǎng)地為西南交通大學(xué)9 號(hào)教學(xué)樓。

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

1）峰峰值探測(cè)實(shí)驗(yàn)

實(shí)驗(yàn)?zāi)繕?biāo)在教學(xué)樓A 區(qū)的一樓到三樓的樓梯中行走，先上樓再下樓，回到起點(diǎn)，目的是驗(yàn)證本文提出的峰峰值探測(cè)法的有效性。如圖10 所示，是分別采用峰值檢測(cè)法和峰峰值探測(cè)法后推算出的行走軌跡對(duì)比。從圖中看，使用峰值檢測(cè)法時(shí)，由于多處發(fā)生錯(cuò)漏判，導(dǎo)致軌跡偏離真實(shí)軌跡較多，而使用峰峰值探測(cè)法時(shí)，軌跡明顯優(yōu)于前者且接近真實(shí)軌跡。在定位誤差上，使用峰值檢測(cè)法的首尾均方根（RMSE）定位誤差為2.37 m，平均水平定位誤差為2.33 m，而峰峰值探測(cè)法的對(duì)應(yīng)誤差分別為0.69 m 和0.68 m。實(shí)驗(yàn)結(jié)果表明當(dāng)合加速度波形不是關(guān)于X 軸對(duì)稱(chēng)時(shí)，峰峰值探測(cè)法有著更好的探測(cè)精度。

圖10 上下樓梯軌跡Fig.10 The trajectory of stairway up and down

2）弧形三角行走實(shí)驗(yàn)

弧形三角實(shí)驗(yàn)是沿著圖9 中標(biāo)注的一塊草坪小道C 區(qū)行走，全程為133 m，路徑中的兩條直角邊在主方向上，弧形斜邊在非主方向上，目的是驗(yàn)證沿著非主方向長(zhǎng)時(shí)間行走時(shí)本文提出的航向角準(zhǔn)確修正算法的性能。本文對(duì)比了主方向間隔分別為90 °、22.5 °，采用HDE 算法航向角修正及本文航向角修正算法（IHDE）的性能，圖11 分別是三種算法的航向角曲線和PDR 推算的行走軌跡。從圖中可見(jiàn)，在弧形斜邊行走時(shí)，本文算法的航向角和軌跡更加接近真實(shí)值。

圖11 航向角曲線和行走軌跡Fig.11 Curve of heading and track

表1 所示為三種算法到終點(diǎn)時(shí)的航向角以及定位誤差多次實(shí)驗(yàn)的統(tǒng)計(jì)結(jié)果，進(jìn)一步驗(yàn)證了本文算法的有效性。本實(shí)驗(yàn)得到的定位精度接近文獻(xiàn)[18]三角形實(shí)驗(yàn)的結(jié)果，但是本文行走軌跡為弧形三角且行走的距離約為前者的2.6 倍，實(shí)驗(yàn)條件更加苛刻，表明本文算法對(duì)不同的道路環(huán)境的魯棒性。

表1 弧形三角實(shí)驗(yàn)精度分析Tab.1 Precision analysis of arc trigonometric experiment

3）繞圈實(shí)驗(yàn)

沿著9 號(hào)教學(xué)樓B 區(qū)上的起點(diǎn)開(kāi)始行走10 圈回到起點(diǎn)，全程共行走約2340 m，一圈路線中有5 段是處于主方向，1 段非主方向。圖12 分別是基于HDE算法及本文IHDE 算法得到的行走軌跡，結(jié)果表明本文算法能在較長(zhǎng)時(shí)間、較大范圍內(nèi)實(shí)行對(duì)室內(nèi)消防員的行走軌跡實(shí)現(xiàn)準(zhǔn)確定位跟蹤。實(shí)驗(yàn)測(cè)得首尾誤差為2.33 m，水平定位誤差為0.1%D。

圖12 HDE 和IHDE 算法行走的軌跡Fig.12 The trajectory of the HDE and IHDE algorithms

相比于文獻(xiàn)[19]中的非矩形實(shí)驗(yàn)，本文算法有著更高的定位精度和更長(zhǎng)的續(xù)航時(shí)間。

4）多樓層行走實(shí)驗(yàn)

實(shí)驗(yàn)地點(diǎn)為西南交通大學(xué)9 號(hào)教學(xué)樓，范圍為負(fù)一樓到四樓，從樓外進(jìn)入，先在地下室繞墻柱環(huán)繞行走一圈，接著爬樓梯到地上每一層行走一圈，全程行走了大約1123 m。本實(shí)驗(yàn)?zāi)康氖悄M消防員救災(zāi)的復(fù)雜室內(nèi)場(chǎng)景，且在地下室繞柱行走以及爬4 層樓梯及途中轉(zhuǎn)彎都沿著非主方向行走，可檢驗(yàn)本文算法對(duì)復(fù)雜場(chǎng)景的適應(yīng)性。圖13 所示是測(cè)試得到的兩種算法的行走軌跡，圖14 是本文改進(jìn)算法IHDE 的3d 行走軌跡。

圖13 HDE 和IHDE 算法多樓層行走軌跡Fig.13 The multi-floor trajectory of the HDE and IHDE algorithm

實(shí)驗(yàn)結(jié)果表明，本文算法對(duì)于復(fù)雜實(shí)驗(yàn)環(huán)境具有更好的適應(yīng)性，定位得到的行走軌跡更接近真實(shí)軌跡，對(duì)非主方向上的航向角漂移有著很好的抑制作用。在定位誤差方面，本實(shí)驗(yàn)的首尾誤差僅0.77 m，水平定位誤差僅為0.069%D；而HDE 算法的首尾誤差達(dá)到了7.51 m，水平定位誤差為0.67%D，本文算法誤差降低了一個(gè)數(shù)量級(jí)。

圖14 多樓層行走3d 軌跡Fig.14 Multi-floor 3d tracks

4 結(jié) 論

針對(duì)消防員室內(nèi)搶險(xiǎn)救災(zāi)時(shí)對(duì)消防員自身的定位問(wèn)題，實(shí)現(xiàn)在多樓層、上下樓梯、轉(zhuǎn)彎時(shí)有多種轉(zhuǎn)向角度等復(fù)雜行走場(chǎng)景下對(duì)消防員的準(zhǔn)確定位跟蹤，通過(guò)提出一種峰峰值步頻探測(cè)法，提高了上下樓梯步頻探測(cè)的精度；對(duì)航向角修正定義了更小的主方向間隔及更準(zhǔn)確的航向角修正方法。實(shí)驗(yàn)測(cè)試結(jié)果表明，本文算法對(duì)于各種復(fù)雜室內(nèi)環(huán)境具有良好的適應(yīng)性，能夠準(zhǔn)確定位跟蹤的有效續(xù)航時(shí)間長(zhǎng)、定位精度高，能夠較好地達(dá)到對(duì)消防員室內(nèi)準(zhǔn)確定位的目的。