本發(fā)明涉及一種航向誤差修正方法，尤其涉及一種陀螺儀航向漂移誤差修正方法。本發(fā)明屬于室內(nèi)人員自主定位技術(shù)領(lǐng)域，具體說是一種利用穩(wěn)態(tài)磁場修正陀螺儀航向漂移誤差的方法與裝置及磁場檢測方法與裝置。

背景技術(shù)：

在基于微慣導(dǎo)的人員自主定位技術(shù)中，利用固定在人員足部微機(jī)電系統(tǒng)(micro-electromechanicalsystems,mems)mems的慣性測量單元(包括微型加速度計(jì)和微型陀螺儀)對(duì)人體運(yùn)動(dòng)進(jìn)行測量，積分推演獲得人員運(yùn)動(dòng)軌跡。該技術(shù)無需在定位場所預(yù)先安裝任何設(shè)備，具有自主性、低功耗及便攜性等優(yōu)勢(shì)，已成為衛(wèi)星導(dǎo)航定位不可靠情況下重要的“補(bǔ)盲”手段。

受制造工藝水平的限制，微型陀螺儀在實(shí)際的應(yīng)用中會(huì)產(chǎn)生漂移，導(dǎo)致陀螺儀解算航向誤差隨時(shí)間不斷積累，導(dǎo)致定位誤差不斷增大。地磁場是一種穩(wěn)定的地理信息，通常被用于輔助修正陀螺儀航向誤差。但在室內(nèi)場景中，由于地磁場受墻體、電子設(shè)備等的干擾比較嚴(yán)重，導(dǎo)致地磁場解算航向存在較大誤差。

考慮到地磁場在某些地點(diǎn)或者時(shí)間段受到的干擾很小，受到干擾很小的磁場稱為穩(wěn)態(tài)磁場。在穩(wěn)態(tài)磁場中，解算出的航向誤差可以忽略，因此可用來修正陀螺儀航向誤差。

從受干擾磁場中提取穩(wěn)態(tài)磁場依賴于穩(wěn)態(tài)磁場檢測方法。傳統(tǒng)的穩(wěn)態(tài)磁場檢測方法通過磁場幅度的變換率作為判斷磁場受干擾程度的依據(jù)，當(dāng)磁場幅度變化率小于某一閾值時(shí)，判定該時(shí)刻磁場處于穩(wěn)態(tài)。若磁場在短時(shí)間內(nèi)受到比較穩(wěn)定的干擾，則該方法很容易造成虛警檢測，即誤將受干擾磁場判定為穩(wěn)態(tài)磁場。若用其解算航向值修正陀螺儀航向，反而會(huì)引入額外的誤差，極大地影響測量精度。在受到干擾的情況下，磁場在導(dǎo)航坐標(biāo)系(東-北-天)三個(gè)軸向的受干擾程度往往是不相同的，即各軸向磁場變化比例不同，導(dǎo)致解算出來的航向存在較大的波動(dòng)，而這樣的磁場航向角無法有效的被用來修正陀螺儀航向角的長期漂移特性。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明要解決的問題是針對(duì)現(xiàn)有技術(shù)中利用穩(wěn)態(tài)磁場的解算航向值修正陀螺儀航向會(huì)引入額外的誤差的問題，提供一種新的航向誤差修正方法與裝置及磁場檢測方法與裝置。

為解決上述技術(shù)問題，本發(fā)明采用的技術(shù)方案是：一種航向誤差修正方法，包括如下步驟：

(a)人員行進(jìn)中，以固定周期采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)；

(b)利用下式計(jì)算檢驗(yàn)統(tǒng)計(jì)量t

其中，α1、α2、α3、α4分別為第一權(quán)值、第二權(quán)值、第三權(quán)值、第四權(quán)值，且α1、α2、α3、α4的取值范圍均為[0,1]，且α1+α2+α3+α4＝1，m為人員行進(jìn)的步數(shù)間隔，m＞2，s為人員步數(shù)，bi、θi、ξi分別為人員行進(jìn)中第i步的導(dǎo)航坐標(biāo)系下的磁場幅度、磁場解算航向、陀螺儀解算航向、磁場解算航向與陀螺儀解算航向的偏差，分別為磁場幅度的噪聲方差、磁場解算航向的噪聲方差、陀螺儀解算航向的噪聲方差、磁場解算航向與陀螺儀解算航向的偏差的方差，bi、θi、ξi、均利用步驟(a)中的磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)計(jì)算得到；

(c)若t＜γ，則判定在人員行進(jìn)中第s步到第s+m-1步之間對(duì)應(yīng)的時(shí)間區(qū)間內(nèi)磁場處于穩(wěn)態(tài)，并跳轉(zhuǎn)到步驟(d)，否則跳轉(zhuǎn)到步驟(a)，其中，γ為磁場穩(wěn)態(tài)檢測門限；

(d)在由步驟(c)得到的人員行進(jìn)中第s步到第s+m-1步之間對(duì)應(yīng)的時(shí)間區(qū)間內(nèi)，將磁場解算航向作為觀測量，利用卡爾曼濾波對(duì)航向角誤差進(jìn)行估計(jì)，利用航向角誤差對(duì)人員的航向進(jìn)行修正。

