黃欣， 熊智， 許建新， 徐麗敏

(南京航空航天大學 自動化學院， 江蘇 南京 210016)

基于零速/航向自觀測/地磁匹配的行人導航算法研究

黃欣， 熊智， 許建新， 徐麗敏

(南京航空航天大學 自動化學院， 江蘇 南京 210016)

目前行人導航技術(shù)正發(fā)揮著越來越重要的作用，而無全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)環(huán)境下的行人導航定位成為其不可或缺的環(huán)節(jié)。以自包含傳感器為硬件平臺，針對無GNSS環(huán)境下的行人自主導航定位展開研究，提出一種基于“2+2”分級模式的零速判別方法，并設(shè)計一種慣性導航系統(tǒng)的零速修正卡爾曼濾波算法，有效提高同一參數(shù)閾值下零速判別的準確性與可靠性、抑制傳感器誤差發(fā)散；研究行人初始靜態(tài)下磁航向誤差觀測算法及行人運動狀態(tài)下的零速航向誤差自觀測算法，解決了行人長時間行走航向發(fā)散問題；提出基于多層約束和K近鄰算法的地磁匹配算法，并實現(xiàn)基于零速修正/航向誤差自觀測的行人導航算法與地磁匹配算法的融合，提高了行人導航定位精度與可靠性。實際數(shù)據(jù)測試驗證，所提基于零速修正/航向誤差自觀測/地磁匹配的行人導航算法可有效提高定位精度79%以上。

控制科學與技術(shù)； 組合導航； 零速分級判別； 卡爾曼濾波； 航向誤差自觀測； 地磁匹配； 多層約束

0 引言

目前，國內(nèi)外已經(jīng)掀起研究行人導航技術(shù)的熱潮。無論商場購物、生活健身、野外勘探還是火災救援，行人導航定位正發(fā)揮著越來越重要的作用。室外環(huán)境下，行人可以借助全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)實現(xiàn)定位，但是在室內(nèi)環(huán)境下，衛(wèi)星信號微弱甚至缺失，因此有必要針對無GNSS環(huán)境下的行人導航定位展開更深入的研究。

目前，許多學者專家均已針對無GNSS環(huán)境下的行人導航定位開展了大量的研究工作，研究內(nèi)容主要包括以下兩方面：1)借助無線網(wǎng)絡(luò)輔助行人定位，如藍牙、無線保真(WIFI)、超寬帶(UWB)等其他射頻輔助信息；2)基于固定于行人頭部、肩部、腰部和腳部的慣性傳感器，通過觀測慣性傳感器的運動特性，借助零速修正技術(shù)(ZUPT)和零角速修正(ZARU)技術(shù)，輔助慣性導航系統(tǒng)實現(xiàn)高精度行人定位。其中，根據(jù)腰部和脛骨的傳感器特性，文獻[1]使用支持向量機(SVM)分析行人步態(tài)并借助ZUPT實現(xiàn)了行人定位，但未針對不同行人/不同步速下ZUPT的可靠性開展研究，同時該方法的航向可觀測性較差、航向誤差難以消除；文獻[2]提出了一種利用雙慣性測量單元(IMU)架構(gòu)實現(xiàn)室內(nèi)行人定位的方法，但該方法會增加額外成本，不適合廣泛推廣；文獻[3]采用額外射頻信息來輔助修正慣性導航(簡稱慣導)位置誤差，但該方法的應用場景較狹窄、可移植性較差；文獻[4]融合建筑物樓向信息來提高行人導航航向的精度，但該方法的適用范圍受到限制、靈活性較低。

地磁場作為地球的固有屬性，已廣泛用于航向計算。限于室內(nèi)金屬會產(chǎn)生硬磁和軟磁干擾，即磁異常現(xiàn)象，室內(nèi)磁航向的精度難以保證。雖然許多學者已經(jīng)提出了地磁標定方法，包括二維橢圓標定和三維橢球標定技術(shù)等，但仍然難以消除室內(nèi)全程磁誤差。部分學者研究利用磁異常來構(gòu)建地磁指紋庫，利用地磁匹配(GM)技術(shù)實現(xiàn)行人定位，但是提出的部分方案消耗內(nèi)存較大且實時性較差，不適合工程實際應用，如動態(tài)時間規(guī)整(DTW)算法、Hausdorff距離算法等[5-8]。

