本發(fā)明涉及室內(nèi)慣性導(dǎo)航與定位領(lǐng)域，尤其是涉及一種利用慣性傳感器和智能手機(jī)實(shí)現(xiàn)的基于行人腳部姿態(tài)識(shí)別和步長模型的行人室內(nèi)慣性導(dǎo)航技術(shù)。

背景技術(shù)：
:

在室內(nèi)環(huán)境，衛(wèi)星衛(wèi)星信號(hào)到達(dá)地面時(shí)信號(hào)較弱、不能穿透建筑物、無法發(fā)揮精確的定位作用。慣性導(dǎo)航是通過測量載體的加速度，并進(jìn)行積分運(yùn)算，獲得載體瞬時(shí)速度和瞬時(shí)位置數(shù)據(jù)的技術(shù)。組成慣性導(dǎo)航系統(tǒng)的設(shè)備都安裝在載體內(nèi)，工作時(shí)不依賴外界信息，也不向外界輻射能量，不易受到干擾，是一種自主式導(dǎo)航設(shè)備。慣性導(dǎo)航技術(shù)作為一種自主式導(dǎo)航技術(shù)，相對其他室內(nèi)定位技術(shù)具有不需要事先在建筑物內(nèi)布設(shè)接收裝置、不受環(huán)境變化和信號(hào)阻擋等影響等優(yōu)點(diǎn)，成為室內(nèi)定位技術(shù)的研究熱點(diǎn)。由于行人在走動(dòng)時(shí)身體運(yùn)動(dòng)情況較復(fù)雜，而慣性導(dǎo)航算法存在誤差累積，這些問題使得慣性導(dǎo)航應(yīng)用在行人室內(nèi)導(dǎo)航時(shí)的精確性很低，限制了它的廣泛應(yīng)用。

室內(nèi)慣性導(dǎo)航技術(shù)的研究從上世紀(jì)90年代開始出現(xiàn)，Levi和Judd在1996年首次提出了行人航跡推算(Pedestrian Dead Reckoning,PDR)的概念，并在Point Research公司開發(fā)的PDR模塊中進(jìn)行了應(yīng)用。由于人員的運(yùn)動(dòng)狀態(tài)比較復(fù)雜，很多科研高校的研究傾向于將慣性傳感器佩戴在室內(nèi)人員身體的一個(gè)或多個(gè)不同部位，包含頭、腿、腰或者腳等，來實(shí)現(xiàn)室內(nèi)行人的導(dǎo)航功能。其中將MEMS(Micro-Electro-Mechanical System)慣性傳感器佩戴在室內(nèi)人員的腳部，通過測量獲取室內(nèi)人員的腳步的運(yùn)動(dòng)數(shù)據(jù)來解算室內(nèi)人員的導(dǎo)航軌跡的實(shí)驗(yàn)效果較為精確。傳統(tǒng)的慣性導(dǎo)航算法依靠對傳感器加速度和速度的積分來計(jì)算運(yùn)動(dòng)距離，在應(yīng)用到行人室內(nèi)導(dǎo)航時(shí)，隨著行人移動(dòng)距離增加和航向角的變化，其累積誤差越來越大進(jìn)而導(dǎo)致精度不高。

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

針對上述現(xiàn)有技術(shù)的不足，本發(fā)明提出了一種以行人腳部姿態(tài)為判斷、以步長為單位劃分行人運(yùn)動(dòng)的導(dǎo)航算法。

一種基于姿態(tài)識(shí)別和步長模型的室內(nèi)慣性導(dǎo)航算法，其特征是，包括以下步驟：

(1)行人慣性數(shù)據(jù)與智能手機(jī)傳感器數(shù)據(jù)的采集、傳輸、存儲(chǔ)以及濾波處理；

(2)多條件零速度判斷法和行人腳部姿態(tài)判別相結(jié)合，解算得出行人的步頻與步數(shù)特征；

(3)對行人的每一步分別進(jìn)行慣性導(dǎo)航解算，建立步長模型，作為行人位移的參考值，根據(jù)行人運(yùn)動(dòng)狀態(tài)調(diào)整步長結(jié)果。