在受到干擾的情況下，磁場在導(dǎo)航坐標(biāo)系(東-北-天)三個(gè)軸向的受干擾程度往往是不相同的，即各軸向磁場變化比例不同，導(dǎo)致解算出來的航向存在較大的波動(dòng)，而這樣的磁場航向角無法有效的被用來修正陀螺儀航向角的長期漂移特性。本發(fā)明中，利用構(gòu)造檢驗(yàn)統(tǒng)計(jì)量t檢驗(yàn)磁場是否為穩(wěn)態(tài)，即判斷磁場解算航向的可信度，若磁場為穩(wěn)態(tài)，則利用磁場航向角修正陀螺儀航向角的漂移。

若α4＝0，則利用磁場解算航向的相對(duì)變化、磁場幅度的相對(duì)變化、陀螺儀短時(shí)航向的相對(duì)變化構(gòu)造檢驗(yàn)統(tǒng)計(jì)量，則對(duì)直線行走的效果較好；若α1、α2、α3、α4均不為0，則由于加入了磁場解算航向與陀螺儀解算航向的偏差值的變化量δξs，因此可以在連續(xù)轉(zhuǎn)彎環(huán)境下確定磁場角度連續(xù)變化以及陀螺連續(xù)角度變化的之間的差異，如果磁場解算航向與陀螺儀解算航向兩者之間的相對(duì)角度變化偏差是穩(wěn)定的，則對(duì)于連續(xù)轉(zhuǎn)彎的行走線路的磁場的穩(wěn)態(tài)策略仍然可以對(duì)陀螺起到修正作用，因此對(duì)曲線行走線路甚至是連續(xù)曲線的行走線路的修正較好。

上述技術(shù)方案中，所述步驟(b)中，bi、θi分別為從人員行進(jìn)中第i步的靜止時(shí)間區(qū)間中任取的時(shí)刻在導(dǎo)航坐標(biāo)系下的磁場幅度、磁場解算航向、陀螺儀解算航向，其中，第i步的靜止時(shí)間區(qū)間為人員行進(jìn)中第i步對(duì)應(yīng)的人員足部與地接觸的時(shí)間區(qū)間。

上述技術(shù)方案中，所述步驟(b)中，第i步的靜止時(shí)間區(qū)間通過如下步驟計(jì)算：

(b1)計(jì)算人員足部在k時(shí)刻的加速度模值其中(ax(k),ay(k),az(k))為k時(shí)刻人員足部在導(dǎo)航坐標(biāo)系下的加速度；

(b2)利用下式計(jì)算偏差σk和方差λk，

其中m1的取值范圍為3≤m1≤10，m2的取值范圍為3≤m2≤10，且m1、m2均為整數(shù)；

(b3)設(shè)置偏差門限ηbias、方差門限ηv，若式子λk≥ηv和σk≥ηbias中至少有一項(xiàng)成立，則判斷人員足部在k時(shí)刻為運(yùn)動(dòng)狀態(tài)，反之，則判斷人員足部在k時(shí)刻為靜止?fàn)顟B(tài)；

(b4)若檢測到人員足部在時(shí)間區(qū)間[k1i,k2i]內(nèi)的每個(gè)時(shí)刻均為靜止?fàn)顟B(tài)，而檢測到人員足部在k1i-1時(shí)刻、k2i+1時(shí)刻均為運(yùn)動(dòng)狀態(tài)，則認(rèn)為時(shí)間區(qū)間[k1i,k2i]為人員行進(jìn)中的第i步對(duì)應(yīng)的靜止時(shí)間區(qū)間。

上述技術(shù)方案中，所述步驟(b)中，θi通過姿態(tài)四元數(shù)計(jì)算得到，bi、ξi通過下式計(jì)算得到

其中，(bi,x,bi,y,bi,z)為人員行進(jìn)中的第i步時(shí)人員足部在導(dǎo)航坐標(biāo)系下的磁場強(qiáng)度，d為地磁偏角。

上述技術(shù)方案中，所述的步驟(b)中，m＝5。

上述技術(shù)方案中，所述的步驟(c)中，磁場穩(wěn)態(tài)檢測門限γ通過接收器操作特性曲線確定。

上述技術(shù)方案中，所述步驟(d)中，利用航向角誤差對(duì)人員行進(jìn)中第s+m-1步對(duì)應(yīng)的航向進(jìn)行修正或?qū)Φ趕步到第s+m-1步中對(duì)應(yīng)的各個(gè)航向進(jìn)行修正。

本發(fā)明還提供一種實(shí)現(xiàn)上述航向誤差方法的航向誤差修正裝置，包括設(shè)置于人員足部且具有相同載體坐標(biāo)系的加速度計(jì)、陀螺儀、磁力計(jì)，還包括用于采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)的信號(hào)采集單元，還包括與所述信號(hào)采集單元連接的信號(hào)處理單元。

本發(fā)明還提供一種磁場檢測方法，在人員足部設(shè)置具有相同載體坐標(biāo)系的加速度計(jì)、陀螺儀、磁力計(jì)，所述磁場檢測方法包括如下步驟：

