吉俊杰，蔣佳杰，姚航宇，楊敏之，吳向臣

(1. 華東宜興抽水蓄能有限公司，江蘇 宜興 214200;2. 河海大學(xué) 計(jì)算機(jī)與信息學(xué)院，南京 211100)

0 引言

隨著移動(dòng)互聯(lián)網(wǎng)技術(shù)的快速發(fā)展，對(duì)位置服務(wù)的需求也快速增長(zhǎng)，人們?cè)谌粘Ｉ钪懈宇l繁地使用定位導(dǎo)航技術(shù)。在室外環(huán)境中，全球衛(wèi)星定位系統(tǒng)導(dǎo)航技術(shù)已發(fā)展成熟[1]，但在室內(nèi)環(huán)境中，衛(wèi)星信號(hào)由于建筑物的遮擋無(wú)法發(fā)揮作用，無(wú)法提供準(zhǔn)確的位置信息。隨著移動(dòng)設(shè)備和智能手機(jī)的普及，及其內(nèi)部集成傳感器的快速發(fā)展，鑒于其精準(zhǔn)度高且制造成本低，以智能設(shè)備內(nèi)部集成傳感器系統(tǒng)為基礎(chǔ)的室內(nèi)定位系統(tǒng)算法的研究具有廣闊的應(yīng)用前景[2-4]。例如在抽蓄電站廠房中，因安全性要求與地理位置限制依賴無(wú)線信號(hào)的定位方法難于適用，此時(shí)僅利用傳感器信號(hào)采樣與分析的定位方法便體現(xiàn)了其良好的適應(yīng)性。

行人航位推算(PDR，pedestrian dead reckoning)是室內(nèi)定位的一種常用方法，它利用行人運(yùn)動(dòng)過(guò)程中采集的加速度、磁場(chǎng)強(qiáng)度等數(shù)據(jù)推算出行人每一步的步長(zhǎng)與航向，更新行人位置坐標(biāo)，達(dá)到定位目的[5-6]。行人航位推算算法主要包含步數(shù)檢測(cè)(計(jì)步)、步長(zhǎng)估計(jì)和航向估計(jì)三方面內(nèi)容?，F(xiàn)今步數(shù)檢測(cè)的主要方法有峰值檢測(cè)法[7-8]，零速修正法[9]，自相關(guān)分析法[10]，動(dòng)態(tài)時(shí)間規(guī)整算法以及步長(zhǎng)周期識(shí)別等方法[11-12]。其中，峰值檢測(cè)法實(shí)現(xiàn)簡(jiǎn)單但是算法窗口的設(shè)定難以適用于不同的行走狀態(tài)；零速修正法需要嚴(yán)苛的設(shè)備條件，難以推廣；自相關(guān)分析法、動(dòng)態(tài)時(shí)間規(guī)整以及步長(zhǎng)周期識(shí)別3種方法秉承相同的設(shè)計(jì)理念，其計(jì)算復(fù)雜度高，易受噪音干擾。步長(zhǎng)估計(jì)的精度高低影響定位算法的效果，現(xiàn)有的步長(zhǎng)估計(jì)有常數(shù)步長(zhǎng)模型，線性步長(zhǎng)模型[13]，倒立單擺模型[14]和非線性模型[15-16]等。然而，常數(shù)步長(zhǎng)模型準(zhǔn)確度較差；倒立單擺模型難以適用于不同行步狀態(tài)；非線性步長(zhǎng)模型參數(shù)較少難以泛化。在航向估計(jì)方面，傳統(tǒng)的基于方向角與陀螺儀進(jìn)行航向估計(jì)算法會(huì)產(chǎn)生航向角誤差累計(jì)的問(wèn)題。

航位推算作為一種無(wú)外源的室內(nèi)定位方法主要依賴加速度傳感器、陀螺儀傳感器以及磁場(chǎng)強(qiáng)度傳感器實(shí)時(shí)獲取數(shù)據(jù)進(jìn)行分析，計(jì)算出行人的步態(tài)、步長(zhǎng)以及航向信息，再對(duì)這3種信息進(jìn)行動(dòng)態(tài)融合以推算出行人的實(shí)時(shí)運(yùn)動(dòng)軌跡，從而實(shí)現(xiàn)室內(nèi)定位。

基于以上分析，本文對(duì)行人航位推算算法進(jìn)行了系統(tǒng)性研究，對(duì)相關(guān)計(jì)步方法、步長(zhǎng)估計(jì)方法和航向估計(jì)方法分別進(jìn)行了改進(jìn)，主要工作如下：

1)在計(jì)步上，針對(duì)傳統(tǒng)的峰值檢測(cè)算法過(guò)度計(jì)步問(wèn)題，提出一種結(jié)合自相關(guān)分析法和峰值檢測(cè)法的計(jì)步算法；通過(guò)實(shí)驗(yàn)發(fā)現(xiàn)，改進(jìn)計(jì)步算法優(yōu)于傳統(tǒng)峰值檢測(cè)法，還在一定程度上增強(qiáng)了抗干擾能力。