本文針對上述問題，構(gòu)建了基于捷聯(lián)慣性導航技術(shù)與GM技術(shù)的行人組合導航算法模型，主要解決無GNSS環(huán)境下的高精度、高可靠性行人導航定位問題。該方法研究了基于“2+2”分級模式的零速綜合判別方法，并設(shè)計了一種基于慣導系統(tǒng)的零速修正卡爾曼濾波算法，以有效提高同一閾值下零速判別的準確性與可靠性，解決不同行人、不同步速的適應性問題，抑制傳感器誤差發(fā)散；研究了行人初始靜態(tài)下基于磁航向穩(wěn)定性的航向誤差觀測算法(MHESO)及行人運動狀態(tài)下的零速航向誤差自觀測算法(ZHESO)，增強了行人航向可觀測性，提高了航向精度；研究了基于多層約束和K近鄰(KNN)算法的GM模型，并實現(xiàn)了基于零速修正/航向誤差自觀測(ZUPT_HESO)的捷聯(lián)慣性導航算法與GM的融合，進一步提高了行人導航定位精度。本文所提出的基于零速修正/航向誤差自觀測/地磁匹配(ZUPT_HESO_GM)的行人導航算法在保證高精度的基礎(chǔ)上，大大保證了無GNSS環(huán)境下行人導航定位的可靠性。

1 行人運動特性和地磁特性分析

1.1 行人運動特性

正常步態(tài)周期下，行人腳后跟首先著地，表現(xiàn)為加速度計產(chǎn)生一個很大的峰值。伴隨著行人腳部的阻尼振動，陀螺儀反映出連續(xù)的峰值輸出。當行人單腳著地時，加速度計與陀螺儀進入穩(wěn)態(tài)，即零速狀態(tài)，此時可以借助ZUPT或ZARU抑制誤差發(fā)散。如圖1所示(為與后續(xù)零速檢測算法相對應，均取3軸幅值的平方)，行人正常步態(tài)下，零速狀態(tài)與擺動狀態(tài)交替出現(xiàn)。穩(wěn)態(tài)下，行人速度的理論值為0，位置基本保持不變。

圖1 加速度計與陀螺儀零速/擺動交替示意圖Fig.1 ZUPT/swing alternation of accelerometer and gyroscope

1.2 地磁特性

圖2 地磁組成Fig.2 Components of geomagnetism

地球磁場具有7大屬性，如圖2所示，包括水平磁場強度H、垂直磁場強度Z、總磁場強度F、北向磁場強度X、東向磁場強度Y、傾角I和磁航向角D[9]. 憑借這7大屬性，可以構(gòu)建指紋庫、實現(xiàn)GM. 構(gòu)建指紋庫之前需滿足先決條件，即不同位置之間的磁場強度需有明顯的變化，變化越大，匹配誤差越??；相同位置的磁指紋需保持長期不變[10]。

圖3 小范圍總磁場強度變化示意Fig.3 Change of total magnetic field strength in a small range

圖4 不同時間段總磁場強度對比Fig.4 Comparison of total magnetic field strengths in different time periods

2 行人導航算法構(gòu)建與融合

2.1 基于ZUPT_HESO的行人導航算法

行人運動特性和地磁特性是構(gòu)建行人導航定位誤差修正模型的基礎(chǔ)，而行人導航解算算法是實現(xiàn)行人導航定位的前提。本文研究了基于足部的捷聯(lián)慣性導航解算算法，包括姿態(tài)解算、速度解算和位置解算，如(1)式[11]所示：

(1)

(2)

式中：ab和ωb分別為3軸加速度計零偏和3軸陀螺儀零偏；N為用于求取零偏的個數(shù)；ai和ωi分別為第i個3軸加速度與3軸角速度數(shù)據(jù)信息。

由于IMU零位偏差會隨時間漂移，在缺乏其他系統(tǒng)輔助的情況下，純捷聯(lián)解算精度無法滿足行人的日常需求。因此，本文研究利用行人運動學特性來抑制位置、速度誤差的漂移。

在行人的行走過程中，加速度計與陀螺儀周期性變化?？紤]到傳感器單軸誤差及3軸間的耦合影響，構(gòu)建如下4種子零速判別方式：

(3)

(4)

(5)

(6)

