技術(shù)特征：

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

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

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

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

2.根據(jù)權(quán)利要求1所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，采集行人慣性數(shù)據(jù)時(shí)，將慣性傳感器佩戴在行人腳部，對(duì)慣性傳感器數(shù)據(jù)進(jìn)行初始校準(zhǔn)，消除零偏，設(shè)置采樣頻率100Hz和波特率特征；使用藍(lán)牙模塊將器件與智能手機(jī)連接，通過手機(jī)端的程序記錄藍(lán)牙傳輸?shù)绞謾C(jī)的行人慣性數(shù)據(jù)和手機(jī)自身的電子羅盤數(shù)據(jù)，并存儲(chǔ)在手機(jī)中。

3.根據(jù)權(quán)利要求2所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，對(duì)慣性傳感器數(shù)據(jù)進(jìn)行低通數(shù)字濾波，對(duì)電子羅盤數(shù)據(jù)進(jìn)行滑動(dòng)均值濾波，消除器件的噪聲。

4.根據(jù)權(quán)利要求1所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，對(duì)處理后的數(shù)據(jù)進(jìn)行多條件零速度檢測(cè)：包括加速度幅值檢測(cè)、角速度幅值檢測(cè)和加速度方差檢測(cè)，設(shè)定閾值，對(duì)三個(gè)檢測(cè)條件進(jìn)行與運(yùn)算求出零速度時(shí)間范圍；使用加速度計(jì)、角速度計(jì)和磁力計(jì)解算出器件的姿態(tài)角信息，根據(jù)姿態(tài)角檢測(cè)在零速度檢測(cè)結(jié)果的基礎(chǔ)上得到行人零速度時(shí)刻。

5.根據(jù)權(quán)利要求1所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，進(jìn)行慣性導(dǎo)航解算，將零速度時(shí)的行人加速度置零，把行人的運(yùn)動(dòng)以每一步為分割分別計(jì)算；統(tǒng)計(jì)每一步的步頻和對(duì)應(yīng)的慣性導(dǎo)航位移信息，將步頻和姿態(tài)穩(wěn)定時(shí)的結(jié)果進(jìn)行分類統(tǒng)計(jì)并建立步長(zhǎng)模型；

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

6.根據(jù)權(quán)利要求1所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，多條件零速度判斷法和行人腳部姿態(tài)判別相結(jié)合解算得出行人的步頻與步數(shù)特征的具體實(shí)施過程如下：

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

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

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

