本發(fā)明涉及通信技術(shù)領(lǐng)域，尤其涉及一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算方法及裝置。

背景技術(shù)：

在室內(nèi)、叢林等環(huán)境中，衛(wèi)星定位系統(tǒng)由于受遮擋而無法實(shí)現(xiàn)定位，在上述場景中的定位應(yīng)用中，基于慣性元件的步行者軌跡推算方案由于可以不受外界環(huán)境影響而實(shí)現(xiàn)全場景定位，因此越來越引起人們的廣泛關(guān)注。在基于慣性元件進(jìn)行步行者軌跡推算的方案中，應(yīng)用比較多的是零速修正算法。該算法利用步行者腳部著地時的零速區(qū)間獲得運(yùn)動觀測量而對運(yùn)動參數(shù)進(jìn)行修正，抑制陀螺儀漂移而提高定位精度。由于零速修正算法對零速區(qū)間檢測的精度要求很高，因此零速檢測方法直接決定了系統(tǒng)的整體性能。

傳統(tǒng)的零速檢測方法基于補(bǔ)償后的加速度計與陀螺儀測量數(shù)據(jù)，利用聯(lián)合閾值的方式進(jìn)行判斷，但由于所采用的閾值為固定的經(jīng)驗(yàn)值，無法適應(yīng)人的各種形態(tài)運(yùn)動而導(dǎo)致精度較差；基于Neyman-Pearson準(zhǔn)則的方法采用極大似然估計方法對零速狀態(tài)進(jìn)行估計，但實(shí)現(xiàn)過于復(fù)雜難以在裝置中獲得實(shí)際應(yīng)用；目前應(yīng)用的閾值自適應(yīng)調(diào)整方法一般為對人的行走與跑步兩種狀態(tài)做判斷，并以固定比例設(shè)置閾值，但該比例仍為提前設(shè)定，因此對快走與慢跑等復(fù)雜步態(tài)存在誤判的可能，從而導(dǎo)致精度降低。

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

鑒于上述的分析，本發(fā)明旨在提供一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算方法及裝置，可完全或者至少部分地解決上述問題。

為解決上述問題，本發(fā)明主要是通過以下技術(shù)方案實(shí)現(xiàn)的：

本發(fā)明一方面提供了一種閾值自適應(yīng)調(diào)整零速檢測步行者軌跡的推算方法，該方法包括：獲取步行者腳部的加速度數(shù)據(jù)、角速度數(shù)據(jù)以及磁場數(shù)據(jù)，對所述加速度數(shù)據(jù)、所述角速度數(shù)據(jù)以及所述磁場數(shù)據(jù)進(jìn)行補(bǔ)償，并對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；

根據(jù)前一步的采樣區(qū)間對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第一角速度能量均值，并根據(jù)當(dāng)前步的滑動窗口對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第二角速度能量均值；

根據(jù)計算得到的步伐頻率計算步伐頻率對應(yīng)的比例系數(shù)；

根據(jù)所述第一角速度能量均值與所述比例系數(shù)的乘積計算得到自適應(yīng)閾值，根據(jù)所述第二角速度能量均值與所述自適應(yīng)閾值進(jìn)行步行者的零速狀態(tài)判定。

進(jìn)一步地，所述對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率，具體包括：

計算加速度能量其中，a_x,a_y,a_z分別為加速度在x軸、y軸以及z軸的分量；

對加速度能量E_g進(jìn)行低通濾波，得到濾波后的加速度能量E′_g；

對低通濾波后的加速度能量E′_g進(jìn)行波峰與波谷初步判決，得到初步判決后的波峰與波谷序列；

對初步判決后的波谷進(jìn)行二次判決，確定有效波谷和無效波谷；

對有效波谷間隔取倒數(shù)得到步行者的步伐頻率v_f。

進(jìn)一步地，在初步判決后的波谷進(jìn)行二次判決，確定有效波谷和無效波谷，具體包括：

在初步判決后的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。