式中：akx、aky、akz代表第k時刻加速度計采集得到的3軸加速度信息；ωkx、ωky、ωkz代表第k時刻陀螺儀采集得到的3軸角速度信息；n為方差計算區(qū)間大??；εa、εω、εa2和εω2分別為人為設(shè)定的閾值。

考慮到不同行人、不同步速及固定參數(shù)閾值下，僅用λ1、λ2、λ3、λ4其中之一進行零速判別穩(wěn)定性較差，容易發(fā)生誤判或者漏判，同時鑒于不同行人、不同步速下3軸加速度計模值方差與3軸陀螺儀模值方差的穩(wěn)定性較強(即模值方差的大小基本不變)，針對4種子零速研究采用“2+2”分級模式判別總零速的起始時刻和終結(jié)時刻。即用加速度計與陀螺儀的方差子零速(λ1和λ2)判別出總零速的起始時刻；以加速度計與陀螺儀的幅值子零速(λ3和λ4)判別出總零速的終結(jié)時刻，以此提高不同行人、不同步速下，同一參數(shù)閾值的可靠性。圖5所示為一步零速區(qū)間判別示意圖。由圖5可見，第1級判別(藍色)為方差子零速判別曲線，第2級判別(棕色)為幅值子零速判別曲線，紅色為綜合零速判別曲線。由于方差子零速計算區(qū)間為20，第20個時刻與前19個時刻的耦合性較強，當?shù)?0個時刻為行人運動態(tài)時，前19個時刻亦判為非零速態(tài)，將出現(xiàn)漏判現(xiàn)象，此時需用幅值子零速進行進一步判別。同時，每一級利用加速度計與陀螺儀兩者共同作用，可以抑制零速誤判現(xiàn)象。

圖5 “2+2”零速分級判別示意圖Fig.5 Schematic diagram of ZUPT detection based on “2+2”hierarchical model

對于第2級模式，λ3和λ4作適當調(diào)整，形如λ5和λ6，如(7)式所示。其中：當k-1時刻λ1(k-1)=1且k時刻λ1(k)=0時a_number=1，當λ5(k)=0時a_number=0，當a_number>0且λ5(k)=1時a_number=a_number+1；同理，當k-1時刻λ2(k-1)=1且k時刻λ2(k)=0時ω_number=1，當λ6(k)=0時ω_number=0，當ω_number>0且λ6(k)=1時ω_number=ω_number+1. 其中，a_number和ω_number分別為加速度計與陀螺儀幅值的子零速個數(shù)。

(7)

子零速綜合判別模型如(8)式所示：

(8)

式中：Z(k)表示k時刻的綜合零速判別結(jié)果，若為1則表示當前時刻行人為理論上的零速時刻。流程示意如圖6所示。本文針對λ1(k)、λ2(k)、λ3(k)、λ4(k)判別方式和本文提出的Z(k)判別算法進行了不同步速、不同行人穿戴下的大量測試，其中一組零速判別效果如圖7所示，可以發(fā)現(xiàn)λ1(k)、λ2(k)、λ3(k)、λ4(k)判別方式存在誤判現(xiàn)象。由此可見本文方法能有效保證在同一閾值下不同行人、不同步速下零速判別的準確性和可靠性，提高了參數(shù)的可重載能力。

圖7(a)和圖7(b)中綠色曲線為綜合零速檢測曲線(高電平為檢測到零速，幅值僅為顯示用)，紅色曲線分別為單加速度計、單陀螺儀零速檢測曲線。

部分學者在零速狀態(tài)下將速度置0，無法有效估計速度、位置、姿態(tài)及傳感器誤差。本文提出利用卡爾曼濾波器、基于人體運動學設(shè)計來構(gòu)建綜合零速觀測模型，在檢測到零速時，利用卡爾曼濾波器估計速度、位置、姿態(tài)及傳感器誤差，抑制誤差發(fā)散。

圖6 零速綜合判別示意圖Fig.6 Flow chart of ZUPT comprehensive judgment

圖7 零速檢測對比曲線Fig.7 Comparison of ZUPT detection

(9)

式中：A為系統(tǒng)矩陣；G為系統(tǒng)噪聲矩陣；W為系統(tǒng)噪聲。

利用偽量測信息構(gòu)建零速狀態(tài)下的六維量測模型。令veloPk=[0;0;0]，posiPk=posiNk-1(k-1表示上一時刻)，則零速狀態(tài)下的量測方程如(10)式所示(增加位置量測量相比于僅有速度量測可以提升7%的定位精度)：