2)在估計(jì)上，提出一種基于Scarlett[17]模型融合前一步步長(zhǎng)的改進(jìn)步長(zhǎng)估算模型。對(duì)Weinberg[18]、Scarlett和Kim[19]3種典型非線性步長(zhǎng)模型做了對(duì)照試驗(yàn)，選取表現(xiàn)最優(yōu)的Scarlett模型進(jìn)行改進(jìn)，將前一步步長(zhǎng)作為自適應(yīng)參數(shù)融入步長(zhǎng)模型，實(shí)驗(yàn)結(jié)果表明該模型相較于Scarlett模型在精度上有所提升。

3)在航向估計(jì)上，基于方向傳感器算法，提出通過(guò)設(shè)定主導(dǎo)航向，并利用卡爾曼濾波對(duì)行人沿直線行走時(shí)的航向進(jìn)行處理來(lái)降低航向估計(jì)誤差的方法；實(shí)驗(yàn)結(jié)果表明改進(jìn)后方法減少了航向角的誤差，提升了航向估計(jì)的穩(wěn)定性。

1 步數(shù)檢測(cè)算法改進(jìn)

1.1 人體行步原理

如圖1中，人體行進(jìn)步伐的一個(gè)周期由兩步構(gòu)成，可以詳解為七個(gè)階段[20]。第一階段右腳邁出，腳后跟著地，左腳抬起離地；第二階段右腳腳掌與地面接觸，腿未完全伸直，左腳離地有向前的趨勢(shì)；第三階段右腿站定，此刻人處于比較穩(wěn)定的狀態(tài)，左腳邁出向前；第四階段左腳向前跨步完成，左腳后跟即將接觸地面，此時(shí)落后的右腳被帶起，腳后跟稍許離開(kāi)地面，這個(gè)階段人體重心處于變化狀態(tài)；第五階段左腳基本站定，身體帶動(dòng)右腳向前；第六階段重心穩(wěn)定，右腿屈膝開(kāi)始向前邁動(dòng)；第七階段人體微微向前，重心前移，右腿邁出，最終回到第一階段。

圖1 人體步數(shù)周期圖

對(duì)人體行進(jìn)步伐的一個(gè)周期進(jìn)行分類，以重心狀態(tài)分類可以將整個(gè)過(guò)程劃分為右雙支撐期，右單支撐期，左雙支撐期和左單支撐期，分別占人體行進(jìn)步伐周期的10%，40%，10%和40%。以右腿狀態(tài)分類可以將整個(gè)過(guò)程劃分為右站立相和右邁步相，即指定右邊，右腳足跟著地和在地面支撐的時(shí)間為右站立相，右腳邁步不與地面接觸的時(shí)間為右邁步相，約占40%。

分析可見(jiàn)，人的行步過(guò)程具有周期性，要想對(duì)步數(shù)進(jìn)行識(shí)別從而分析人的行為最終為定位服務(wù)，還須從數(shù)據(jù)分析入手。從模型圖和日常生活體驗(yàn)來(lái)看，在人平常走動(dòng)時(shí)，無(wú)論垂直方向還是水平方向，都會(huì)出現(xiàn)加速階段和減速階段交替的情況，依據(jù)各個(gè)階段人行走的特征，可以更容易理解行人行走過(guò)程中加速度值的周期性變化。如在第一階段中，因?yàn)槟_跟著地與地面撞擊造成瞬間反沖力從而產(chǎn)生較大的瞬時(shí)加速度，故在垂直方向上，加速度值會(huì)達(dá)到一個(gè)頂峰。在右站立相這一階段內(nèi)，人體重心較為穩(wěn)定，左腳擺動(dòng)，水平方向也會(huì)有對(duì)應(yīng)加速度值的波動(dòng)。根據(jù)以上對(duì)不同計(jì)步方法的特征分析，選取峰值檢測(cè)法作為改進(jìn)對(duì)象。

1.2 峰值檢測(cè)算法改進(jìn)

在峰值檢測(cè)法基礎(chǔ)上自適應(yīng)峰值檢測(cè)法取得了良好的效果，能夠同時(shí)較好地適應(yīng)傳感器姿態(tài)和運(yùn)動(dòng)狀態(tài)變化，實(shí)現(xiàn)高準(zhǔn)確率計(jì)步。但這仍受限于諸多條件，難以直接將原算法應(yīng)用到各種復(fù)雜的實(shí)際行步環(huán)境中并形成良好的自適應(yīng)性與泛化性的計(jì)步效果[19-20]。本文結(jié)合傳感器的放置位置與實(shí)驗(yàn)參與者的行走狀態(tài)，設(shè)計(jì)了一種基于峰值檢測(cè)與自適應(yīng)的改進(jìn)算法，如算法1所示，步驟如下：