進(jìn)一步，采集行人慣性數(shù)據(jù)時(shí)，將慣性傳感器佩戴在行人腳部，對慣性傳感器數(shù)據(jù)進(jìn)行初始校準(zhǔn)，消除零偏，設(shè)置采樣頻率100Hz和波特率特征；使用藍(lán)牙模塊將器件與智能手機(jī)連接，通過手機(jī)端的程序記錄藍(lán)牙傳輸?shù)绞謾C(jī)的行人慣性數(shù)據(jù)和手機(jī)自身的電子羅盤數(shù)據(jù)，并存儲(chǔ)在手機(jī)中。

進(jìn)一步，對慣性傳感器數(shù)據(jù)進(jìn)行低通數(shù)字濾波，對電子羅盤數(shù)據(jù)進(jìn)行滑動(dòng)均值濾波，消除器件的噪聲。

進(jìn)一步，對處理后的數(shù)據(jù)進(jìn)行多條件零速度檢測：包括加速度幅值檢測、角速度幅值檢測和加速度方差檢測，設(shè)定閾值，對三個(gè)檢測條件進(jìn)行與運(yùn)算求出零速度時(shí)間范圍；使用加速度計(jì)、角速度計(jì)和磁力計(jì)解算出器件的姿態(tài)角信息，根據(jù)姿態(tài)角檢測在零速度檢測結(jié)果的基礎(chǔ)上得到行人零速度時(shí)刻。

進(jìn)一步，進(jìn)行慣性導(dǎo)航解算，將零速度時(shí)的行人加速度置零，把行人的運(yùn)動(dòng)以每一步為分割分別計(jì)算；統(tǒng)計(jì)每一步的步頻和對應(yīng)的慣性導(dǎo)航位移信息，將步頻和姿態(tài)穩(wěn)定時(shí)的結(jié)果進(jìn)行分類統(tǒng)計(jì)并建立步長模型；

在行人姿態(tài)及步頻變化、與模型不匹配時(shí)以慣性導(dǎo)航解算為位移結(jié)果，當(dāng)慣性導(dǎo)航結(jié)果有累積誤差累加的傾向時(shí)，把當(dāng)解算結(jié)果不合理的步長舍棄并把該步頻對應(yīng)的步長模型信息作為參考結(jié)果，最終將位移及方向信息融合，得到行人導(dǎo)航信息，在手機(jī)端顯示。

本發(fā)明所采用的技術(shù)方案是：基于智能手機(jī)和MEMS慣性傳感器，使用零速度檢測方法和腳部姿態(tài)判別方法將將行人運(yùn)動(dòng)分割成以單步為單位，分別解算單步位移并建立行人慣常步長模型，根據(jù)行人步頻變化情況判斷行人的運(yùn)動(dòng)狀態(tài)實(shí)時(shí)校正步長算法，通過實(shí)時(shí)判斷行人的運(yùn)動(dòng)情況相應(yīng)匹配相應(yīng)的步長算法，得到行人精確的運(yùn)動(dòng)軌跡。本發(fā)明的技術(shù)方案是：

(1)零速度檢測和姿態(tài)角檢測相結(jié)合的步頻檢測算法

為了針對行人運(yùn)動(dòng)的特點(diǎn)設(shè)計(jì)導(dǎo)航模型，需要對行人進(jìn)行腳部姿態(tài)判斷。行人在行走時(shí)，通過鞋底與地面的摩擦力，向運(yùn)動(dòng)方向跨出一步。鞋踏向地面、與地面完全接觸、又離開地面。根據(jù)慣性傳感器相對于地面有相對靜止的一段時(shí)間，我們可以認(rèn)為此時(shí)行人處于零速度狀態(tài)，兩個(gè)零速度之間，就是行人行走了一步。在這個(gè)“零速度”時(shí)刻，行人的加速度、角速度以及磁力計(jì)數(shù)據(jù)都在接近零值的狀態(tài)，可以通過設(shè)置這些數(shù)據(jù)閾值的方法判斷“零速度”點(diǎn)，然而這種判斷方法在行人偶爾處于慢速或停頓的時(shí)刻會(huì)出現(xiàn)無效判定，影響計(jì)步精度；而隨著腳的抬起和落下，腳部相對于水平面的俯仰角也有著一個(gè)循環(huán)變化的過程，在腳部處于地面時(shí)，俯仰角接近水平，而即使行人慢速行走時(shí)其俯仰角判斷也不會(huì)受影響。所以本發(fā)明結(jié)合了零速度檢測和姿態(tài)角檢測，能夠準(zhǔn)確判斷出行人行走的姿態(tài)，確定行人腳部的落地點(diǎn)，從而確定步數(shù)和步頻。具體步驟如下：