(10)

式中：Zvp(t)為觀測量；veloN和posiN為慣導實時解算的速度、位置；Mv和Mp為行人零速狀態(tài)下速度誤差與位置誤差；Hvp為量測矩陣，如(11)式和(12)式所示；Vvp為量測噪聲。

(11)

(12)

式中：L為緯度；Rm和Rn分別為子午圈和卯酉圈半徑。

同時，限于零速周期非一致性，本文對比了固定離散周期和時變離散周期的差異，并最終選擇(13)式所示的方案，即時變離散周期。

(13)

式中：old_t為上一個零速時刻；t為當前零速時刻。

因此得到最基本的基于ZUPT的行人導航算法如圖8所示，包括慣性測量單元、零速檢測單元、卡爾曼濾波單元及導航計算單元。

圖8 基于ZUPT的行人導航算法框架Fig.8 Framework of ZUPT-based pedestrian navigation algorithm

在上述基礎(chǔ)上研究了行人初始靜止狀態(tài)下MHESO模型及行人運動狀態(tài)下ZHESO模型，以增強航向可觀測性。

行人初始靜止狀態(tài)下，磁航向角(利用二維橢圓標定)具有較強的穩(wěn)定性，因此構(gòu)建一維量測方程，如(14)式所示：

Zmφ(t)=[ψINS-ψmg]=[δφ+Mmφ]=
Hmφ(t)X(t)+Vmφ(t)，

(14)

式中：ψINS為捷聯(lián)解算的航向角；ψmg為磁航向角；Mmφ為磁航向角解算誤差；Hmφ為量測矩陣；Vmφ為量測噪聲。

行人初始靜止狀態(tài)下，利用(15)式實現(xiàn)對傳感器誤差進一步估計：

(15)

行人運動狀態(tài)下，每一步經(jīng)歷一段零速時刻，記該步第1零速時刻點的航向角為ψk-i，如圖9所示，其中紅點表示每一步的第1零速時刻點。

圖9 零速航向自觀測示意Fig.9 Indication of heading error self-observation

記該步其余零速時刻點的航向角為ψk-o，理論上零速段的計算公式為

Δψ=ψk-i-ψk-o=0.

(16)

構(gòu)建零速航向自觀測量測方程為

Zzφ(t)=[ψk-i-ψk-o]=[δφ+Mzφ]=
Hzφ(t)X(t)+Vzφ(t).

(17)

由于姿態(tài)誤差角與平臺誤差角存在如下關(guān)系：

(18)

式中：θ為俯仰角；δφx、δφy和δφy為3個平臺誤差角。因此：

(19)

于是可得運動狀態(tài)下ZUPT_HESO算法的7維量測模型為

(20)

即在每一步中，在零速階段利用零速航向誤差自觀測算法估計航向誤差與陀螺誤差。

2.2GM算法

本文將GM分為兩種場景：1)人- 人跟蹤模式；2)非跟蹤模式。部分專家學者已經(jīng)針對室內(nèi)地磁異常特性提出了相應的匹配算法[13-15]，包括動態(tài)時間規(guī)整算法、Hausdorff距離算法與最小距離算法。各種算法的對比如表1所示。

表1 GM算法對比

表1中：H為總距離之和；d為兩點間距離；A和B分別為磁指紋庫及欲匹配磁數(shù)據(jù)；i和j分別為A和B的當前時刻；r=1，2，分別代表曼哈頓距離和歐氏距離。由表1可見，同一路徑下，行人的行走速度不同將導致磁數(shù)據(jù)間隔的收縮或放大，如圖10所示(黑色代表兩磁場數(shù)據(jù)最為接近)，此時相關(guān)度匹配誤差較大。動態(tài)時間規(guī)整算法雖然可以避免同一路徑不同行走速度下的動態(tài)影響，但是內(nèi)存消耗較大且無法實現(xiàn)實時；Hausdorff距離算法需要事先獲取所有欲匹配磁信息，不適合工程實際應用；第3種算法需要設(shè)置約束條件以避免誤匹配[13]。

圖10 同一路徑不同速度下的磁對應示意圖Fig.10 Magnetic map of the same path at different speeds