1)計(jì)算整體加速度，將三軸加速度數(shù)據(jù)合成表示以更好地反映人體垂直加速度變化，同時(shí)消除部分噪聲，有利于更好地測(cè)算人體步態(tài)。

2)將滑動(dòng)窗口sliding window設(shè)為20，用以遍歷獲得潛在峰值，再根據(jù)加速度閾值進(jìn)行判斷，加速度閾值α、β分別設(shè)定為1.25 g、3.5 g。

3)計(jì)算前一峰值與潛在波峰之間的時(shí)間差，判斷是否滿足行人行走一步的時(shí)間閾值范圍，以防止將行人特殊情況下的運(yùn)動(dòng)行為誤判成走步，時(shí)間閾值范圍為(0.2 s，0.8 s)。

4)最后對(duì)檢測(cè)結(jié)果中的波峰數(shù)據(jù)進(jìn)行判斷，利用行步過(guò)程中峰值變換幅度γ=2 g作為標(biāo)準(zhǔn)濾除偽波峰點(diǎn)。

改進(jìn)波峰檢測(cè)法結(jié)合了閾值檢測(cè)與自適應(yīng)檢測(cè)的設(shè)計(jì)思想，其中閾值檢測(cè)法固定了檢測(cè)范圍，缺乏靈活性，在行走速度變化較大或行人處于較為復(fù)雜的狀態(tài)時(shí)，不規(guī)律的運(yùn)動(dòng)狀態(tài)會(huì)使得檢測(cè)結(jié)果的精度下降。自相關(guān)分析法比起常用的波峰檢測(cè)算法可以很大程度上消除行人運(yùn)動(dòng)狀態(tài)以及手機(jī)位置對(duì)手機(jī)計(jì)步器計(jì)步結(jié)果的影響，可以方便快捷地應(yīng)用在室內(nèi)人員定位中，但如上所述，此方法復(fù)雜度較高。因而，改進(jìn)方法僅選取部分計(jì)算過(guò)程進(jìn)行融合，在提高識(shí)別精度的同時(shí)也保證了計(jì)算的實(shí)時(shí)性。

1.3 實(shí)驗(yàn)數(shù)據(jù)獲取

實(shí)驗(yàn)過(guò)程將統(tǒng)一使用RedmiK30U手機(jī)硬件設(shè)備，搭載Android9.0系統(tǒng)，手機(jī)內(nèi)封裝了ICM-40607六軸傳感器和AK09918地磁傳感器。實(shí)驗(yàn)以智能手機(jī)為研究工具，旨在研究快捷簡(jiǎn)單靈敏度高的算法以滿足普適性，對(duì)設(shè)備的要求低并適用于大部分生活場(chǎng)景。

為方便進(jìn)行本節(jié)及之后的試驗(yàn)，作者開(kāi)發(fā)了一個(gè)采集數(shù)據(jù)的Android應(yīng)用程序，用以收集各種情況下行人步進(jìn)狀態(tài)下加速度傳感器、陀螺儀、磁力傳感器以及方向傳感器的數(shù)據(jù)，程序界面如圖2所示，所采集數(shù)據(jù)保存為txt文件。

圖2 數(shù)據(jù)采集程序

Algorithm1:Improved Peak Checking

Input:signal arrayD,the peak time of last timepl,queue sliding windowsw

Output:step numbers

whileDis not nulldo

Fetch an elementαfromD;

ifα>andα<βthen

Put α intosw;

esleContinue;

γ=MAX(sw);

d=γ-pl;

ifγ>γthen

if0.2

s+=1;

esleContinue;

else

if0.3

s+=1;

esleContinue;

pl=γ;

endwhile

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

實(shí)驗(yàn)地點(diǎn)設(shè)置為某大樓走廊。實(shí)驗(yàn)人員以3種不同的設(shè)備攜帶方式進(jìn)行實(shí)驗(yàn)，每次以相同速率平穩(wěn)行走，在傳感器獲得加速度數(shù)據(jù)后使用巴特沃斯濾波法進(jìn)行預(yù)處理[23]，再使用傳統(tǒng)的峰值檢測(cè)法和改進(jìn)計(jì)步算法進(jìn)行步數(shù)統(tǒng)計(jì)和準(zhǔn)確率分析。

由表1可知，本節(jié)改進(jìn)算法在精確度上優(yōu)于傳統(tǒng)峰值檢測(cè)法。實(shí)驗(yàn)發(fā)現(xiàn)，傳統(tǒng)峰值檢測(cè)法測(cè)算步數(shù)總是多于真實(shí)步數(shù)，這是因?yàn)樵谠肼曈绊懴?，閾值處理和潛在波峰識(shí)別方法并不能很好地區(qū)分真實(shí)波峰和偽波峰，導(dǎo)致結(jié)果誤差較大。相較而言，改進(jìn)算法在將設(shè)備拿在手上、置于上衣口袋、置于褲子口袋3種步行模式下都具有較好的準(zhǔn)確率，誤差步數(shù)都在4到6步之間，誤差率也在2%左右。

