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

(南京航空航天大學(xué) 自動(dòng)化學(xué)院， 江蘇 南京 210016)

基于零速/航向自觀測(cè)/地磁匹配的行人導(dǎo)航算法研究

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

(南京航空航天大學(xué) 自動(dòng)化學(xué)院， 江蘇 南京 210016)

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

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

0 引言

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

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

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

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

1 行人運(yùn)動(dòng)特性和地磁特性分析

1.1 行人運(yùn)動(dòng)特性

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

圖1 加速度計(jì)與陀螺儀零速/擺動(dòng)交替示意圖Fig.1 ZUPT/swing alternation of accelerometer and gyroscope

1.2 地磁特性

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

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

圖3 小范圍總磁場(chǎng)強(qiáng)度變化示意Fig.3 Change of total magnetic field strength in a small range

圖4 不同時(shí)間段總磁場(chǎng)強(qiáng)度對(duì)比Fig.4 Comparison of total magnetic field strengths in different time periods

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

2.1 基于ZUPT_HESO的行人導(dǎo)航算法

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

(1)

(2)

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

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

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

(3)

(4)

(5)

(6)

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

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

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

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

(7)

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

(8)

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

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

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

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

圖7 零速檢測(cè)對(duì)比曲線Fig.7 Comparison of ZUPT detection

(9)

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

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

(10)

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

(11)

(12)

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

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

(13)

式中：old_t為上一個(gè)零速時(shí)刻；t為當(dāng)前零速時(shí)刻。

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

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

在上述基礎(chǔ)上研究了行人初始靜止?fàn)顟B(tài)下MHESO模型及行人運(yùn)動(dòng)狀態(tài)下ZHESO模型，以增強(qiáng)航向可觀測(cè)性。

行人初始靜止?fàn)顟B(tài)下，磁航向角(利用二維橢圓標(biāo)定)具有較強(qiáng)的穩(wěn)定性，因此構(gòu)建一維量測(cè)方程，如(14)式所示：

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

(14)

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

行人初始靜止?fàn)顟B(tài)下，利用(15)式實(shí)現(xiàn)對(duì)傳感器誤差進(jìn)一步估計(jì)：

(15)

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

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

記該步其余零速時(shí)刻點(diǎn)的航向角為ψk-o，理論上零速段的計(jì)算公式為

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

(16)

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

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

(17)

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

(18)

式中：θ為俯仰角；δφx、δφy和δφy為3個(gè)平臺(tái)誤差角。因此：

(19)

于是可得運(yùn)動(dòng)狀態(tài)下ZUPT_HESO算法的7維量測(cè)模型為

(20)

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

2.2GM算法

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

表1 GM算法對(duì)比

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

圖10 同一路徑不同速度下的磁對(duì)應(yīng)示意圖Fig.10 Magnetic map of the same path at different speeds

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

(21)

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

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

(22)

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

2.3ZUPT_HESO_GM算法

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

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

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

(23)

(24)

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

3 行人導(dǎo)航實(shí)驗(yàn)

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

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

圖12 室外路徑Fig.12 Outdoor path

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

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

3.2 算法實(shí)驗(yàn)驗(yàn)證

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

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

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

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

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

表2 不同算法誤差對(duì)比

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

4 結(jié)論

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

References)

[1] 汪少初. 基于PDR的定位與跟蹤技術(shù)研究及系統(tǒng)設(shè)計(jì)[D].天津：天津大學(xué), 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)個(gè)人導(dǎo)航方法(英文)[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 熊明亮, 刁夢(mèng)雯, 趙國(guó)梁. 基于地球磁場(chǎng)的室內(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] 宋鏢, 程磊, 周明達(dá),等. 基于慣導(dǎo)輔助地磁的手機(jī)室內(nèi)定位系統(tǒng)設(shè)計(jì)[J]. 傳感技術(shù)學(xué)報(bào), 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距離的地磁匹配導(dǎo)航算法[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].中國(guó)慣性技術(shù)學(xué)報(bào)， 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

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

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

熊智(1976—)，男，研究員，博士生導(dǎo)師。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