本文在上述算法的基礎(chǔ)上，研究利用KNN算法和多層約束實現(xiàn)GM. KNN算法屬于機器學習方法，依賴于極限理論，但只有少量的相鄰樣本與決策類型相關(guān)。本文實際運用中利用了準時制(JIT)理論來加速循環(huán)。為更加精準地實現(xiàn)GM，本文設(shè)置了多層約束，并基于兩種場景構(gòu)建兩種不同的多層約束模型，其中人- 人跟蹤場景下的多層約束模型如(21)式所示：

(21)

式中：i和j分別表示實時采集的磁與指紋庫序列號；ε1表示前后間隔；ε2為GM誤差；ti和tj分別為指紋庫時間點和實時采集地磁時間點。多層約束限制了其分類范圍并有助于選擇匹配點。每當GM成功，更新last_MM=tj.

另一場景為非跟蹤狀態(tài)，此時需適當修改多層約束，如(22)式所示：

(22)

式中：posiMj為指紋庫的位置；posiNi為當前點的位置，由基于ZUPT_HESO的行人導航算法得到。每當GM成功，更新last_MM=tj. 利用實時采集的磁信息，借助KNN算法和多層約束進行磁匹配，若滿足多層約束則磁匹配成功。

2.3ZUPT_HESO_GM算法

事實上，雖然借助ZUPT、卡爾曼濾波估計技術(shù)及ZHESO算法能有效抑制慣導誤差發(fā)散，但是無法保證百分之百的成功率；GM受限于室內(nèi)環(huán)境，且不能提供連續(xù)的位置信息。因此，本文有效組合兩種算法，在保證行人導航定位精度的前提下，提高其可靠性。

在正常環(huán)境下，由基于ZUPT_HESO的行人導航算法提供位置信息。當僅有GM成功時，采用(23)式修正慣導誤差；當同時檢測到零速及GM成功時，使用集中濾波器構(gòu)建8維量測，輔助修正位置誤差、速度誤差及傳感器誤差，如(24)式所示。整個組合導航系統(tǒng)框架如圖11所示。

Z6(t)=[posiN-posiM]=[δp+M6]=

(23)

(24)

圖11 組合導航系統(tǒng)框架Fig.11 Framework of integrated navigation system

3 行人導航實驗

3.1 測試路徑構(gòu)建及地磁指紋庫的構(gòu)建

為了測試算法的可行性，分別于室外和室內(nèi)選擇測試路徑，其中兩條路徑如圖12和圖13所示。圖12為室外環(huán)繞足球場輪廓，耗時約10 min；圖13為室內(nèi)方形多圈行走，耗時約5 min. 測試前利用高精度激光測距儀測量得到相對距離。本文實驗采用荷蘭Xsens公司生產(chǎn)的MTi-30器件(見圖14)采集室內(nèi)磁信息。磁采集頻率為100 Hz，加速度計與陀螺儀采集頻率為200 Hz.

圖12 室外路徑Fig.12 Outdoor path

圖13 室內(nèi)路徑Fig.13 Indoor path

圖14 MTi-30外觀圖Fig.14 MTi-30

3.2 算法實驗驗證

下面驗證上述算法的可行性，主要包括以下3部分：

1) ZUPT算法?；诘统杀镜募兘萋?lián)慣性導航解算算法誤差發(fā)散較快，兩種場景下誤差可達千米級，導航曲線與真實路徑完全不相匹配；在捷聯(lián)慣性導航的基礎(chǔ)上，增加了零速修正算法。如圖15(a)與圖15(b)中藍色曲線為加入零速修正的導航結(jié)果，導航曲線與真實路徑已基本相同，但長時間導航狀態(tài)下的最終誤差稍大。

2) ZUPT-HESO算法。觀測圖15(a)與圖15(b)中藍色曲線，盡管有零速修正算法估計速度誤差、位置誤差和傳感器誤差，但由于航向可觀測性較差，長時間導航時航向亦會發(fā)散。如表2所示，通過增加航向自觀測算法有效提高了航向精度，降低了整體定位誤差，室內(nèi)和室外導航定位精度分別提高98.6%和79.3%. 如圖15(a)與圖15(b)中紅色曲線所示，航向精度的提升反映在最終位置的誤差較小。