表1 不同計(jì)步方法的實(shí)驗(yàn)結(jié)果

另外，為驗(yàn)證改進(jìn)計(jì)步算法是否彌補(bǔ)了自相關(guān)分析法實(shí)時(shí)性的不足，在程序中添加了獲取時(shí)間差值的代碼，并記錄每一步檢測(cè)消耗的時(shí)間，最后求平均并進(jìn)行比較，結(jié)果如表2所示，改進(jìn)計(jì)步算法雖然略慢于傳統(tǒng)峰值檢測(cè)法，但在實(shí)時(shí)性上明顯優(yōu)于自相關(guān)分析法。

表2 單步計(jì)算消耗時(shí)間比較

最后，改進(jìn)步數(shù)算法融合了自相關(guān)分析法以提高計(jì)步算法對(duì)復(fù)雜情況的適應(yīng)性。實(shí)驗(yàn)設(shè)計(jì)對(duì)比了峰值檢測(cè)法、小米手機(jī)應(yīng)用商店中的計(jì)步器、自相關(guān)分析法和改進(jìn)算法在不同類型手機(jī)運(yùn)動(dòng)模式下的計(jì)步情況。此處定義正常計(jì)數(shù)為誤差在5%以內(nèi)，大量計(jì)數(shù)為超出實(shí)際步數(shù)5%，少量計(jì)數(shù)為低于實(shí)際步數(shù)5%。試驗(yàn)結(jié)果如表3所示，在任意晃動(dòng)手機(jī)情況下，改進(jìn)計(jì)步算法與自適應(yīng)相關(guān)分析法都只產(chǎn)生了少量計(jì)數(shù)，表現(xiàn)良好。

表3 計(jì)步結(jié)果

2 步長(zhǎng)估計(jì)算法改進(jìn)

根據(jù)人體行步過(guò)程中的影響因素分析現(xiàn)有經(jīng)典步長(zhǎng)模型，通過(guò)對(duì)比實(shí)驗(yàn)選擇表現(xiàn)最優(yōu)的模型，并基于此進(jìn)行模型改進(jìn)設(shè)計(jì)，最后通過(guò)實(shí)驗(yàn)驗(yàn)證改進(jìn)模型的有效性。

2.1 步長(zhǎng)估計(jì)模型選擇

行人航位推算中的步長(zhǎng)不是一成不變的，一般來(lái)說(shuō)，人們正常行走一步的單步步長(zhǎng)為60～80 cm，但是人的身高腿長(zhǎng)，人在行進(jìn)中的速度，步頻等因素都能不同程度地影響到人行進(jìn)過(guò)程中每一步的步長(zhǎng)。

設(shè)計(jì)三組實(shí)驗(yàn)，評(píng)估3種非線性步長(zhǎng)模型的穩(wěn)定程度與準(zhǔn)確程度，從而選擇合適的模型。本節(jié)實(shí)驗(yàn)使用硬件設(shè)備與上一節(jié)保持一致。以正常走、慢走和快走的不同步速直線前進(jìn)，并使用第一節(jié)中的數(shù)據(jù)采集軟件和計(jì)步算法收集行進(jìn)過(guò)程中的加速度數(shù)據(jù)和步長(zhǎng)，在手機(jī)每次檢測(cè)到行人走了一步時(shí)，將這一步時(shí)間段內(nèi)的加速度數(shù)據(jù)寫(xiě)入到txt文件中并標(biāo)注第幾步與不同模型測(cè)算下的該步步長(zhǎng)，再清除程序中存儲(chǔ)加速度信息的數(shù)組以進(jìn)行下一步的信息收集，如此便能收集到行人的步數(shù)以及每一步期間加速度變化的數(shù)據(jù)以及每一步的步長(zhǎng)信息，步長(zhǎng)信息用于本小節(jié)的實(shí)驗(yàn)比較，加速度信息可以用于后文的實(shí)驗(yàn)訓(xùn)練。

除了沿直線行走，還要求測(cè)試人員前進(jìn)過(guò)程中盡量保持勻速，采集數(shù)據(jù)期間測(cè)試人員模擬現(xiàn)實(shí)行人導(dǎo)航尋路的狀態(tài)，手持手機(jī)在胸前，手機(jī)保持水平狀態(tài)。測(cè)試所在宿舍地面磚塊尺寸為0.8 m*0.8 m，利用該地磚控制步長(zhǎng)，共有3種模式，一塊地磚一步走，兩塊地磚三步走，三塊地磚四步走，所得到的單步實(shí)際步長(zhǎng)分別為0.8 m，0.533 m和0.6 m。