(a)人員行進(jìn)中，以固定周期采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)；

(b)利用下式計(jì)算檢驗(yàn)統(tǒng)計(jì)量t

其中，α1、α2、α3、α4分別為第一權(quán)值、第二權(quán)值、第三權(quán)值、第四權(quán)值，且α1、α2、α3、α4的取值范圍均為[0,1]，且α1+α2+α3+α4＝1，m為人員行進(jìn)的步數(shù)間隔，m＞2，s為人員步數(shù)，bi、θi、ξi分別為人員行進(jìn)中第i步的導(dǎo)航坐標(biāo)系下的磁場幅度、磁場解算航向、陀螺儀解算航向、磁場解算航向與陀螺儀解算航向的偏差，分別為磁場幅度的噪聲方差、磁場解算航向的噪聲方差、陀螺儀解算航向的噪聲方差、磁場解算航向與陀螺儀解算航向的偏差的方差，bi、θi、ξi、均利用步驟(a)中的磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)計(jì)算得到；

(c)若t＜γ，則判定在人員行進(jìn)中第s步到第s+m-1步之間對(duì)應(yīng)的時(shí)間區(qū)間內(nèi)磁場處于穩(wěn)態(tài)。

本發(fā)明還提供一種磁場檢測裝置，包括設(shè)置于人員足部且具有相同載體坐標(biāo)系的加速度計(jì)、陀螺儀、磁力計(jì)，還包括用于采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)的信號(hào)采集單元，還包括與所述信號(hào)采集單元連接的信號(hào)處理單元。

本發(fā)明具有的優(yōu)點(diǎn)和積極效果是：本發(fā)明可以極大改善利用磁力計(jì)航向角修正慣導(dǎo)航向誤差的精度，降低室內(nèi)外環(huán)境因素對(duì)磁力計(jì)干擾的影響；新的檢測策略在保證有效磁場向角檢測，降低檢測誤檢率的同時(shí)，通過融合修正抑制了慣導(dǎo)航向的累積誤差。本發(fā)明直線行走的效果較好，而且對(duì)曲線行走線路甚至是連續(xù)曲線的行走線路的修正也有較好的效果。相比較傳統(tǒng)檢測方法，本發(fā)明可大幅降低穩(wěn)態(tài)磁場的檢測虛警率，提高檢測精度。采用本發(fā)明的方法，導(dǎo)航定位軌跡的航向誤差明顯降低。

附圖說明

圖1為本發(fā)明的航向誤差修正方法的處理流程示意圖；

圖2為本發(fā)明的航向誤差修正裝置的結(jié)構(gòu)示意圖；

圖3為本發(fā)明的磁場檢測方法處理流程示意圖；

圖4為檢測統(tǒng)計(jì)量曲線，圖中橫坐標(biāo)為步數(shù)，縱坐標(biāo)為檢測統(tǒng)計(jì)量，圖中采用的參數(shù)為：m1、m2取值為5，σθ＝0.56rad/s,

圖5為定位效果對(duì)比圖，圖中外圍實(shí)線表示墻體，實(shí)心方形曲線為采用傳統(tǒng)檢測方法解算軌跡，空心圓形曲線為采用本發(fā)明的航向誤差修正方法的解算軌跡；

圖中，1、人員足部，2、加速度計(jì)，3、陀螺儀，4、磁力計(jì)，5、信號(hào)采集單元，6、信號(hào)處理單元，7、數(shù)據(jù)傳輸單元。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步說明。

本發(fā)明中，若判定人員行進(jìn)過程中某一步的磁場為穩(wěn)態(tài)磁場，則對(duì)相應(yīng)的靜止時(shí)間區(qū)間內(nèi)的各個(gè)時(shí)刻的磁場數(shù)據(jù)求出的航向角進(jìn)行修正。

穩(wěn)態(tài)磁場是對(duì)地磁場穩(wěn)定性的判斷性描述，由于在復(fù)雜的室內(nèi)或者室外環(huán)境下地磁場會(huì)受到不同程度的污染干擾，從而導(dǎo)致依據(jù)其計(jì)算出來的航向角存在干擾，并間接的影響后續(xù)陀螺航向角修正。穩(wěn)態(tài)磁場中，在固定時(shí)間間隔內(nèi)，磁傳感器獲取的多個(gè)當(dāng)?shù)氐卮艌隹倧?qiáng)度采樣值保持穩(wěn)定，或者說磁傳感器獲取到的磁場強(qiáng)度隨時(shí)間的變化率約等于零，磁傳感器可采用磁力計(jì)。

本發(fā)明的基本思路是：在受到干擾的情況下，磁場在導(dǎo)航坐標(biāo)系(東-北-天)三個(gè)軸向的受干擾程度往往是不相同的，即各軸向磁場變化比例不同，導(dǎo)致解算出來的航向存在較大的波動(dòng)，而這樣的磁場航向角無法有效的被用來修正陀螺儀航向角的長期漂移特性。因此本發(fā)明利用磁場解算航向的相對(duì)變化、磁場幅度的相對(duì)變化以及陀螺儀短時(shí)航向的相對(duì)變化共同作為磁場解算航向可信度的一個(gè)判斷，即發(fā)明中提出的穩(wěn)態(tài)磁場檢測。

