對閾值平滑自適應調整的零速修正步行者軌跡的推算方法及裝置與流程

文檔序號：11100613閱讀：766來源：國知局

本發明涉及通信技術領域，尤其涉及一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法及裝置。

背景技術：

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

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

技術實現要素：

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

為解決上述問題，本發明主要是通過以下技術方案實現的：

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

根據前一步的采樣區間對補償后的角速度數據，計算對應的第一角速度能量均值，并根據當前步的滑動窗口對補償后的角速度數據，計算對應的第二角速度能量均值；

根據計算得到的步伐頻率計算步伐頻率對應的比例系數；

根據所述第一角速度能量均值與所述比例系數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定。

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

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

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

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

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

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

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

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

進一步地，根據前一步的采樣區間對補償后的角速度數據，計算對應的第一角速度能量均值，并據根據當前步的滑動窗口對補償后的角速度數，計算對應的第二角速度能量均值，具體包括：

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

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

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

進一步地，根據所述第一角速度能量均值與所述比例系數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定，具體包括：

對步行者的運動狀態做判定：當T＝1時，認為當前處于零速狀態，比例系數R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預設的經驗值。

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

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

第二計算單元，用于根據前一步的采樣區間對補償后的角速度數據，計算對應的第一角速度能量均值，并根據當前步的滑動窗口對補償后的角速度數據，計算對應的第二角速度能量均值，根據計算得到的步伐頻率計算步伐頻率對應的比例系數；

第三計算單元，用于根據所述第一角速度能量均值與所述比例系數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定。

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

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

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

進一步地，所述第三計算單元還用于，對步行者的運動狀態做判定：當T＝1時，認為當前處于零速狀態，比例系數R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預設的經驗值。

本發明有益效果如下：

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

本發明的其他特征和優點將在隨后的說明書中闡述，并且部分的從說明書中變得顯而易見，或者通過實施本發明而了解。本發明的目的和其他優點可通過在所寫的說明書、權利要求書、以及附圖中所特別指出的結構來實現和獲得。

附圖說明

圖1為本發明實施例的一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法的流程示意圖；

圖2為本發明實施例的另一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法的流程示意圖；

圖3為本發明實施例的再一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法的流程示意圖；

圖4為應用本發明實施例的所述方法得到的沿運動場運動的軌跡圖；

圖5為應用本發明實施例的所述方法得到的另一種沿運動場運動的軌跡圖；

圖6為本發明實施例的一種對閾值平滑自適應調整的零速修正步行者軌跡的推算裝置的結構示意圖。

具體實施方式

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

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

本發明實施例提供了一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法，參見圖1，該方法包括：

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

S102、根據前一步的采樣區間對補償后的角速度數據，計算對應的第一角速度能量均值，并根據當前步的滑動窗口對補償后的角速度數據，計算對應的第二角速度能量均值；

S103、根據計算得到的步伐頻率計算步伐頻率對應的比例系數；

S104、根據所述第一角速度能量均值與所述比例系數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定。

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

具體來說，本發明實施例是通過位對準誤差補償來對獲取步行者腳部的加速度數據、角速度數據以及磁場數據進行補償。當然，本領域的技術人員也可以通過其他的方法來對獲取的各個數據進行補償。

具體實施是，本發明所述對補償后的加速度數據進行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率，具體包括：

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

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

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

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

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

本發明實施例在初步判決后的波谷進行二次判決，確定有效波谷和無效波谷，具體包括：

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

本發明實施例所述根據前一步的采樣區間對補償后的角速度數據，計算對應的第一角速度能量均值，并據根據當前步的滑動窗口對補償后的角速度數，計算對應的第二角速度能量均值，具體包括：

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

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

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

本發明實施例所述根據所述第一角速度能量均值與所述比例系數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定，具體包括：

對步行者的運動狀態做判定：當T＝1時，認為當前處于零速狀態，比例系數R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預設的經驗值。

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

如圖3所示，本發明所提供的步行者零速狀態判定方法，首先需要進行步頻計算，主要步驟包括：

計算加速度能量

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

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

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

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

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

計算第i個采樣點的角速度模值||w_i||為：

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

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

計算比例系數R_p：R_p＝k₀·v_f+α₀(其中，k₀與α₀為提前預設的經驗值)；

對運動狀態做判定：

當T＝1時，認為當前處于零速狀態。

總體來說，本發明實施例所述的方法包括：

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

所述原始傳感數據需經過相關補償方法進行補償；

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

補償后的角速度數據需根據前一步的采樣區間，計算對應的角速度能量均值；

補償后的角速度數據需根據當前步的滑動窗口，計算對應的角速度能量均值；

依據所計算的步伐頻率，計算對應的比例系數；

依據前一步角速度能量均值與所述比例系數的乘積計算自適應閾值；

比較當前步滑動窗口內的角速度能量均值與所述自適應閾值，進行步行者的零速狀態判定。

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

總體來說，通過MEMS傳感器的應用，可以在室內、叢林等衛星定位系統受遮擋而失效的環境中實現定位；通過對傳感器原始數據進行補償、對補償后的加速度數據進行低通濾波以及二次判決、角速度能量均值計算、閾值自適應調整等過程，可以在步行者行走、跑步、快走、慢跑等各類行動條件下實現零速狀態的準確判決，從而有效提高定位精度。

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

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

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

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

具體實施時，本發明實施例所述第三計算單元還用于，對步行者的運動狀態做判定：當T＝1時，認為當前處于零速狀態，比例系數R_p＝k₀·v_f+α₀，其中，k₀與α₀為提前預設的經驗值。

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

具體實施時，本發明對所述步行者軌跡推算的裝置也可包括慣性傳感模塊、初始狀態設置模塊、傳感數據補償模塊、判斷模塊、估計模塊、系統遞推模塊以及輸出模塊。

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

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

所述初始狀態設置模塊用于設置系統初始狀態；

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

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

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

建立卡爾曼濾波的系統狀態方程為選取系統狀態變量為其中：φ_E、φ_N、φ_U為俯仰角誤差、橫滾角誤差和方位角誤差；δV_E、δV_N、δV_U分別為東向、北向和天向速度誤差；δλ為緯度、經度誤差；ε_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為系統噪聲方差陣)。