在實(shí)驗(yàn)設(shè)計(jì)中Weinberg、Scarlett和Kim模型的取值分別為0.375，1.724和1.983，相較而言Scarlett步長(zhǎng)估計(jì)模型較為穩(wěn)定且與實(shí)際步長(zhǎng)接近。雖然實(shí)驗(yàn)過(guò)程中，實(shí)驗(yàn)人員按格子邁步，盡量保證勻速，但是實(shí)際上人走路難以保持步長(zhǎng)不變，所以我們對(duì)非線性模型步長(zhǎng)估計(jì)實(shí)驗(yàn)結(jié)果進(jìn)行統(tǒng)計(jì)分析，如表4所示。

表4 3種非線性步長(zhǎng)模型比較實(shí)驗(yàn)步態(tài)實(shí)驗(yàn)方法實(shí)際

根據(jù)表4可以看出，Scarlett模型相較 Weinberg、Kim 模型在平均誤差率和方差上都較小，步長(zhǎng)估計(jì)的穩(wěn)定性好、精度高，所以我們選擇Scarlett模型進(jìn)行改進(jìn)。

2.2 步長(zhǎng)模型改進(jìn)

考慮到在行人正常行走情況下，無(wú)論是慢速走、常速走還是快速走，行人的步長(zhǎng)一般不會(huì)突變，前一步步長(zhǎng)與當(dāng)前步步長(zhǎng)應(yīng)具有連續(xù)性，行人第步步長(zhǎng)應(yīng)該與第步具有相關(guān)性。基于這一觀察，在Scarlett模型基礎(chǔ)上融合上一步步長(zhǎng)信息，提出一種新的步長(zhǎng)估計(jì)模型：

(1)

其中:Lk-1為上一步步長(zhǎng)，q為權(quán)重因子，K是一個(gè)常量參數(shù)，amax和amin是第k速度信號(hào)中的最大值和最小值，ai是第i個(gè)加速度信號(hào)值。接下來(lái)，利用收集的加速度信息求取最優(yōu)參數(shù)值，在原先K值基礎(chǔ)上使用最小二乘法求取權(quán)重因子q值，再以此時(shí)的K值與q值作為初始狀態(tài)使用梯度下降法進(jìn)行參數(shù)最優(yōu)估計(jì)。改進(jìn)步長(zhǎng)估計(jì)的計(jì)算步驟如算法2所示。

Algorithm2:Improved Stride Estimation

Input:signal arrayD,the stride length of last timeLk,1

Output:Stride lengthLk

Presetα,β,t;

αmax=MAX(Lk);

αmin=MIN(Lk);

w=(α,β);

whileJ(wt)>cdo

t=t+1;

endwhile

首先將步長(zhǎng)估計(jì)式(1)改寫(xiě)如下：

于是可將步長(zhǎng)估計(jì)問(wèn)題總結(jié)為如下公式，

Lk=wx+b=WTX

其中:w為參數(shù)，x和b是已知信息，代表實(shí)際變量。

當(dāng)X滿秩時(shí)，最小二乘法可以根據(jù)如下公式計(jì)算參數(shù)：

W=(XTX)-1XTLk

試驗(yàn)收集3種不同步行狀態(tài)下各200步共600步的加速度數(shù)據(jù)進(jìn)行計(jì)算，對(duì)比兩種不同的數(shù)據(jù)預(yù)處理方式，第一種是標(biāo)準(zhǔn)化處理，對(duì)相同步行狀態(tài)下的數(shù)據(jù)，數(shù)值減去平均值，除以其標(biāo)準(zhǔn)差，得到均值為0、標(biāo)準(zhǔn)差為1的服從標(biāo)準(zhǔn)正態(tài)分布的數(shù)據(jù)。第二種是歸一化，對(duì)相同狀態(tài)下的數(shù)據(jù)，數(shù)值減去最小值，再除以最大值與最小值的差值，得到新數(shù)據(jù)范圍在[0,1]之間。

評(píng)價(jià)指標(biāo)為均值方差和均方根誤差，權(quán)重因子q取四位小數(shù)，結(jié)果如表5所示。

表5 不同處理方式下最小二乘法結(jié)果

取得較優(yōu)的權(quán)重因子q值為0.276 5。

有了初始值，可將(1)式改寫(xiě)以應(yīng)用梯度下降法，公式如下：

根據(jù)權(quán)重因子q值0.276 5和常量K值1.724可以求得值為0.276 5，值為1.247 314，m取值為訓(xùn)練集合的大小，將這些作為初始值應(yīng)用梯度下降法。我們將收集的3種不同步行狀態(tài)下各200步的數(shù)據(jù)中每組取180步作為訓(xùn)練集，20步作為測(cè)試集，因此整個(gè)訓(xùn)練集大小為540，測(cè)試集為60。

可將問(wèn)題描述為如下公式:

Lk=WTX