如圖1—圖5所示，將足部模塊安裝在鞋尖處，其中足部模塊融合了加速度計(jì)2、陀螺儀3、磁力計(jì)4，且加速度計(jì)2、陀螺儀3、磁力計(jì)4具有相同方向的載體坐標(biāo)系和導(dǎo)航坐標(biāo)系。測量坐標(biāo)系即為載體坐標(biāo)系。其中載體指人員足部1。實(shí)驗(yàn)中，人員沿著某辦公樓一樓內(nèi)墻行走一圈后回到出發(fā)點(diǎn)，路徑總長約626米。本專利實(shí)驗(yàn)所用模塊采樣率為400hz。磁場測量數(shù)據(jù)即為磁力計(jì)4測量得到的數(shù)據(jù)。磁場解算航向即為由磁場測量數(shù)據(jù)解算得到的航向。利用其它磁場測量裝置測量得到磁場測量數(shù)據(jù)也可實(shí)現(xiàn)本發(fā)明的方法。

本發(fā)明的航向誤差修正方法包括以下處理步驟：

步驟(a)：數(shù)據(jù)采集

人員行進(jìn)中，設(shè)置采樣周期，以固定周期采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)，采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)的固定周期可根據(jù)實(shí)際需要或由各個(gè)產(chǎn)品的實(shí)際參數(shù)確定，例如可設(shè)置為0.001-5秒的任意數(shù)值或其他數(shù)值，k為采樣時(shí)刻，k＝0,1,2,3……；s＝1,2,3……；三軸加速度計(jì)在k時(shí)刻測得人員足部1在載體坐標(biāo)系下的加速度計(jì)測量數(shù)據(jù)為(ax(k),ay(k),az(k))，磁力計(jì)4在k時(shí)刻測得人員足部1部在載體坐標(biāo)系下的磁場強(qiáng)度為(hx(k),hy(k),hz(k))，k時(shí)刻人員足部1在導(dǎo)航坐標(biāo)系下的磁場強(qiáng)度為(bx(k),by(k),bz(k))；

步驟(b)：參數(shù)計(jì)算

利用步驟(a)中的磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)計(jì)算人員行進(jìn)中第i步在導(dǎo)航坐標(biāo)系下的磁場幅度bi、磁場解算航向陀螺儀解算航向θi以及人員行進(jìn)中第i步的導(dǎo)航坐標(biāo)系下的磁場幅度bi、磁場解算航向陀螺儀解算航向θi、磁場解算航向與陀螺儀解算航向的偏差ξi以及磁場幅度的噪聲方差磁場解算航向的噪聲方差陀螺儀解算航向的噪聲方差磁場解算航向與陀螺儀解算航向的偏差的方差其中s為人員步數(shù)，s＝1,2,3……，i∈[s,s+m-1]。本實(shí)施例中，利用步態(tài)靜止檢測方法確定人員各個(gè)步子的靜止時(shí)間區(qū)間，然后利用第i步的靜止時(shí)間區(qū)間中任取的時(shí)刻在導(dǎo)航坐標(biāo)系下的磁場幅度、磁場解算航向、陀螺儀解算航向得到bi、θi、ξi。實(shí)際過程中，也可不計(jì)算靜止時(shí)間區(qū)間，通過其他類似方法得到bi、θi、ξi、等各個(gè)參數(shù)。本實(shí)施例的靜止檢測方法中，將固定長度時(shí)間窗口內(nèi)的加速計(jì)模值和模值方差作為檢測依據(jù)，且時(shí)間窗口隨著時(shí)間變化滑動(dòng)。第i步的靜止時(shí)間區(qū)間為人員行進(jìn)中第i步對(duì)應(yīng)的足部與地接觸的時(shí)間區(qū)間。

(b1)計(jì)算人員足部1在k時(shí)刻的加速度模值

(b2)利用下式計(jì)算偏差σk和方差λk，將方差λk取為短時(shí)間計(jì)算區(qū)間(k-m1,k+m2)，從而避免了判斷延遲，

其中m1的取值范圍為3≤m1≤10，m2的取值范圍為3≤m2≤10，且m1、m2均為整數(shù)，本實(shí)施例中，m1、m2的優(yōu)選值為5；

(b3)設(shè)置偏差門限ηbias、方差門限ηv，若式子λk≥ηv和σk≥ηbias中至少有一項(xiàng)成立，則判斷人員足部1在k時(shí)刻為運(yùn)動(dòng)狀態(tài)，反之，則判斷人員足部1在k時(shí)刻為靜止?fàn)顟B(tài)。本步驟中，考慮了人員行走的規(guī)律，判斷偏差和方差須同時(shí)滿足條件，才能判斷人員足部為靜止?fàn)顟B(tài)。偏差門限ηbias和方差門限ηv根據(jù)加速度計(jì)測量數(shù)據(jù)的統(tǒng)計(jì)特性來確定，例如采集加速度計(jì)測量數(shù)據(jù)，通過試驗(yàn)確定偏差門限ηbias和方差門限ηv的取值。此處的加速度統(tǒng)計(jì)特性是依據(jù)實(shí)際應(yīng)用是所使用的加速度傳感器的型號(hào)不同，它的硬件性能不同，以及器件安裝使用的不同，而確定加速計(jì)統(tǒng)計(jì)特性，本領(lǐng)域的人員可以理解；所述偏差門限ηbias和方差門限ηv通過加速計(jì)測量數(shù)據(jù)的統(tǒng)計(jì)特性計(jì)算得到，本實(shí)施例中，方差門限ηv可設(shè)置為0.05(m/s²)²到0.8(m/s²)²之間的數(shù)值，偏差門限ηbias可設(shè)置為小于0.15m/s²的數(shù)值；