進(jìn)一步地，根據(jù)前一步的采樣區(qū)間對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第一角速度能量均值，并據(jù)根據(jù)當(dāng)前步的滑動窗口對補(bǔ)償后的角速度數(shù)，計算對應(yīng)的第二角速度能量均值，具體包括：

計算第i個采樣點(diǎn)的角速度模值||w_i||為：其中，w_x,w_y,w_z分別為角速度在x軸、y軸以及z軸的分量；

設(shè)(m,n)為第s步的采樣區(qū)間，計算第s步的角速度能量均值為：

計第s+1步內(nèi)的第i個采樣點(diǎn)角速度模值為||w_i||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內(nèi)角速度能量均值為：

進(jìn)一步地，根據(jù)所述第一角速度能量均值與所述比例系數(shù)的乘積計算得到自適應(yīng)閾值，根據(jù)所述第二角速度能量均值與所述自適應(yīng)閾值進(jìn)行步行者的零速狀態(tài)判定，具體包括：

對步行者的運(yùn)動狀態(tài)做判定：當(dāng)T＝1時，認(rèn)為當(dāng)前處于零速狀態(tài)，比例系數(shù)R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預(yù)設(shè)的經(jīng)驗(yàn)值。

本發(fā)明另一方面還提供了一種自適應(yīng)閾值零速檢測步行者軌跡的推算裝置，該裝置包括：

第一計算單元，用于獲取步行者腳部的加速度數(shù)據(jù)、角速度數(shù)據(jù)以及磁場數(shù)據(jù)，對所述加速度數(shù)據(jù)、所述角速度數(shù)據(jù)以及所述磁場數(shù)據(jù)進(jìn)行補(bǔ)償，并對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；

第二計算單元，用于根據(jù)前一步的采樣區(qū)間對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第一角速度能量均值，并根據(jù)當(dāng)前步的滑動窗口對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第二角速度能量均值，根據(jù)計算得到的步伐頻率計算步伐頻率對應(yīng)的比例系數(shù)；

第三計算單元，用于根據(jù)所述第一角速度能量均值與所述比例系數(shù)的乘積計算得到自適應(yīng)閾值，根據(jù)所述第二角速度能量均值與所述自適應(yīng)閾值進(jìn)行步行者的零速狀態(tài)判定。

進(jìn)一步地，所述第一計算單元還用于，計算加速度能量其中，a_x,a_y,a_z分別為加速度在x軸、y軸以及z軸的分量；對加速度能量E_g進(jìn)行低通濾波，得到濾波后的加速度能量E′_g；對低通濾波后的加速度能量E′_g進(jìn)行波峰與波谷初步判決，得到初步判決后的波峰與波谷序列；對初步判決后的波谷進(jìn)行二次判決，確定有效波谷和無效波谷；對有效波谷間隔取倒數(shù)得到步行者的步伐頻率v_f。

進(jìn)一步地，所述第一計算單元還用于，在初步判決后的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。

進(jìn)一步地，所述第一計算單元還用于，計算第i個采樣點(diǎn)的角速度模值||w_i||為：其中，w_x,w_y,w_z分別為角速度在x軸、y軸以及z軸的分量；設(shè)(m,n)為第s步的采樣區(qū)間，計算第s步的角速度能量均值為：計第s+1步內(nèi)的第i個采樣點(diǎn)角速度模值為||w_i||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內(nèi)角速度能量均值為：

進(jìn)一步地，所述第三計算單元還用于，對步行者的運(yùn)動狀態(tài)做判定：當(dāng)T＝1時，認(rèn)為當(dāng)前處于零速狀態(tài)，比例系數(shù)R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預(yù)設(shè)的經(jīng)驗(yàn)值。

本發(fā)明有益效果如下：

本發(fā)明通過對原始數(shù)據(jù)進(jìn)行補(bǔ)償，獲取可靠的原始數(shù)據(jù)，通過對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行低通濾波以及二次判決，進(jìn)行步伐頻率的提取，然后將步伐頻率作為輸入量對角速度能量均值的閾值進(jìn)行調(diào)整，從而達(dá)到閾值平滑自適應(yīng)調(diào)整的目的。與目前通用的閾值自適應(yīng)調(diào)整方法相比，可進(jìn)行閾值平滑調(diào)整，從而適應(yīng)步行者的各類步伐頻率的行動，克服了對快走與慢跑等情況誤判的問題，從而提高系統(tǒng)定位精度。