定義損失函數(shù)為：

更新參數(shù)表達(dá)式如下：

其中:k為梯度下降的步長(zhǎng)，最終得到較優(yōu)的α與β估計(jì)值，分別為0.273 4和1.245 3，在測(cè)試集上的均值方差和均方根誤差分別為0.001 936和0.044。

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

實(shí)驗(yàn)設(shè)計(jì)與上一節(jié)的計(jì)算步驟一致，收集新的數(shù)據(jù)，以對(duì)比所提出的改進(jìn)步長(zhǎng)估算模型與Scarlett步長(zhǎng)模型，測(cè)算結(jié)果如表6所示?？梢钥闯?，在快速走常速走與慢速走3種步數(shù)下，改進(jìn)步長(zhǎng)估算模型在誤差率和方差表現(xiàn)都略優(yōu)于Scarlett模型，表明改進(jìn)后的模型的準(zhǔn)確度與穩(wěn)定性都有一定提升。

表6 改進(jìn)前后Scarlett模型對(duì)比

上一節(jié)實(shí)驗(yàn)仍有不足之處，一是行人步長(zhǎng)并不完全精確，二是新模型利用3種步長(zhǎng)的數(shù)據(jù)訓(xùn)練，再在相似的場(chǎng)景下測(cè)試說(shuō)服力略顯不足。所以，本節(jié)實(shí)驗(yàn)旨在測(cè)試新模型在行人自由行走狀態(tài)下是否能對(duì)行人行走距離進(jìn)行較為精確的測(cè)算。

在此次實(shí)驗(yàn)中，行人不控制步長(zhǎng)，在宿舍樓道中進(jìn)行100 m的直行，最后統(tǒng)計(jì)步數(shù)步長(zhǎng)信息，測(cè)算不同步長(zhǎng)模型統(tǒng)計(jì)出的行人運(yùn)動(dòng)距離，試驗(yàn)結(jié)果如表7所示。

表7 不同步長(zhǎng)模型實(shí)驗(yàn)對(duì)比

從表中信息可知，在行人自由直線行走情況下，改進(jìn)后的Scarlett步長(zhǎng)模型估計(jì)結(jié)果誤差最小，明顯優(yōu)于其他步長(zhǎng)模型。

3 航向估計(jì)算法改進(jìn)

3.1 航向估計(jì)分析

Android平臺(tái)的方向傳感器在實(shí)際應(yīng)用中反應(yīng)靈敏，精度高，主要缺點(diǎn)在于受磁場(chǎng)影響較大，在室外環(huán)境下磁場(chǎng)影響較小，但是室外使用GPS導(dǎo)航更為方便精確。在室內(nèi)環(huán)境下，因?yàn)楦鞣N不同物體對(duì)磁場(chǎng)的影響，測(cè)得航向角會(huì)出現(xiàn)一定誤差，且在一段時(shí)間行走后，由于航向測(cè)算誤差過(guò)大導(dǎo)致整個(gè)行人航位推算算法處于失靈狀態(tài)[24]；另外，行人手持設(shè)備導(dǎo)航時(shí)，隨著行人身體晃動(dòng)手機(jī)會(huì)出現(xiàn)輕微晃動(dòng)導(dǎo)致測(cè)得航向角與真實(shí)航向角之間出現(xiàn)誤差。這兩種現(xiàn)象都會(huì)在行人沿著某一方向行走時(shí)導(dǎo)致航向誤差較大，室內(nèi)環(huán)境越復(fù)雜，誤差越大。本節(jié)在SHS-PDR(Step and Heading Systems PDR)算法框架[25]基礎(chǔ)上，設(shè)計(jì)了一種新算法，適用于磁場(chǎng)影響較小的室內(nèi)環(huán)境。

3.2 改進(jìn)航向算法

首先引入主導(dǎo)航向的概念，主導(dǎo)航向就是設(shè)備認(rèn)為行人此時(shí)行走的方向，從歷史航向中推測(cè)出來(lái)，當(dāng)行人的航向偏角在合理變化范圍內(nèi)，則認(rèn)為行人沿直線行走，在此基礎(chǔ)上結(jié)合卡爾曼濾波對(duì)航向信息進(jìn)行修正，減少誤差提高定位精度。改進(jìn)航向算法如算法3所示，具體步驟如下。

第一步：設(shè)定主導(dǎo)航向。

主導(dǎo)航向由方向傳感器測(cè)算所得航向序列計(jì)算可得，取一組航向平均值作為主導(dǎo)航向，定義下式為當(dāng)前時(shí)刻的主導(dǎo)航向。

第二步：辨別是否改變主導(dǎo)航向。

D=(|φ(t)-φ(t-1)|+|φ(t)-φ(t-2)|)/2