(b4)若檢測到人員足部1在時(shí)間區(qū)間[k1i,k2i]內(nèi)的每個(gè)時(shí)刻均為靜止?fàn)顟B(tài)，而檢測到人員足部1在k1i-1時(shí)刻、k2i+1時(shí)刻均為運(yùn)動(dòng)狀態(tài)，則認(rèn)為時(shí)間區(qū)間[k1i,k2i]為人員行進(jìn)中的第i步對(duì)應(yīng)的靜止時(shí)間區(qū)間，同理即可得到人員行進(jìn)中每一步對(duì)應(yīng)的靜止時(shí)間區(qū)間，第i步的靜止時(shí)間區(qū)間即為人員行進(jìn)中第i步對(duì)應(yīng)的足部與地接觸的時(shí)間區(qū)間，其中k1i＜k2i；

(b5)將靜止時(shí)間區(qū)間中的磁場數(shù)據(jù)由載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系

設(shè)k時(shí)刻磁力計(jì)的載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的歐拉角為(r(k),p(k),y(k))，其中r(k)為橫滾角，p(k)為俯仰角，y(k)為航向角。在足部著地時(shí)的靜止時(shí)間區(qū)間內(nèi)，r(k)和p(k)可由人員足部1的加速度數(shù)據(jù)計(jì)算得出，即：

其中，g為重力加速度。

由于在k時(shí)刻測得的人員足部1的磁場強(qiáng)度(hx(k),hy(k),hz(k))是在載體坐標(biāo)系中的數(shù)據(jù)，而通常定位是在導(dǎo)航坐標(biāo)系下進(jìn)行，因此，需在進(jìn)行解算航向前將載體坐標(biāo)系下的磁場數(shù)據(jù)(hx(k),hy(k),hz(k))轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下的磁場強(qiáng)度(bx(k),by(k),bz(k))，轉(zhuǎn)換通過下式進(jìn)行：

(b6)計(jì)算靜止時(shí)間區(qū)間內(nèi)磁場幅度、磁場解算航向、陀螺儀解算航向、磁場解算航向與陀螺儀解算航向的偏差

磁場幅度的噪聲方差磁場解算航向的噪聲方差陀螺儀解算航向的噪聲方差磁場解算航向與陀螺儀解算航向的偏差可通過長時(shí)間進(jìn)行實(shí)驗(yàn)，利用統(tǒng)計(jì)方法計(jì)算得出，可將足部模塊靜止放置，連續(xù)采集兩小時(shí)的磁場幅度的噪聲數(shù)據(jù)、磁場解算航向的噪聲數(shù)據(jù)及陀螺儀解算航向的噪聲數(shù)據(jù)，經(jīng)過計(jì)算得到本實(shí)施例中的采集時(shí)間不對(duì)本發(fā)明作出限制，數(shù)據(jù)的采集時(shí)間根據(jù)實(shí)際需要確定。根據(jù)磁場解算航向的噪聲數(shù)據(jù)及陀螺儀解算航向的噪聲數(shù)據(jù)計(jì)算，因?yàn)檫@個(gè)變量是與這兩個(gè)變量相關(guān)的。

在人員足部1行進(jìn)中某一步的靜止時(shí)間區(qū)間內(nèi)，人員足部1的磁場強(qiáng)度基本保持穩(wěn)定，即磁場強(qiáng)度基本不發(fā)生變化，因此，計(jì)算該步磁場幅度時(shí)，可取靜止時(shí)間區(qū)間中的任意采樣點(diǎn)或根據(jù)一小段采樣點(diǎn)的平均數(shù)據(jù)得到。若在人員行進(jìn)中第i步的靜止時(shí)間區(qū)間[k1i,k2i]內(nèi)選取采樣點(diǎn)，則可任取時(shí)刻k3i，其中k3i∈[k1i,k2i]。若在人員行進(jìn)中第i步的靜止時(shí)間區(qū)間[k1i,k2i]內(nèi)選取一小段采樣點(diǎn)的平均值，則可選取時(shí)間區(qū)間[k11i,k12i]并將各個(gè)采樣點(diǎn)求平均值，其中k1i、k12i均為采樣時(shí)刻，且k11i≥k1i、k12i≤k2i。

當(dāng)下一步(第i+1步)靜止時(shí)間區(qū)間到來時(shí)，用相同方法計(jì)算第i+1步的陀螺儀航向、磁場幅度及磁場解算航行，并分別計(jì)算相鄰兩步(第i和第i+1步之間)的變化量。