本發(fā)明的其他特征和優(yōu)點(diǎn)將在隨后的說明書中闡述，并且部分的從說明書中變得顯而易見，或者通過實(shí)施本發(fā)明而了解。本發(fā)明的目的和其他優(yōu)點(diǎn)可通過在所寫的說明書、權(quán)利要求書、以及附圖中所特別指出的結(jié)構(gòu)來實(shí)現(xiàn)和獲得。

附圖說明

圖1為本發(fā)明實(shí)施例的一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算方法的流程示意圖；

圖2為本發(fā)明實(shí)施例的另一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算方法的流程示意圖；

圖3為本發(fā)明實(shí)施例的再一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算方法的流程示意圖；

圖4為應(yīng)用本發(fā)明實(shí)施例的所述方法得到的沿運(yùn)動場運(yùn)動的軌跡圖；

圖5為應(yīng)用本發(fā)明實(shí)施例的所述方法得到的另一種沿運(yùn)動場運(yùn)動的軌跡圖；

圖6為本發(fā)明實(shí)施例的一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算裝置的結(jié)構(gòu)示意圖。

具體實(shí)施方式

下面結(jié)合附圖來具體描述本發(fā)明的優(yōu)選實(shí)施例，其中，附圖構(gòu)成本申請一部分，并與本發(fā)明的實(shí)施例一起用于闡釋本發(fā)明的原理。為了清楚和簡化目的，當(dāng)其可能使本發(fā)明的主題模糊不清時，將省略本文所描述的器件中已知功能和結(jié)構(gòu)的詳細(xì)具體說明。

本發(fā)明提供了一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算方法，本發(fā)明通過對原始數(shù)據(jù)進(jìn)行補(bǔ)償，獲取可靠的原始數(shù)據(jù)，通過對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行低通濾波以及二次判決，進(jìn)行步伐頻率的提取，然后將步伐頻率作為輸入量對角速度能量均值的閾值進(jìn)行調(diào)整，從而達(dá)到閾值自適應(yīng)調(diào)整的目的。與目前通用的閾值自適應(yīng)調(diào)整方法相比，可進(jìn)行閾值平滑調(diào)整，從而適應(yīng)步行者的各類步伐頻率的行動，克服了對快走與慢跑等情況誤判的問題，從而提高系統(tǒng)定位精度。以下結(jié)合附圖以及幾個實(shí)施例，對本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并不限定本發(fā)明。

本發(fā)明實(shí)施例提供了一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算方法，參見圖1，該方法包括：

S101、獲取步行者腳部的加速度數(shù)據(jù)、角速度數(shù)據(jù)以及磁場數(shù)據(jù)，對所述加速度數(shù)據(jù)、所述角速度數(shù)據(jù)以及所述磁場數(shù)據(jù)進(jìn)行補(bǔ)償，并對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；

S102、根據(jù)前一步的采樣區(qū)間對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第一角速度能量均值，并根據(jù)當(dāng)前步的滑動窗口對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第二角速度能量均值；

S103、根據(jù)計算得到的步伐頻率計算步伐頻率對應(yīng)的比例系數(shù)；

S104、根據(jù)所述第一角速度能量均值與所述比例系數(shù)的乘積計算得到自適應(yīng)閾值，根據(jù)所述第二角速度能量均值與所述自適應(yīng)閾值進(jìn)行步行者的零速狀態(tài)判定。

也就是說，本發(fā)明通過對原始數(shù)據(jù)進(jìn)行補(bǔ)償，獲取可靠的原始數(shù)據(jù)，通過對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行低通濾波以及二次判決，進(jìn)行步伐頻率的提取，然后將步伐頻率作為輸入量對角速度能量均值的閾值進(jìn)行調(diào)整，從而達(dá)到閾值自適應(yīng)調(diào)整的目的。與目前通用的閾值自適應(yīng)調(diào)整方法相比，可進(jìn)行閾值平滑調(diào)整，從而適應(yīng)步行者的各類步伐頻率的行動，克服了對快走與慢跑等情況誤判的問題，從而提高系統(tǒng)定位精度。