3) ZUPT_HESO_GM算法。室內(nèi)環(huán)境中的磁異常環(huán)境較明顯，室內(nèi)測試路徑2中構(gòu)建了地磁指紋庫，其中地磁指紋間隔為10 cm，組合導航結(jié)果如圖15(b)中的綠色曲線所示。由表2可見，室內(nèi)定位精度在ZUPT_HESO算法的基礎(chǔ)上提高了導航定位精度98.3%，進一步增加了無GNSS環(huán)境下行人導航定位的可靠性 。

圖15 室內(nèi)外路徑不同算法效果對比Fig.15 Comparison of navigation effects in indoor and outdoor paths

表2 不同算法誤差對比

綜上，基于ZUPT_HESO_GM算法輔助的行人導航定位算法精度較好、可靠性較高。

4 結(jié)論

本文以自包含傳感器為硬件基礎(chǔ)，提出了一種基于ZUPT_HESO的捷聯(lián)慣性導航解算算法與GM算法融合的組合導航算法。該方法研究了基于“2+2”分級零速判別模型下的ZUPT模型，基于卡爾曼濾波算法有效修正行人長時定位下的傳感器誤差與導航結(jié)果誤差；設(shè)計了MHESO模型與ZHESO模型解決了行人航向發(fā)散問題；研究了基于多層約束和KNN算法的GM算法，并實現(xiàn)了基于ZUPT_HESO輔助的捷聯(lián)慣性導航解算算法與GM算法融合，進一步提高了GNSS失效環(huán)境下的行人導航定位精度與可靠性。

References)

[1] 汪少初. 基于PDR的定位與跟蹤技術(shù)研究及系統(tǒng)設(shè)計[D].天津：天津大學, 2014.

WANG Shao-chu. Research on the technologies of personnel positioning and tracking and system design based on PDR [D]. Tianjing: Tianjin University, 2014. (in Chinese)

[2] 徐元, 陳熙源, 李慶華,等. 一種基于雙IMU框架的室內(nèi)個人導航方法(英文)[J]. 中國慣性技術(shù)學報, 2015, 23(6):714-717.

XU Yuan, CHEN Xi-yuan, LI Qing-hua, et al. Indoor pedestrian navigation based on double-IMU framework [J]. Journal of Chinese Inertial Technology, 2015, 23(6):714-717. (in Chinese)

[3] Ruiz A R J, Granja F S, Honorato J C P, et al. Pedestrian indoor navigation by aiding a foot-mounted IMU with RFID signal strength measurements[C]∥International Conference on Indoor Positioning and Indoor Navigation. Zurich, Switzerland: IEEE, 2010.

[4] Abdulrahim K, Hide C, Hill T M C. Aiding low cost inertial navigation with building heading for pedestrian navigation [J]. Journal of Navigation, 2011, 64(2):219-233.

[5] 熊明亮, 刁夢雯, 趙國梁. 基于地球磁場的室內(nèi)定位系統(tǒng)的研究[J]. 無線互聯(lián)科技, 2015(18):14-15.

XIONG Ming-liang, DIAO Meng-wen, ZHAO Guo-liang. Research on indoor positioning system based on the magnetic field of the earth [J]. Wireless Internet Technology, 2015(18):14-15. (in Chinese)

[6] Robertson P, Frassl M, Angermann M, et al. Simultaneous localization and mapping for pedestrians using distortions of the local magnetic field intensity in large indoor environments[C]∥International Conference on Indoor Positioning and Indoor Navigation. Montbeliard-Belfort, France: IEEE, 2013.

[7] Liu K, Motta G, Ma T, et al. Multi-floor indoor navigation with geomagnetic field positioning and ant colony optimization algorithm[C]∥Proceedings of Symposium on Service-Oriented System Engineering. Oxford, UK: IEEE, 2016:314-323.

[8] 宋鏢, 程磊, 周明達,等. 基于慣導輔助地磁的手機室內(nèi)定位系統(tǒng)設(shè)計[J]. 傳感技術(shù)學報, 2015, 28(8):1249-1254.

SONG Biao, CHENG Lei, ZHOU Ming-da, et al. The design of cellphone indoor positioning system based magnetic assisted inertial navigation technology [J]. Chinese Journal of Sensors and Actuators, 2015, 28(8):1249-1254. (in Chinese)

[9] Li Y, Zhuang Y, Zhang P, et al. An improved inertial/WiFi/magnetic fusion structure for indoor navigation [J]. Information Fusion, 2016, 34:101-119.