1)對MEMS慣性傳感器采集到的行人運(yùn)動(dòng)時(shí)的初始數(shù)據(jù)(包括加速度、角速度、磁力計(jì))進(jìn)行初始校準(zhǔn)處理和濾波處理，濾除器件的漂移誤差。

2)對加速度、角速度和磁力計(jì)數(shù)據(jù)進(jìn)行數(shù)據(jù)分析，設(shè)定閾值，分別判定零速度時(shí)刻，建立零速度檢測數(shù)據(jù)。

3)使用姿態(tài)解算算法計(jì)算行人的姿態(tài)角，包括橫滾角、偏轉(zhuǎn)角和俯仰角，利用俯仰角數(shù)據(jù)研究行人抬起和落下的動(dòng)作。

4)采取零速度檢測和姿態(tài)角檢測雙判斷方法，當(dāng)行人的姿態(tài)角變化不明顯、有多極值出現(xiàn)的時(shí)間段，使用零速度檢測判斷法檢測步數(shù)，在姿態(tài)角極值變化穩(wěn)定時(shí)使用姿態(tài)角步態(tài)檢測方法，并將兩者結(jié)果結(jié)合，確定行人的零速度點(diǎn)。

5)通過零速度點(diǎn)，記錄行人每一步的步頻、步數(shù)，為下一步驟做準(zhǔn)備。

(2)基于步長模型的實(shí)時(shí)校正慣性導(dǎo)航算法

考慮到行人運(yùn)動(dòng)的復(fù)雜性，本發(fā)明對誤差累積使用校正機(jī)制。實(shí)驗(yàn)研究發(fā)現(xiàn)，人的步頻和步幅之間有著對應(yīng)關(guān)系，行人的步頻一定時(shí)，其步幅是基本固定的?；谶@一研究，本發(fā)明對行人的運(yùn)動(dòng)進(jìn)行分割，把位移以每一步為單位分別進(jìn)行慣性導(dǎo)航解算，并通過統(tǒng)計(jì)累積誤差并未擴(kuò)大的前10步的數(shù)據(jù)結(jié)果建立步長模型，在行人以慣常步態(tài)行走時(shí)以此模型為主要參考。步長的慣性導(dǎo)航算法可以很好的減少累積誤差，即使是在長距離行走之后，慣性導(dǎo)航的計(jì)算結(jié)果仍處于較為精確的范圍?；诓介L的慣性導(dǎo)航算法具體包括以下步驟：

1)以零速度點(diǎn)劃分，兩個(gè)零速度點(diǎn)之間分別作為起始和終止間隔，通過對加速度的二次積分得出位移信息。

2)統(tǒng)計(jì)不同步頻對應(yīng)的步長結(jié)果，通過多步訓(xùn)練建立步頻與步長對應(yīng)的步長模型。

3)在行人實(shí)際行走過程中，通過步頻和航向角判斷行人是否處于慣常步態(tài)，如果是，則調(diào)用步長模型對慣導(dǎo)解算結(jié)果進(jìn)行校正；如果不是，則使用慣導(dǎo)解算的結(jié)果。結(jié)合這兩種結(jié)果輸出相對精確的行人位置信息。

本發(fā)明與現(xiàn)有技術(shù)相比，具有的有益效果是：

1)本系統(tǒng)基于MEMS慣性傳感器和智能手機(jī)，屬于自主式導(dǎo)航設(shè)備，不需要在建筑物內(nèi)布設(shè)信號(hào)接收裝置，適應(yīng)性強(qiáng)、抗干擾能力強(qiáng)。

2)使用多條件零速度檢測法與基于姿態(tài)檢測的姿態(tài)角檢測法相結(jié)合來確定行人運(yùn)動(dòng)時(shí)的零速度點(diǎn)，計(jì)算行人的步頻和步數(shù)，有效提高了對行人運(yùn)動(dòng)步態(tài)的判斷精度。