具體來說，本發(fā)明實(shí)施例是通過位對準(zhǔn)誤差補(bǔ)償來對獲取步行者腳部的加速度數(shù)據(jù)、角速度數(shù)據(jù)以及磁場數(shù)據(jù)進(jìn)行補(bǔ)償。當(dāng)然，本領(lǐng)域的技術(shù)人員也可以通過其他的方法來對獲取的各個數(shù)據(jù)進(jìn)行補(bǔ)償。

具體實(shí)施是，本發(fā)明所述對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率，具體包括：

計算加速度能量(其中，a_x,a_y,a_z分別為加速度在x軸、y軸以及z軸的分量)；

對加速度能量E_g進(jìn)行低通濾波，得到濾波后的加速度能量E′_g；

對低通濾波后的加速度能量E′_g進(jìn)行波峰與波谷初步判決，得到初步判決后的波峰與波谷序列；

對初步判決后的波谷進(jìn)行二次判決，確定有效波谷和無效波谷；

對有效波谷間隔取倒數(shù)得到步行者的步伐頻率v_f。

本發(fā)明實(shí)施例在初步判決后的波谷進(jìn)行二次判決，確定有效波谷和無效波谷，具體包括：

在初步判決后的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。

本發(fā)明實(shí)施例所述根據(jù)前一步的采樣區(qū)間對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第一角速度能量均值，并據(jù)根據(jù)當(dāng)前步的滑動窗口對補(bǔ)償后的角速度數(shù)，計算對應(yīng)的第二角速度能量均值，具體包括：

計算第i個采樣點(diǎn)的角速度模值||w_i||為：(其中，w_x,w_y,w_z分別為角速度在x軸、y軸以及z軸的分量)；

設(shè)(m,n)為第s步的采樣區(qū)間，計算第s步的角速度能量均值為：

計第s+1步內(nèi)的第i個采樣點(diǎn)角速度模值為||w_i||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內(nèi)角速度能量均值為：

本發(fā)明實(shí)施例所述根據(jù)所述第一角速度能量均值與所述比例系數(shù)的乘積計算得到自適應(yīng)閾值，根據(jù)所述第二角速度能量均值與所述自適應(yīng)閾值進(jìn)行步行者的零速狀態(tài)判定，具體包括：

對步行者的運(yùn)動狀態(tài)做判定：當(dāng)T＝1時，認(rèn)為當(dāng)前處于零速狀態(tài)，比例系數(shù)R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預(yù)設(shè)的經(jīng)驗(yàn)值。

如圖2所示，本發(fā)明提供一種自適應(yīng)閾值零速檢測的步行者軌跡推算方法，該方法的主要步驟包括：在步行者腳部固定微機(jī)電系統(tǒng)(MEMS)慣性傳感元件，包括加速度計、陀螺儀以及磁力計；系統(tǒng)開始工作后，首先選定起始工作點(diǎn)(原點(diǎn))，設(shè)定初始狀態(tài)；然后獲取由所述MEMS傳感器所提供的傳感數(shù)據(jù)，并進(jìn)行補(bǔ)償；隨后，利用自適應(yīng)閾值零速檢測方法，判定步行者是否處于零速狀態(tài)；若是，則基于擴(kuò)展卡爾曼濾波對系統(tǒng)狀態(tài)進(jìn)行校正，并進(jìn)行系統(tǒng)狀態(tài)遞推；若否，則直接進(jìn)行系統(tǒng)狀態(tài)遞推；循環(huán)執(zhí)行步行者零速狀態(tài)判定、基于擴(kuò)展卡爾曼濾波對系統(tǒng)狀態(tài)進(jìn)行更新、系統(tǒng)狀態(tài)遞推三個步驟；最后，輸出定位結(jié)果。