其中:φ(t)、φ(t-1)和φ(t-2)分別代表當(dāng)前時(shí)刻和前兩個(gè)時(shí)刻測(cè)得的航向，當(dāng)D小于10度時(shí)判定行人沿直線行走，主導(dǎo)航向未變；若大于10度，則判定行人不再沿當(dāng)前直線行走，改變主導(dǎo)航向，此時(shí)清空之前存儲(chǔ)航向值，從當(dāng)前航向值開(kāi)始重新獲取并存儲(chǔ)航向序列直至下次主導(dǎo)航向發(fā)生改變。

Algorithm3:Improved Heading Estimation

Input:direction signal arrayO

Output:orientationdi,1

Initialize array headSquence;

WhileOis not nulldo

Fetch an elementofromO;

Append o to headSquence;

md=(|o-op|+|p-opr|)/2;

ifp>then

di=Kalman-filtering(headSquence);

else

di=md,op=o,opr=op;

Clear headSquence;

endwhile

在未改變主導(dǎo)航向情況下，計(jì)算當(dāng)前航向與主導(dǎo)航向的差值θ(i)：

θ(t)=|φ(t)-φ(t)′|

第三步：卡爾曼濾波處理。

以下介紹采用卡爾曼濾波修正主導(dǎo)航向的過(guò)程。將當(dāng)前航向與主導(dǎo)航向的差值θ(i)作為觀測(cè)量。定義差值誤差為Rt，估計(jì)差值初值為X0|0，估計(jì)差值誤差設(shè)置為Pt|t-1，此時(shí)待調(diào)整的卡爾曼增益可以表示為Pt|t-1/(Rt+Pt|t-1)。初始化X0|0為1.5，差值誤差Pt|t-1為1，觀測(cè)差值誤差Rt取值為1～1.5之間；然后通過(guò)迭代計(jì)算卡爾曼增益直至收斂，從而得到最優(yōu)當(dāng)前航向就是主導(dǎo)航向。

令狀態(tài)轉(zhuǎn)移矩陣為單位矩陣，只考慮噪聲，得到預(yù)測(cè)方程 (2)：

Xt∣t-1=Xk-1∣k-1+ωk

(2)

計(jì)算誤差方差陣 (3)：

Pt∣t-1=Pt-1∣t-1+Qt

(3)

觀測(cè)方程為(4)：

Z(t)=θ(t)+V(t)

(4)

計(jì)算卡爾曼增益，令觀測(cè)矩陣為單位矩陣如下：

Kt=Pt∣t-1(Pt∣t-1+Rt)-1

然后，結(jié)合觀測(cè)值進(jìn)行狀態(tài)估計(jì)值進(jìn)行優(yōu)化，得到狀態(tài)估計(jì)值，即：

Xt∣t=Xt∣t-1+Kt(Zt-Xt∣t-1)

接下來(lái)，更新誤差協(xié)方差矩陣P(t|t)：

Pt∣t=(I-Kt)Pt∣t-1

從而得到最優(yōu)估計(jì)偏差，進(jìn)而計(jì)算當(dāng)前航向角最優(yōu)值并存入下式的航向序列：

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

本節(jié)實(shí)驗(yàn)主要有兩組實(shí)驗(yàn)過(guò)程組成，第一組實(shí)驗(yàn)旨在驗(yàn)證卡爾曼濾波的有效性及算法能否正確識(shí)別轉(zhuǎn)向。行人首先手持手機(jī)在胸前沿90度方向以正常步速行走，對(duì)比濾波前后方向傳感器測(cè)算所得航向角；之后行人改變航向行走，記錄此時(shí)行人航向角估計(jì)值變化，實(shí)驗(yàn)結(jié)果如圖3～5所示。

圖3 濾波前航向角變化

圖4 濾波后航向角變化

圖5 航向角變化

對(duì)比發(fā)現(xiàn)使用卡爾曼濾波修正后航向角變化相對(duì)平穩(wěn)，偏移誤差也由±4度減少為±2度，表明卡爾曼濾波法有效。

行人在轉(zhuǎn)向前后沿直線行走，主導(dǎo)航向變化平穩(wěn)，從圖5看，對(duì)轉(zhuǎn)向反應(yīng)靈敏，表明改進(jìn)航向算法有效。

第二組實(shí)驗(yàn)設(shè)計(jì)主要有兩個(gè)目的：一是對(duì)比改進(jìn)前后的航向估計(jì)算法的有效性；二是將改進(jìn)前后的航向估計(jì)算法與第1節(jié)的改進(jìn)計(jì)步算法和第2節(jié)的改進(jìn)步長(zhǎng)估算模型整合起來(lái)，通過(guò)在每一步檢測(cè)中對(duì)三者計(jì)算結(jié)果的融合推算并可視化展示出行人的實(shí)時(shí)航跡，以驗(yàn)證改進(jìn)的行人航位推算算法在室內(nèi)導(dǎo)航定位上的優(yōu)化效果。