3)使用基于步長模型的慣性導(dǎo)航算法，將行人的行走分割成以單步為單位分別計(jì)算，并訓(xùn)練出步長模型，在行人以慣常步態(tài)行走時(shí)參考步長模型的結(jié)果，有效避免了慣性導(dǎo)航算法的誤差隨著時(shí)間增加而逐漸累積，提高了定位精度。

附圖說明：

圖1是本發(fā)明系統(tǒng)的總體流程圖；

圖2是設(shè)備佩戴示圖；

圖3是步頻/步數(shù)檢測方法框圖；

圖4是以步長為單位的行人慣性導(dǎo)航算法框圖；

圖5是手機(jī)端行人軌跡顯示軟件界面；

具體實(shí)施方式：

下面結(jié)合附圖和實(shí)例對本發(fā)明做進(jìn)一步說明。本發(fā)明的系統(tǒng)的總體流程圖見圖1。

本發(fā)明使用了集成MPU6050慣性模塊、AK8963磁力計(jì)模塊以及藍(lán)牙HC-06模塊的慣性傳感器，慣性傳感器件通過藍(lán)牙與安卓智能手機(jī)無線連接并且實(shí)時(shí)的將傳感器測得的行人運(yùn)動(dòng)數(shù)據(jù)傳輸?shù)绞謾C(jī)中，同時(shí)調(diào)用手機(jī)內(nèi)置的加速度傳感器、電子羅盤數(shù)據(jù)，在智能手機(jī)中實(shí)現(xiàn)數(shù)據(jù)存儲(chǔ)、處理以及導(dǎo)航輸出。行人佩戴設(shè)備如圖2所示。本發(fā)明針對MEMS慣性傳感器本身精度不高、存在漂移誤差導(dǎo)致慣導(dǎo)解算算法的結(jié)果存在累積誤差的問題，根據(jù)行人運(yùn)動(dòng)時(shí)腳部的姿態(tài)特點(diǎn)，將行人位移分割以單步長為單位，通過建立步長模型來實(shí)時(shí)校正慣導(dǎo)解算結(jié)果的準(zhǔn)確性。通過多條件零速度檢測和行人姿態(tài)角檢測算法相結(jié)合的步頻算法計(jì)算出行人的步頻和步數(shù)；通過慣導(dǎo)解算和實(shí)時(shí)建立步長模型并根據(jù)行人的步頻、姿態(tài)變化特性實(shí)時(shí)校正步長結(jié)果、計(jì)算出行人的位移情況；結(jié)合基于滑動(dòng)均值濾波的航向角算法判斷行人的實(shí)時(shí)航向角，從而計(jì)算得出行人完整的運(yùn)動(dòng)軌跡；最終在手機(jī)上顯示行人的運(yùn)動(dòng)軌跡，實(shí)現(xiàn)行人的室內(nèi)導(dǎo)航功能。具體實(shí)施過程如下：

(1)基于加速度計(jì)、角速度計(jì)和磁力計(jì)的行人步頻與步數(shù)算法。

步頻/步數(shù)檢測方法框圖如圖3所示。步驟如下：

1)對MPU6050傳感器進(jìn)行初始校準(zhǔn)和低通濾波處理，濾除高于5Hz的高頻信號(hào)，消除傳感器的零偏和高頻噪聲，降低傳感器自身的漂移誤差。

2)使用多條件判斷法判定行人的零速度點(diǎn)，分別是加速度幅值、加速度方差和角速度幅值三種檢測方法，公式如下：

其中，

k為當(dāng)前數(shù)據(jù)點(diǎn)，b表示載體坐標(biāo)系b系，c₁、c₂和c₃取邏輯值0或1，是三個(gè)軸向上的合加速度的幅值，其閾值th_amtr和th_amax分別設(shè)為0.1和1.0，若在此范圍內(nèi)，則將c₁置1，表示此時(shí)為合加速度幅值判定的“零速度”時(shí)刻，否則置0，表示此時(shí)為零速度時(shí)刻；是合加速度的方差，s是計(jì)算方差的數(shù)據(jù)間隔，一般設(shè)為3或4，其閾值th_a的大小設(shè)為0.5，若在此范圍內(nèi)，則將c₂置1，表示此時(shí)為加速度方差判定的“零速度”時(shí)刻，否則置0；是三個(gè)軸向上的合角速度的幅值，其閾值th_ω的大小設(shè)為6，若在此范圍內(nèi)，則將c₃置1，表示此時(shí)為角速度判定的“零速度”時(shí)刻，否則置0；以上閾值的設(shè)定是根據(jù)實(shí)驗(yàn)行人的具體行走情況，經(jīng)過大量的實(shí)驗(yàn)測試與統(tǒng)計(jì)，確定的最優(yōu)零條件判斷數(shù)值。在不同的行人及硬件情況下，可以重新設(shè)定其最優(yōu)閾值。最后，將單個(gè)條件結(jié)果進(jìn)行“與”運(yùn)算，求出腳著地，即“零速度”的時(shí)刻：