人員行進(jìn)過程中，每一步都對(duì)應(yīng)靜止時(shí)間區(qū)間和運(yùn)動(dòng)時(shí)間區(qū)間，且兩步分別對(duì)應(yīng)的靜止時(shí)間區(qū)間之間的時(shí)間區(qū)間即為人員的運(yùn)動(dòng)時(shí)間區(qū)間。在本發(fā)明中，人員開始行進(jìn)，即利用上述步驟(b1)-(b4)檢測各個(gè)靜止時(shí)間區(qū)間，由此可以將人員行進(jìn)中第i步與第i步對(duì)應(yīng)的靜止時(shí)間區(qū)間相對(duì)應(yīng)。

人員行進(jìn)中第i步的導(dǎo)航坐標(biāo)系下的磁場幅度bi由下式計(jì)算：

同理計(jì)算得到第i+1步的磁場幅度bi+1，則相鄰兩步(第i和第i+1步之間)的磁場幅度變化量δbi為：

δbi＝|bi+1-bi|，

初始值δb1＝0。人員行進(jìn)中第i步的導(dǎo)航坐標(biāo)系下的磁場解算航向由下式計(jì)算：

其中d為地磁偏角，可通過查閱資料獲得。

同理計(jì)算得到第i+1步的磁場解算航向則相鄰兩步(第i+1和第i步之間)的磁場解算航向變化量為：

人員行進(jìn)中，利用陀螺儀測量數(shù)據(jù)，通過姿態(tài)四元數(shù)解算獲取陀螺航向角，人員行進(jìn)中姿態(tài)四元數(shù)的計(jì)算本領(lǐng)域的人員是可以理解的。人員行進(jìn)中第i步的陀螺儀解算航向表示為θi。

同理計(jì)算得到第i+1步的陀螺儀解算航向θi+1，則相鄰兩步(第i+1和第i步之間)的陀螺儀解算航向變化量δθi為：

δθi＝|θi+1-θi|

人員行進(jìn)中第i步的磁場解算航向與陀螺儀解算航向的偏差表示為

同理計(jì)算得到第i+1步的磁場解算航向與陀螺儀解算航向的偏差ξi+1，則相鄰兩步(第i+1和第i步之間)的磁場解算航向與陀螺儀解算航向的偏差變化量δξi為：

δξi＝|ξi+1-ξi|。

(b7)計(jì)算檢驗(yàn)統(tǒng)計(jì)量

結(jié)合似然比檢測，令檢測統(tǒng)計(jì)量t為

為提高魯棒性，在上式的磁場穩(wěn)態(tài)檢測中計(jì)算相鄰m步的磁場變化量，其中m的取值范圍為m＞2，優(yōu)選取值范圍為2＜m≤7，m優(yōu)選取值為5。s為人員步數(shù)。

α1、α2、α3、α4分別為第一權(quán)值、第二權(quán)值、第三權(quán)值、第四權(quán)值，且α1、α2、α3、α4的取值范圍均為[0,1]，且α1+α2+α3+α4＝1，α1、α2、α3、α4為分別與磁場幅度變化量、磁場解算航向變化量、陀螺儀解算航向變化量、磁場解算航向與陀螺儀解算航向的偏差變化量對(duì)應(yīng)的權(quán)值。

α1、α2、α3、α4根據(jù)實(shí)際試驗(yàn)或?qū)嶋H工程需要進(jìn)行計(jì)算，或可根據(jù)實(shí)際效果進(jìn)行調(diào)整，或可利用現(xiàn)有的數(shù)學(xué)方法，對(duì)α1、α2、α3、α4的最優(yōu)值進(jìn)行計(jì)算。

若α4＝0，則檢驗(yàn)統(tǒng)計(jì)量t僅由磁場幅度變化量、磁場解算航向變化量、陀螺儀解算航向變化量決定。

若α1、α2、α3、α4均不為0，則由于加入了磁場解算航向與陀螺儀解算航向的偏差值的變化量δξs，則可以在連續(xù)轉(zhuǎn)彎環(huán)境下確定磁場角度連續(xù)變化以及陀螺連續(xù)角度變化的之間的差異，如果磁場解算航向與陀螺儀解算航向兩者之間的相對(duì)角度變化偏差是穩(wěn)定的，則對(duì)于連續(xù)轉(zhuǎn)彎的環(huán)境下磁場的穩(wěn)態(tài)策略依舊可以對(duì)陀螺起到修正作用，因此對(duì)曲線行走線路甚至是連續(xù)曲線的行走線路的修正較好。

檢測統(tǒng)計(jì)量曲線如圖2所示。圖中橫坐標(biāo)為步數(shù)，縱坐標(biāo)為檢測統(tǒng)計(jì)量，其中m1、m2取值為5，σθ＝0.56rad/s,γ＝554mgs。

步驟(c)：磁場穩(wěn)態(tài)檢測