實(shí)驗(yàn)地點(diǎn)選取在某室內(nèi)場(chǎng)所中，房屋長(zhǎng)11.42 m，寬10.9 m，設(shè)計(jì)了由簡(jiǎn)至繁3種路線，每條線路用紅色線條標(biāo)記，其起點(diǎn)是綠點(diǎn)，終點(diǎn)為紅點(diǎn)。第一條路線由前門(mén)門(mén)口出發(fā)，向北步行至客廳中央后轉(zhuǎn)向行走至小屋門(mén)口，進(jìn)入屋內(nèi)后繼續(xù)向北行走直至墻面；第二條路線由后門(mén)出發(fā)，經(jīng)過(guò)4個(gè)直角轉(zhuǎn)彎后行至大屋墻面；第三條路線從前門(mén)門(mén)口出發(fā)，進(jìn)出3個(gè)房屋后回到起始原點(diǎn)。將兩種算法行人步行軌跡繪制在家中平面圖上以便比較，實(shí)驗(yàn)結(jié)果如圖6所示。

圖6 改進(jìn)算法效果對(duì)比

圖6(a)為方向傳感器算法結(jié)合前文改進(jìn)步長(zhǎng)估算模型和改進(jìn)步數(shù)算法計(jì)算出的行人位移軌跡，子圖6(b)為改進(jìn)航向估計(jì)算法結(jié)合兩種算法計(jì)算出的位移軌跡?？梢钥吹皆诘谝弧⒍蓷l路線中，改進(jìn)前算法在幾次航向改變后就不再準(zhǔn)確，而改進(jìn)后算法表現(xiàn)良好；在第三條路線中，因?yàn)樾凶哕壽E復(fù)雜，改進(jìn)前算法已經(jīng)完全失靈，而改進(jìn)后算法的推算軌跡基本正確，且終點(diǎn)也幾乎落在起點(diǎn)旁，誤差在1.2米左右。因此，改進(jìn)后航向估計(jì)算法明顯優(yōu)于改進(jìn)前。

此外，從圖6(b)中還可以看出，融合計(jì)步、步長(zhǎng)估計(jì)、航向估計(jì)3種改進(jìn)方法的綜合行人航位推算算法可以實(shí)現(xiàn)較為準(zhǔn)確的綜合定位效果，還不會(huì)隨著實(shí)驗(yàn)路線變得繁雜而導(dǎo)致定位精度降低，這表明改進(jìn)的行人航位推算算法不僅提高了室內(nèi)定位的精度，而且具有較好的魯棒性。

4 結(jié)束語(yǔ)

本文在智能手機(jī)設(shè)備上研究改進(jìn)室內(nèi)行人航位推算算法，對(duì)行人航位推算的計(jì)步、步長(zhǎng)估算以及航向估計(jì)方法進(jìn)行改進(jìn)。本文提出了一種結(jié)合自相關(guān)分析法和峰值檢測(cè)法的計(jì)步算法，增強(qiáng)了抗干擾能力；提出一種基于Scarlett模型融合前一步步長(zhǎng)的改進(jìn)步長(zhǎng)估算模型，相較于Scarlett基本模型具有更高的檢測(cè)精度；通過(guò)設(shè)定主導(dǎo)航向并對(duì)采樣結(jié)果進(jìn)行卡爾曼濾波，減少了航向估計(jì)誤差。最后，將3種改進(jìn)方法相結(jié)合實(shí)現(xiàn)了室內(nèi)定位系統(tǒng)。實(shí)驗(yàn)表明，改進(jìn)方法取得了較好的室內(nèi)定位效果。本文提出的算法已應(yīng)用于某抽蓄電站廠房運(yùn)維人員定位，因其易于部署、成本低與良好的人員及環(huán)境適應(yīng)性，能較好地滿足這一場(chǎng)景下的的定位需求。

本文方法仍有改進(jìn)空間，這將是下一步的研究工作。對(duì)于復(fù)雜環(huán)境下的行人計(jì)步，一是引入氣壓傳感器測(cè)算高度信息，將行人的軌跡拓展到三維層面；二是考慮對(duì)不同情況下行人加速度數(shù)據(jù)的特征進(jìn)行收集，結(jié)合機(jī)器學(xué)習(xí)等方法進(jìn)行識(shí)別。航向估計(jì)可以結(jié)合陀螺儀積分方法進(jìn)行研究，陀螺儀不受磁場(chǎng)影響，但積分累計(jì)誤差加上本身漂移誤差嚴(yán)重，可以嘗試將兩種方法結(jié)合。對(duì)于行人初始位置的確定，可考慮在更精密的設(shè)備中將行人在接收不到GPS信號(hào)前的最后位置作為初始位置，在行人航位推算算法由于長(zhǎng)時(shí)間誤差累積而精度減弱后，還可結(jié)合外源信號(hào)定位技術(shù)如藍(lán)牙和UWB等輔助獲取行人初始位置。