如圖3所示，本發(fā)明所提供的步行者零速狀態(tài)判定方法，首先需要進(jìn)行步頻計算，主要步驟包括：

計算加速度能量

對加速度能量E_g進(jìn)行低通濾波，得到E′_g；

對低通濾波后的E′_g進(jìn)行波峰與波谷判決，得到初步判決后的波峰與波谷序列；

對初步判決后的波峰與波谷序列進(jìn)行二次判決，判決方法為：在初步判決后的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定此波谷為有效波谷；否則，視為無效波谷；

對有效波谷間隔取倒數(shù)，即可得到步頻v_f。

計算得到步頻v_f之后，以其為參考計算閾值比例系數(shù)；并基于角速度能量均值，以前一步角速度能量均值與所述比例系數(shù)的乘積為閾值，進(jìn)行零速狀態(tài)檢測，具體步驟包括：

計算第i個采樣點(diǎn)的角速度模值||w_i||為：

設(shè)(m,n)為第s步的采樣區(qū)間，計算第s步的角速度能量均值為：

計第s+1步內(nèi)的第i個采樣點(diǎn)角速度模值為||w_i||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內(nèi)角速度能量均值為：

計算比例系數(shù)R_p：R_p＝k₀·v_f+α₀(其中，k₀與α₀為提前預(yù)設(shè)的經(jīng)驗(yàn)值)；

對運(yùn)動狀態(tài)做判定：

當(dāng)T＝1時，認(rèn)為當(dāng)前處于零速狀態(tài)。

總體來說，本發(fā)明實(shí)施例所述的方法包括：

在步行者腳部固定MEMS傳感器，獲取腳部的加速度、角速度以及磁場等原始傳感數(shù)據(jù)；

所述原始傳感數(shù)據(jù)需經(jīng)過相關(guān)補(bǔ)償方法進(jìn)行補(bǔ)償；

補(bǔ)償后的加速度數(shù)據(jù)需進(jìn)行能量計算、低通濾波與兩次判決過程，以確定步行者的步伐頻率；

補(bǔ)償后的角速度數(shù)據(jù)需根據(jù)前一步的采樣區(qū)間，計算對應(yīng)的角速度能量均值；

補(bǔ)償后的角速度數(shù)據(jù)需根據(jù)當(dāng)前步的滑動窗口，計算對應(yīng)的角速度能量均值；

依據(jù)所計算的步伐頻率，計算對應(yīng)的比例系數(shù)；

依據(jù)前一步角速度能量均值與所述比例系數(shù)的乘積計算自適應(yīng)閾值；

比較當(dāng)前步滑動窗口內(nèi)的角速度能量均值與所述自適應(yīng)閾值，進(jìn)行步行者的零速狀態(tài)判定。

圖5和圖6為利用本發(fā)明提供的方法對步行者軌跡進(jìn)行推算的結(jié)果圖，本發(fā)明采用了荷蘭Xsens公司的MTI 100模塊進(jìn)行了兩組步行實(shí)驗(yàn)。圖5對應(yīng)的是實(shí)驗(yàn)的場地為威海北洋電氣集團(tuán)股份有限公司的籃球場，沿籃球場白線行走，總路程約505米，回到起始點(diǎn)，始終點(diǎn)誤差為0.87米，定位精度為0.17％。圖6對應(yīng)的是實(shí)驗(yàn)的場地為山東大學(xué)(威海)體育場，沿跑道行走，回到起始點(diǎn)，總路程>800米，始終點(diǎn)誤差為2.89米，定位精度為0.36％。

總體來說，通過MEMS傳感器的應(yīng)用，可以在室內(nèi)、叢林等衛(wèi)星定位系統(tǒng)受遮擋而失效的環(huán)境中實(shí)現(xiàn)定位；通過對傳感器原始數(shù)據(jù)進(jìn)行補(bǔ)償、對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行低通濾波以及二次判決、角速度能量均值計算、閾值自適應(yīng)調(diào)整等過程，可以在步行者行走、跑步、快走、慢跑等各類行動條件下實(shí)現(xiàn)零速狀態(tài)的準(zhǔn)確判決，從而有效提高定位精度。