磁場穩(wěn)態(tài)檢測門限γ可通過接收器操作特性曲線(receiveroperatingcharacteristiccurve,或稱roc曲線)確定，接收器操作特性曲線通過改變磁場穩(wěn)態(tài)檢測門限，計(jì)算該磁場穩(wěn)態(tài)檢測門限下樣本數(shù)據(jù)的檢測概率和虛警概率，以虛警概率為橫坐標(biāo)，檢測概率為縱坐標(biāo)繪制得出的曲線。磁場穩(wěn)態(tài)檢測門限γ的值不恒定。γ的單位為毫高斯(mgs)。本實(shí)施例中，γ的優(yōu)選取值為850mgs，此值根據(jù)所在實(shí)驗(yàn)環(huán)境，考慮了周圍磁場干擾的情況下確定的磁場穩(wěn)態(tài)檢測門限γ。在實(shí)際專利應(yīng)用中，可以根據(jù)需要改變?chǔ)弥?，但γ的變?dòng)幅度應(yīng)參考標(biāo)準(zhǔn)地球磁場標(biāo)準(zhǔn)值。

將t與磁場穩(wěn)態(tài)檢測門限比較γ，若t＜γ，則判定磁場處于穩(wěn)態(tài)。若磁場處于穩(wěn)態(tài)，則跳轉(zhuǎn)到步驟(d)，否則跳轉(zhuǎn)到步驟(a)，繼續(xù)進(jìn)行數(shù)據(jù)采集。

步驟(d)：利用磁場解算航向?qū)ν勇輧x航向誤差進(jìn)行修正

若在步驟(b)中檢測得到磁場處于穩(wěn)態(tài)，則將步驟(b)中的磁場解算航向作為零速更新輔助的卡爾曼濾波的觀測量對(duì)陀螺儀航向誤差進(jìn)行修正。狀態(tài)轉(zhuǎn)移矩陣φk、過程噪聲wk-1、觀測矩陣h、觀測噪聲nk、速度誤差vk的計(jì)算為本領(lǐng)域公知常識(shí)。在本專利中，使用現(xiàn)有的卡爾曼濾波方法或卡爾曼濾波方法的變形也可以實(shí)現(xiàn)。通過本專利的卡爾曼濾波方法，可以求解得到陀螺和加速度誤差，并在后續(xù)的計(jì)算加以修正。本專利中，在滿足磁場穩(wěn)態(tài)條件的人員行進(jìn)中第s步到第s+m-1步之間的靜止時(shí)間區(qū)間(足部與地接觸的時(shí)間區(qū)間)內(nèi)進(jìn)行卡爾曼濾波，卡爾曼濾波是一直進(jìn)行的過程。

令卡爾曼濾波的狀態(tài)向量δxk|k為：

其中δφk為人員足部1的姿態(tài)角誤差，人員足部1的姿態(tài)角誤差包括橫滾角誤差、俯仰角誤差、航向角誤差，δωk為人員足部1的三軸角速度誤差，δrk為人員足部1的三維位置誤差，δvk為人員足部1的三軸速度誤差，δak為人員足部1的三軸加速度誤差?？柭鼮V波的狀態(tài)方程為：

δxk|k-1＝φkδxk-1|k-1+wk-1，

其中δxk|k-1為一步預(yù)測誤差狀態(tài)向量，δxk-1|k-1為上一步濾波后狀態(tài)向量，φk為狀態(tài)轉(zhuǎn)移矩陣，wk-1為過程噪聲，噪聲協(xié)方差矩陣

卡爾曼濾波量測方程為：

zk＝hδxk|k+nk，

其中zk為預(yù)測誤差觀測，h為觀測矩陣，nk為觀測噪聲，其協(xié)方差矩陣當(dāng)磁場穩(wěn)態(tài)檢測結(jié)果為穩(wěn)態(tài)時(shí)，磁場解算航向可作為參考校準(zhǔn)陀螺儀解算誤差。此時(shí)真實(shí)誤差觀測為：

mk＝[δψk,vk]，

其中vk為速度誤差，δψk為陀螺儀解算航向誤差。其中為陀螺儀解算航向，θk為磁場解算航向值。

卡爾曼濾波的估計(jì)誤差協(xié)方差矩陣pk|k-1可表示為：

令gk為卡爾曼濾波增益，則有：

gk＝pk|k-1h^t(hpk|k-1h^t+rk)^-1；

估計(jì)誤差狀態(tài)量可通過下式更新：

δxk|k＝δxk|k-1+gk·[mk-hδxk|k-1]；

其中，δxk|k包含航向角誤差估計(jì)，因此獲得δxk|k后可對(duì)航向角進(jìn)行校正。

本發(fā)明還提供一種實(shí)現(xiàn)上述航向誤差修正方法的航向誤差修正裝置，航向誤差修正裝置包括設(shè)置于人員足部1且具有相同載體坐標(biāo)系的加速度計(jì)2、陀螺儀3、磁力計(jì)4，還包括用于采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)的信號(hào)采集單元5，還包括與所述信號(hào)采集單元5連接的信號(hào)處理單元6?？衫脭?shù)據(jù)傳輸單元7將信號(hào)處理單元6處理得到的數(shù)據(jù)進(jìn)行傳輸。在信號(hào)處理單元6中執(zhí)行本發(fā)明中的航向誤差修正方法。信號(hào)處理單元6可由單片機(jī)、dsp、fpga等構(gòu)成。