$\left\{\begin{array}{c}\left|a_k^b\right|=\sqrt{\[a_k^b{\left(1\right)}^2+a_k^b{\left(2\right)}^2+a_k^b{\left(3\right)}^2\]},c_1=\left\{\begin{array}{cc}1,& {\mathrm{th}}_{a\min }\<\left|a_k^b\right|\<{\mathrm{th}}_{a\max }\\ 0,& \left|a_k^b\right|\>{\mathrm{th}}_a\end{array}\right.\\ {\mathrm{\σ}}_{a_k^b}^2=\frac{1}{2s+1}{\Σ}_{f=k-s}^{k+s}{(a_f^b-\stackrel{\‾}{a_k^b})}^2,c_2=\left\{\begin{array}{cc}1,& {\mathrm{\σ}}_{a_k^b}^2\<{\mathrm{th}}_{\mathrm{\σ}}\\ 0,& {\mathrm{\σ}}_{a_k^b}^2\>={\mathrm{th}}_{\mathrm{\σ}}\end{array}\right.\\ \left|{\mathrm{\ω}}_k^b\right|=\sqrt{\[{\mathrm{\ω}}_k^b{\left(1\right)}^2+{\mathrm{\ω}}_k^b{\left(2\right)}^2+{\mathrm{\ω}}_k^b{\left(3\right)}^2\]},c_3=\left\{\begin{array}{cc}1,& \left|{\mathrm{\ω}}_k^b\right|\<{\mathrm{th}}_{\mathrm{\ω}}\\ 0,& \left|{\mathrm{\ω}}_k^b\right|\>={\mathrm{th}}_{\mathrm{\ω}}\end{array}\right.\end{array}\right.$

其中，

k為當(dāng)前數(shù)據(jù)點(diǎn)，b表示載體坐標(biāo)系b系，c₁、c₂和c₃取邏輯值0或1，是三個(gè)軸向上的合加速度的幅值，其閾值th_amin和th_amax分別設(shè)為0.1和1.0，若在此范圍內(nèi)，則將c₁置1，表示此時(shí)為合加速度幅值判定的“零速度”時(shí)刻，否則置0，表示此時(shí)為零速度時(shí)刻；是合加速度的方差，s是計(jì)算方差的數(shù)據(jù)間隔，設(shè)為3或4，其閾值th_σ的大小設(shè)為0.5，若在此范圍內(nèi)，則將c₂置1，表示此時(shí)為加速度方差判定的“零速度”時(shí)刻，否則置0；是三個(gè)軸向上的合角速度的幅值，其閾值th_ω的大小設(shè)為6，若在此范圍內(nèi)，則將c₃置1，表示此時(shí)為角速度判定的“零速度”時(shí)刻，否則置0；最后，將單個(gè)條件結(jié)果進(jìn)行“與”運(yùn)算，求出腳著地，即“零速度”的時(shí)刻：

c＝c₁*c₂*c₃

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

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

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

$g^b=C_n^b{g}^n=\left[\begin{array}{c}-g\sin \mathrm{\θ}\\ g\sin \mathrm{\γ}\cos \mathrm{\θ}\\ g\cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

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

$\mathrm{\γ}={\tan }^{-1}\left(\frac{g_y^b}{g_z^b}\right)$

$\mathrm{\θ}={\sin }^{-1}\left(\frac{-g_x^b}{g}\right)$

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

一個(gè)動(dòng)坐標(biāo)系相對(duì)參考坐標(biāo)系的方位由動(dòng)坐標(biāo)系依次繞3個(gè)不同的軸轉(zhuǎn)動(dòng)的3個(gè)角度來(lái)確定；如把載體坐標(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)系；

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{ccc}cos\mathrm{\θ}& sin\mathrm{\γ}sin\mathrm{\θ}& cos\mathrm{\γ}sin\mathrm{\θ}\\ 0& cos\mathrm{\θ}cos\mathrm{\γ}& -sin\mathrm{\γ}cos\mathrm{\θ}\\ 0& \sin \mathrm{\γ}& cos\mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_x^b\\ {\mathrm{\ω}}_y^b\\ {\mathrm{\ω}}_z^b\end{array}\right]$

上式中左側(cè)，是本次更新后的歐拉角，對(duì)應(yīng)橫滾角γ、俯仰角θ和偏轉(zhuǎn)角ψ；右側(cè)，是上個(gè)周期測(cè)算出來(lái)的角度，以及三軸陀螺儀在周期t內(nèi)測(cè)得的角速度；因此求解這個(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度之間，其極大值和極小值分別是在踏向地面時(shí)和離開地面時(shí)的姿態(tài)角，由此判斷出行人的步數(shù)間隔。

7.根據(jù)權(quán)利要求1所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，采用滑動(dòng)均值濾波算法對(duì)電子羅盤輸出數(shù)據(jù)進(jìn)行校正；滑動(dòng)均值濾波器公式如下：

$y_k=\frac{1}{m}\underset{i=k-m+1}{\overset{k}{\Σ}}{x}_i$

其中，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ù)。

8.根據(jù)權(quán)利要求1所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，步驟(3)中的行人慣性導(dǎo)航算法，步驟如下：

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

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

$a^b={\left[\begin{array}{ccc}{a}_x^b& a_y^b& a_z^b\end{array}\right]}^T$

通過已經(jīng)結(jié)算出的姿態(tài)角求出的坐標(biāo)轉(zhuǎn)換矩陣通過獲得沿n系下的輸出數(shù)據(jù)aⁿ:

$a^n=C_b^n{a}^b$

然后再將求得的aⁿ減去重力加速度，即獲得運(yùn)動(dòng)物體在n系下的加速度數(shù)據(jù)a′ⁿ；通過慣性傳感器的采樣頻率得到采樣間隔Δt，由牛頓第二定律可知，在導(dǎo)航坐標(biāo)系下，速度變化量等于加速度值對(duì)極短時(shí)間的積分，即：

${\mathrm{\Δv}}_x^n=a_x^{\′n}\·\mathrm{\Δ}t$

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

$v_x^n(t+\mathrm{\Δ}t\]=v_x^n(t)+{\mathrm{\Δv}}_x^n=v_x^n\left(t\right)+a_x^{\′n}\·\mathrm{\Δ}t$

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

${\mathrm{\ΔS}}_x^n=v_x^n\·\mathrm{\Δ}t+\frac{1}{2}{a}_x^{\′n}\·{\mathrm{\Δt}}^2$

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

$S_x^n(t+\mathrm{\Δ}t)=S_x^n\left(t\right)+{\mathrm{\ΔS}}_x^n=S_x^n\left(t\right)+v_x^n\·\mathrm{\Δ}t+\frac{1}{2}{a}_x^{\′n}\·{\mathrm{\Δt}}^2$

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

9.根據(jù)權(quán)利要求1所述的基于姿態(tài)識(shí)別和步長(zhǎng)模型的室內(nèi)慣性導(dǎo)航算法，其特征在于，步驟(3)具體為：

根據(jù)在初始階段誤差并未累積時(shí)解算的步長(zhǎng)計(jì)算結(jié)果訓(xùn)練出行人以固定步頻運(yùn)動(dòng)的步長(zhǎng)模型，取步頻差在5步/min范圍以內(nèi)的前10步的步長(zhǎng)為模型參考數(shù)據(jù)，以此模型作為長(zhǎng)距離行走時(shí)步長(zhǎng)的參考值；

當(dāng)行人當(dāng)前步的步頻在步長(zhǎng)模型所包含的步頻范圍內(nèi)時(shí)，將慣導(dǎo)解算的不合理結(jié)果閾值范圍設(shè)置為該步步頻對(duì)應(yīng)步長(zhǎng)模型結(jié)果的0.5倍以下或2倍以上，在這個(gè)范圍內(nèi)解算結(jié)果誤差較大，認(rèn)為這一步解算出的步長(zhǎng)結(jié)果是不合理的，將其舍棄，解算結(jié)果出現(xiàn)不合理結(jié)果時(shí)，采用步長(zhǎng)模型的數(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é)果；

在獲得了步頻、步長(zhǎng)以及航向角的數(shù)據(jù)之后，經(jīng)過每一步的累加，獲得完整的位移數(shù)據(jù)。