與圖1所示的方法相對應(yīng)的，本發(fā)明實(shí)施例還提供了一種對閾值平滑自適應(yīng)調(diào)整的零速修正步行者軌跡的推算裝置，參見圖6，該裝置包括：

第一計算單元，用于獲取步行者腳部的加速度數(shù)據(jù)、角速度數(shù)據(jù)以及磁場數(shù)據(jù)，對所述加速度數(shù)據(jù)、所述角速度數(shù)據(jù)以及所述磁場數(shù)據(jù)進(jìn)行補(bǔ)償，并對補(bǔ)償后的加速度數(shù)據(jù)進(jìn)行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；

第二計算單元，用于根據(jù)前一步的采樣區(qū)間對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第一角速度能量均值，并根據(jù)當(dāng)前步的滑動窗口對補(bǔ)償后的角速度數(shù)據(jù)，計算對應(yīng)的第二角速度能量均值，根據(jù)計算得到的步伐頻率計算步伐頻率對應(yīng)的比例系數(shù)；

第三計算單元，用于根據(jù)所述第一角速度能量均值與所述比例系數(shù)的乘積計算得到自適應(yīng)閾值，根據(jù)所述第二角速度能量均值與所述自適應(yīng)閾值進(jìn)行步行者的零速狀態(tài)判定。

具體實(shí)施時，本發(fā)明實(shí)施例所述第一計算單元還用于，計算加速度能量(其中，a_x,a_y,a_z分別為加速度在x軸、y軸以及z軸的分量)；對加速度能量E_g進(jìn)行低通濾波，得到濾波后的加速度能量E′_g；對低通濾波后的加速度能量E′_g進(jìn)行波峰與波谷初步判決，得到初步判決后的波峰與波谷序列；對初步判決后的波谷進(jìn)行二次判決，確定有效波谷和無效波谷；對有效波谷間隔取倒數(shù)得到步行者的步伐頻率v_f。

進(jìn)一步地，本發(fā)明實(shí)施例所述第一計算單元還用于，在初步判決后的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。

進(jìn)一步地，本發(fā)明實(shí)施例所述第一計算單元還用于，計算第i個采樣點(diǎn)的角速度模值||w_i||為：(其中，w_x,w_y,w_z分別為角速度在x軸、y軸以及z軸的分量)；設(shè)(m,n)為第s步的采樣區(qū)間，計算第s步的角速度能量均值為：計第s+1步內(nèi)的第i個采樣點(diǎn)角速度模值為||w_i||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內(nèi)角速度能量均值為：

具體實(shí)施時，本發(fā)明實(shí)施例所述第三計算單元還用于，對步行者的運(yùn)動狀態(tài)做判定：當(dāng)T＝1時，認(rèn)為當(dāng)前處于零速狀態(tài)，比例系數(shù)R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預(yù)設(shè)的經(jīng)驗(yàn)值。

本實(shí)施例提供一種自適應(yīng)零速點(diǎn)檢測的步行者軌跡推算系統(tǒng)，通過固定在步行者腳部的MEMS慣性傳感模塊進(jìn)行定位，其中，所述的MEMS慣性傳感模塊包括三軸加速度計、三軸陀螺儀以及三軸磁力計。

具體實(shí)施時，本發(fā)明對所述步行者軌跡推算的裝置也可包括慣性傳感模塊、初始狀態(tài)設(shè)置模塊、傳感數(shù)據(jù)補(bǔ)償模塊、判斷模塊、估計模塊、系統(tǒng)遞推模塊以及輸出模塊。

本發(fā)明涉及的方法，包括如下步驟：

將所述的MEMS慣性傳感模塊固定于步行者的腳部；

所述初始狀態(tài)設(shè)置模塊用于設(shè)置系統(tǒng)初始狀態(tài)；