[10] Frassl M, Angermann M, Lichtenstern M, et al. Magnetic maps of indoor environments for precise localization of legged and non-legged locomotion[C]∥Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo, Japan: IEEE, 2013:913-920.

[11] Li Y, Zhuang Y, Lan H, et al. Self-contained indoor pedestrian navigation using smartphone sensors and magnetic features[J]. IEEE Sensors Journal, 2016, 16(19):1-1.

[12] Li Y, Georgy J, Niu X, et al. An automatic multi-level gyro calibration architecture for consumer portable devices[C]∥Proceedings of International Conference on Indoor Positioning and Indoor Navigation. Busan, Korea: IEEE, 2014:656-660.

[13] Ghose A, Pal A, Dutta Choudhury A, et al. Indoor positioning system: US, 9557178[P]. 2017-01-31.

[14] 孔亞男, 魯浩, 徐劍蕓. 基于Hausdorff距離的地磁匹配導航算法[J]. 航空兵器, 2011(4):26-29.

KONG Ya-nan, LU Hao, XU Jian-yun. Study on geomagnetic matching algorithm based on Hausdorff distance [J]. Aeroweaponry, 2011(4):26-29. (in Chinese)

[15] 劉玉霞，周軍，葛致磊. 基于隱馬爾可夫模型的地磁匹配算法[J].中國慣性技術(shù)學報， 2011, 19(2):224-228.

LIU Yu-xia, ZHOU Jun, GE Zhi-lei. Geomagnetic matching algorithm based on hidden Markov model[J]. Journal of Chinese Inertial Technology, 2011, 19(2):224-228.(in Chinese)

ResearchonPedestrianNavigationAlgorithmBasedonZeroVelocityUpdate/HeadingErrorSelf-observation/GeomagneticMatching

HUANG Xin, XIONG Zhi, XU Jian-xin, XU Li-min
(College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, Jiangsu, China)

Nowadays, pedestrian navigation technology is playing an increasingly important role in supermarket shopping, fire rescue and field exploration, and the pedestrian navigation and positioning without global navigation satellite system (GNSS) has become an indispensable link. The self-contained sensors are used as a hardware platform for research on pedestrian autonomous navigation in non-GNSS environment. A zero-speed comprehensive discriminant algorithm based on the “2+2” hierarchical model is studied to improve the accuracy and reliability of zero velocity update (ZUPT). Kalman filter algorithm based on ZUPT designed for inertial navigation system is used to effectively suppress the sensor error divergence. To solve the problem of pedestrian long-term heading divergence, the magnetic heading error self-observation algorithm (MHESO) for pedestrian initial static state and the ZUPT heading error self-observation algorithm (ZHESO) for pedestrian movement are studied. In addition, a geomagnetic matching (GM) algorithm based on multi-layer constraint and K-nearest neighbor algorithm is proposed, and the fusion of ZUPT_HESO-pedestrian navigation algorithm and geomagnetic matching algorithm is realized, which improves the accuracy and reliability of pedestrian navigation. The actual data test proves that the proposed pedestrian navigation algorithm based on ZUPT_HESO_GM effectively improves the positioning accuracy by more than 79%.

control science and technology； integrated navigation； zero velocity update grading discrimination； Kalman filter； heading error self-observation； geomagnetic matching； multilayer constraint

2017-03-01

國家自然科學基金項目(61673208、61533008、61533009、61374115、61304227)；江蘇省“333工程”科研立項項目(BRA2016405)；人力資源和社會保障部留學人員擇優(yōu)資助項目(2016年)；江蘇省六大人才高峰項目(2013-JY-013)；江蘇高校優(yōu)勢學科建設(shè)工程項目(2014年)；中央高?；究蒲袠I(yè)務費專項項目(NZ2016104、NS2017016)；江蘇省普通高校研究生科研創(chuàng)新計劃項目(KYLX15_0264)；江蘇省自然科學基金項目(BK20141453)；航空科學基金項目(20165552043)

黃欣(1993—)，男，碩士研究生。E-mail：huangxin@nuaa.edu.cn

熊智(1976—)，男，研究員，博士生導師。E-mail：xiongzhi@nuaa.edu.cn

V249.32+8

A

1000-1093(2017)10-2031-10

10.3969/j.issn.1000-1093.2017.10.020