c＝c₁*c₂*c₃

上式的c是邏輯“與”運(yùn)算的結(jié)果。當(dāng)c取值為1時(shí)，說明此刻是“零速度”時(shí)刻；當(dāng)取值為0時(shí)，說明是行人運(yùn)動(dòng)的“非零”速度時(shí)刻。

多條件零速度檢測判斷法的優(yōu)點(diǎn)是集合了多種行人瞬時(shí)的速度數(shù)據(jù)并求交集，可以有針對性的選擇出各條件判斷都符合的觀察點(diǎn)；其缺點(diǎn)在于需要設(shè)立合理的閾值范圍才能更準(zhǔn)確的判斷，如果閾值范圍很小，會(huì)出現(xiàn)漏判零速度點(diǎn)的情況；而零速度范圍比較大，則會(huì)出現(xiàn)零速度時(shí)刻比實(shí)際時(shí)間長的情況，影響下一步的慣導(dǎo)解算。針對這一問題，本發(fā)明又提出了姿態(tài)角檢測以確定零速度點(diǎn)。

3)通過佩戴在行人腳部的慣性傳感器解算行人行走時(shí)腳部的姿態(tài)數(shù)據(jù)，方法如下：

行人處于靜止?fàn)顟B(tài)時(shí)測得加速度信息中的重力場分量在載體坐標(biāo)系b系下的表達(dá)為而重力場在導(dǎo)航坐標(biāo)系下的分量為gⁿ＝[0 0 g]^T，則有轉(zhuǎn)換關(guān)系為：

其中，是由導(dǎo)航坐標(biāo)系n系到載體坐標(biāo)系b系下的轉(zhuǎn)換矩陣，分別是重力在b系下x、y、z三軸的分量。由上式可以計(jì)算獲得橫滾角γ和俯仰角θ：

此公式只適用于在靜止?fàn)顟B(tài)計(jì)算姿態(tài)角，當(dāng)傳感器處于運(yùn)動(dòng)狀態(tài)時(shí)，由于傳感器本身存在加速度，與重力產(chǎn)生的加速度重合，而單純依靠加速度計(jì)無法分離出重力分量，所以使用加速度計(jì)的作用是獲得初始姿態(tài)角和姿態(tài)角校正，而運(yùn)動(dòng)時(shí)的姿態(tài)角需要通過陀螺儀計(jì)算得出。

一個(gè)動(dòng)坐標(biāo)系相對參考坐標(biāo)系的方位可以完全由動(dòng)坐標(biāo)系依次繞3個(gè)不同的軸轉(zhuǎn)動(dòng)的3個(gè)角度來確定。如把載體坐標(biāo)系作為動(dòng)坐標(biāo)系，把導(dǎo)航坐標(biāo)系作為參考坐標(biāo)系，則姿態(tài)角即為一組歐拉角，按一定的轉(zhuǎn)動(dòng)順序得到導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的關(guān)系。

上式中左側(cè)，是本次更新后的歐拉角，對應(yīng)橫滾角γ、俯仰角θ和偏轉(zhuǎn)角ψ。右側(cè)，是上個(gè)周期測算出來的角度，以及三軸陀螺儀在周期t內(nèi)測得的角速度。因此求解這個(gè)微分方程就能解算出當(dāng)前的歐拉角。

其中代表需要解算的歐拉角，k指當(dāng)前數(shù)據(jù)點(diǎn)，t是慣性傳感器的采樣周期，取為0.01s，e_t為陀螺儀的零偏值。計(jì)算出陀螺儀三個(gè)軸的角度變化，再以加速度計(jì)算出的初始姿態(tài)角為基準(zhǔn)，就能得出載體的姿態(tài)角數(shù)據(jù)。