所述傳感數(shù)據(jù)補(bǔ)償模塊按照既定的數(shù)據(jù)補(bǔ)償方法，對慣性傳感模塊的原始數(shù)據(jù)進(jìn)行補(bǔ)償，并利用基于橢球假設(shè)的最小二乘法對磁力計數(shù)據(jù)進(jìn)行三維校正；

所述判斷模塊用于對步行者的運(yùn)動狀態(tài)進(jìn)行判斷，以確定其是否處于零速狀態(tài)，首先進(jìn)行步伐頻率計算，具體步驟包括：1)計算加速度能量2)對加速度能量E_g進(jìn)行低通濾波，得到E′_g；2)對低通濾波后的E′_g進(jìn)行波峰與波谷判決，得到初步判決后的波峰與波谷序列；3)對初步判決后的波峰與波谷序列進(jìn)行二次判決，判決方法為：在初步判決后的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定此波谷為有效波谷；否則，視為無效波谷；4)對有效波谷間隔取倒數(shù)，得到步伐頻率v_f。得到步伐頻率后，進(jìn)行并基于角速度能量均值，并以前一步角速度能量均值與所述比例系數(shù)的乘積為閾值，進(jìn)行零速狀態(tài)檢測，具體步驟包括：1)計算第i個采樣點(diǎn)的角速度模值||w_i||為：2)設(shè)(m,n)為第s步的采樣區(qū)間，計算第s步的角速度能量均值為：3)計第s+1步內(nèi)的第i個采樣點(diǎn)角速度模值為||w_i||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內(nèi)角速度能量均值為：4)計算比例系數(shù)R_p：R_p＝k₀·v_f+α₀(其中，k₀與α₀為提前預(yù)設(shè)的經(jīng)驗(yàn)值)。最后，對運(yùn)動狀態(tài)做判定：當(dāng)T＝1時，認(rèn)為當(dāng)前處于零速狀態(tài)。

所述估計模塊基于擴(kuò)展卡爾曼濾波器對位置信息誤差進(jìn)行估計，具體方法如下：

建立卡爾曼濾波的系統(tǒng)狀態(tài)方程為選取系統(tǒng)狀態(tài)變量為其中：φ_E、φ_N、φ_U為俯仰角誤差、橫滾角誤差和方位角誤差；δV_E、δV_N、δV_U分別為東向、北向和天向速度誤差；δλ為緯度、經(jīng)度誤差；ε_X、ε_Y、ε_Z分別為陀螺東北天向的零漂；分別為加速度計東北天向的偏置；W(t)＝[w_gx w_gy w_gz w_ax w_ay w_az]^T高斯白噪聲，且E[W(t)]＝0，cov[W_k,W_j]＝E[W_kW_j^T]Q_kδ_kj(Q_k為系統(tǒng)噪聲方差陣)。

其中，

F_A為8×8階矩陣，其中非零元素如下：

F(4,2)＝-f_U

F(4,3)＝f_N

F(5,1)＝f_U

F(5,3)＝-f_E

F(6,1)＝-f_N

F(6,2)＝f_E

2)觀測方程為

y_k＝Hz_k+ν_k

其中，H為觀測矩陣，ν_k為噪聲矩陣。

當(dāng)運(yùn)用本發(fā)明提供的零速檢測方案檢測出零速狀態(tài)時，采用磁力計航向角、零速度和零角速度作為觀測量對系統(tǒng)狀態(tài)進(jìn)行修正，系統(tǒng)的觀測量為z_k＝[Δφ_U,δV_E,δV_N,δV_U,ε_X,ε_Y,ε_Z]，觀測矩陣為

當(dāng)檢測出非零速狀態(tài)時，僅利用磁力計航向角信息對系統(tǒng)狀態(tài)進(jìn)行修正，系統(tǒng)的觀測量為，觀測矩陣為

H＝[[0,0,1] 0_1×3 0_1×2 0_1×3 0_1×3]。

以上所述，僅為本發(fā)明較佳的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。因此，本發(fā)明的保護(hù)范圍應(yīng)該以權(quán)利要求書的保護(hù)范圍為準(zhǔn)。