本發(fā)明中，利用穩(wěn)態(tài)磁場航向角對(duì)慣導(dǎo)航向角進(jìn)行修正時(shí)，可僅修正人員行進(jìn)中第s+m-1步對(duì)應(yīng)的慣導(dǎo)航向角，也可修正第s步到第s+m-1步中對(duì)應(yīng)的人員的航向。例如，若人員行進(jìn)中第4-7步時(shí)，磁場為穩(wěn)態(tài)，且第3步為非穩(wěn)態(tài)，則可僅修正第7步對(duì)應(yīng)的人員的航向，即可滿足要求。如果需要達(dá)到更高的定位精度，也可對(duì)第4、5、6、7步對(duì)應(yīng)的人員的各個(gè)航向均進(jìn)行修正，也可僅修正第6、7步或第5、6、7步對(duì)應(yīng)的人員的各個(gè)航向。實(shí)際工作過程中，根據(jù)運(yùn)算效率或?qū)嶋H需要決定。本發(fā)明提出的航向誤差方法可用于室內(nèi)定位，也可應(yīng)用于其他類似的磁場易受干擾的環(huán)境的定位中。

圖4為實(shí)施例的檢測統(tǒng)計(jì)量的曲線圖。從圖4中可以看出,檢測統(tǒng)計(jì)量的波動(dòng)很大，從側(cè)面反映出室內(nèi)磁場的干擾比較嚴(yán)重。另外，由圖中結(jié)果可知，磁場受干擾較小的情況同樣存在，可用來修正陀螺儀航向誤差。

圖5為實(shí)施例的定位效果對(duì)比圖。圖中，矩形方框分別代表建筑物中的各個(gè)房間結(jié)構(gòu)。從圖5中可以看出，采用了傳統(tǒng)檢測方法解算軌跡航向發(fā)散較嚴(yán)重，定位已出現(xiàn)偏差，與真實(shí)運(yùn)動(dòng)區(qū)域完全偏離，定位結(jié)果已經(jīng)不可信，從圖中可以看出純慣導(dǎo)運(yùn)動(dòng)軌跡相對(duì)實(shí)際運(yùn)動(dòng)建筑物存在較大的誤差，解算軌跡已穿透墻體，定位誤差達(dá)到4m，航向誤差超過11°。采用本發(fā)明的航向誤差修正方法解算軌跡航向誤差明顯減小，定位軌跡和建筑物走向重合度明顯提高，解算軌跡與實(shí)際行走路徑基本吻合，達(dá)到終點(diǎn)時(shí)定位誤差不超過0.5m，航向誤差僅為2°，且定位的軌跡未出現(xiàn)穿過墻體的情況。

本發(fā)明還提供一種磁場檢測方法，在人員足部1設(shè)置具有相同載體坐標(biāo)系的加速度計(jì)、陀螺儀、磁力計(jì)，所述磁場檢測方法包括如下步驟：

(a)人員行進(jìn)中，以固定周期采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)；

(b)利用下式計(jì)算檢驗(yàn)統(tǒng)計(jì)量t

其中，α1、α2、α3、α4分別為第一權(quán)值、第二權(quán)值、第三權(quán)值、第四權(quán)值，且α1、α2、α3、α4的取值范圍均為[0,1]，且α1+α2+α3+α4＝1，m為人員行進(jìn)的步數(shù)間隔，m＞2，s為人員步數(shù)，bi、θi、ξi分別為人員行進(jìn)中第i步的導(dǎo)航坐標(biāo)系下的磁場幅度、磁場解算航向、陀螺儀解算航向、磁場解算航向與陀螺儀解算航向的偏差，分別為磁場幅度的噪聲方差、磁場解算航向的噪聲方差、陀螺儀解算航向的噪聲方差、磁場解算航向與陀螺儀解算航向的偏差的方差，bi、θi、ξi、均利用步驟(a)中的磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)計(jì)算得到；

(c)若t＜γ，則判定在人員行進(jìn)中第s步到第s+m-1步之間對(duì)應(yīng)的時(shí)間區(qū)間內(nèi)磁場處于穩(wěn)態(tài)。

本發(fā)明還提供一種實(shí)現(xiàn)上述磁場檢測方法的磁場檢測裝置，包括設(shè)置于人員足部1且具有相同載體坐標(biāo)系的加速度計(jì)2、陀螺儀3、磁力計(jì)4，還包括用于采集磁場測量數(shù)據(jù)、陀螺儀測量數(shù)據(jù)、加速度計(jì)測量數(shù)據(jù)的信號(hào)采集單元5，還包括與所述信號(hào)采集單元5連接的信號(hào)處理單元6。

以上對(duì)本發(fā)明的實(shí)施例進(jìn)行了詳細(xì)說明，但所述內(nèi)容僅為本發(fā)明的較佳實(shí)施例，不能被認(rèn)為用于限定本發(fā)明的實(shí)施范圍。凡依本發(fā)明范圍所作的均等變化與改進(jìn)等，均應(yīng)仍歸屬于本專利涵蓋范圍之內(nèi)。