行人在行走時(shí)其俯仰角有著周期性的變化，其變化幅度在-10度到90度之間，研究行人的步態(tài)可以發(fā)現(xiàn)，其極大值和極小值分別是在踏向地面時(shí)和離開地面時(shí)的姿態(tài)角，由此可以判斷出行人的步數(shù)間隔。利用姿態(tài)角的判別方法的優(yōu)點(diǎn)是可以根據(jù)行人運(yùn)動(dòng)姿態(tài)判斷步數(shù)，判定結(jié)果更加直觀、準(zhǔn)確，缺點(diǎn)是可能出現(xiàn)多極大值、極小值情況，影響判斷精度。所以在多條件零速度檢測法的檢測出零速度時(shí)間段的基礎(chǔ)上，再利用姿態(tài)角檢測準(zhǔn)確判斷出零速度點(diǎn)，經(jīng)過試驗(yàn)驗(yàn)證，單純使用多條件零速度判斷法，步頻檢測結(jié)果的正確率在90％至95％之間，而使用姿態(tài)角與零速度判斷結(jié)合算法的正確率可以達(dá)到98％以上，該算法有效提高了步頻和步數(shù)的檢測精度。

(2)基于滑動(dòng)均值濾波的航向角更新算法

實(shí)驗(yàn)中模塊被平穩(wěn)固定在行人腳面，行人手持手機(jī)正對行走方向。在個(gè)人航跡推算導(dǎo)航系統(tǒng)中，對運(yùn)動(dòng)方向的估算主要是使用智能手機(jī)中的電子羅盤數(shù)據(jù)，電子羅盤在靜止?fàn)顟B(tài)下航向角非常準(zhǔn)確，可以作為參考；然而，當(dāng)行人手持手機(jī)處于運(yùn)動(dòng)狀態(tài)時(shí)，由于行人重心的移動(dòng)和手部的抖動(dòng)，電子羅盤的輸出數(shù)據(jù)也相應(yīng)的有一些偏移。我們采用滑動(dòng)均值濾波算法對電子羅盤輸出數(shù)據(jù)進(jìn)行校正?；瑒?dòng)均值濾波器公式如下：

其中，k為當(dāng)前數(shù)據(jù)點(diǎn)，為智能手機(jī)電子羅盤的輸出原始數(shù)據(jù)，j取當(dāng)前數(shù)據(jù)點(diǎn)以及當(dāng)前數(shù)據(jù)點(diǎn)最近的m組數(shù)據(jù)，為濾波輸出結(jié)果；m為一次進(jìn)行濾波處理的數(shù)據(jù)個(gè)數(shù)，也稱滑動(dòng)濾波器階數(shù)，本發(fā)明根據(jù)行人的步頻特征和實(shí)驗(yàn)數(shù)據(jù)，將m取值為100。我們將m個(gè)連續(xù)的采樣信號(hào)看成一個(gè)長度為m隊(duì)列，隨著濾波運(yùn)算的進(jìn)行，一個(gè)新數(shù)據(jù)獲得后立即放入隊(duì)列末尾，并拋棄以前隊(duì)首的一個(gè)數(shù)據(jù)，這樣不斷地把獲得新數(shù)據(jù)放隊(duì)首，并且淘汰以前舊的數(shù)據(jù)，這樣可以利用最新數(shù)據(jù)信息進(jìn)行更新并且保證了運(yùn)算的實(shí)時(shí)性。經(jīng)實(shí)驗(yàn)驗(yàn)證，電子羅盤使用航向角更新算法處理后的數(shù)據(jù)保持了較好的平滑性，消除了由于設(shè)備在行人運(yùn)動(dòng)中產(chǎn)生的偏移誤差。

(3)以步長為單位的行人慣性導(dǎo)航算法

行人慣性導(dǎo)航算法框圖所示，步驟如下：

1)加速度積分及位移計(jì)算：

MEMS慣性傳感器獲得的是沿載體坐標(biāo)系b系下三個(gè)軸向加速度數(shù)據(jù)a^b：

通過已經(jīng)結(jié)算出的姿態(tài)角求出的坐標(biāo)轉(zhuǎn)換矩陣通過可以獲得沿n系(即東北天坐標(biāo)系，x軸以地理北向?yàn)檎?、y軸以地理北向?yàn)檎?、z軸以垂直于地理東向和北向的天向?yàn)檎?下的輸出數(shù)據(jù)aⁿ:

然后再將求得的aⁿ減去重力加速度，即可以獲得運(yùn)動(dòng)物體在n系下的加速度數(shù)據(jù)aⁱⁿ。在本系統(tǒng)中，采用的MPU6050慣性傳感器的采樣頻率設(shè)置為100Hz，能夠得到采樣間隔Δt＝0.01s，由于Δt較短，在十分短的時(shí)間內(nèi)我們可以認(rèn)為運(yùn)動(dòng)載體在做勻速直線運(yùn)動(dòng)。由牛頓第二定律可知，在導(dǎo)航坐標(biāo)系下，速度變化量等于加速度值對極短時(shí)間的積分，即：

則，載體在導(dǎo)航坐標(biāo)系下的速度為上一時(shí)刻的速度與瞬時(shí)加速度積分的累加：

再由位移公式，可得到n系下的運(yùn)動(dòng)物體位移的變化量：

最后，可以得到運(yùn)動(dòng)物體在n系下的位置為：

在確定了零速度點(diǎn)之后，兩個(gè)零速度點(diǎn)之間就是每一步的步長，通過上文中的慣導(dǎo)解算算法可以計(jì)算出每一步的步長。

2)訓(xùn)練步長模型：

在步頻穩(wěn)定時(shí)，慣導(dǎo)解算計(jì)算出的步長在真實(shí)值之間振蕩，其偏移大概在20％以內(nèi)，隨著時(shí)間的積累，這些誤差會(huì)累積，導(dǎo)致導(dǎo)航出現(xiàn)較大偏差。根據(jù)在初始階段誤差并未累積時(shí)解算的步長計(jì)算結(jié)果訓(xùn)練出行人以固定步頻運(yùn)動(dòng)的步長模型，取同一步頻(步頻差在5步/min范圍以內(nèi))的前10步的步長為模型參考數(shù)據(jù)，以此模型作為長距離行走時(shí)步長的參考值。下表為一個(gè)步長模型的示例：

當(dāng)行人當(dāng)前步的步頻在步長模型所包含的步頻范圍內(nèi)時(shí)，將慣導(dǎo)解算的不合理結(jié)果閾值范圍設(shè)置為該步步頻對應(yīng)步長模型結(jié)果的0.5倍以下或2倍以上，在這個(gè)范圍內(nèi)解算結(jié)果誤差較大，可以認(rèn)為這一步解算出的步長結(jié)果是不合理的，將其舍棄，解算結(jié)果出現(xiàn)不合理結(jié)果時(shí)，采用步長模型的數(shù)據(jù)作為參考結(jié)果；而當(dāng)行人步頻和航向角變化、即行人在做變向或變速運(yùn)動(dòng)時(shí)，將步頻變化閾值設(shè)為0.2s、航向角變化閾值設(shè)為20度，超過這個(gè)閾值范圍，認(rèn)為行人較上一步其行走狀態(tài)發(fā)生了變化，這時(shí)依然采用瞬時(shí)值更加精確的慣性導(dǎo)航計(jì)算結(jié)果。這兩種匹配策略的目的是將慣導(dǎo)解算過程中出現(xiàn)明顯誤差的不合理數(shù)據(jù)進(jìn)行糾正，根據(jù)行人行走時(shí)的運(yùn)動(dòng)特點(diǎn)實(shí)時(shí)校正慣導(dǎo)結(jié)算結(jié)果，保證導(dǎo)航算法始終處于一個(gè)比較精確的范圍。

最后，在獲得了步頻、步長以及航向角的數(shù)據(jù)之后，經(jīng)過每一步的累加，即可獲得完整的位移數(shù)據(jù)。行人軌跡顯示軟件如圖5所示。經(jīng)過大量試驗(yàn)驗(yàn)證，傳統(tǒng)慣性導(dǎo)航算法在行走距離增加時(shí)累積誤差較大，在100m行走之后其解算結(jié)果可靠性變差，位置誤差超過50％的情形頻繁出現(xiàn)，而采取本文的基于姿態(tài)識(shí)別和步長模型的室內(nèi)慣性導(dǎo)航算法，解算的準(zhǔn)確性并沒有隨著距離的增加出現(xiàn)衰減，在100m行走之后慣導(dǎo)解算依然保持了95％以上的精確度。