對閾值平滑自適應調整的零速修正步行者軌跡的推算方法及裝置與流程
2023-10-18 03:22:09 6
本發明涉及通信技術領域，尤其涉及一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法及裝置。
背景技術：
在室內、叢林等環境中，衛星定位系統由於受遮擋而無法實現定位，在上述場景中的定位應用中，基於慣性元件的步行者軌跡推算方案由於可以不受外界環境影響而實現全場景定位，因此越來越引起人們的廣泛關注。在基於慣性元件進行步行者軌跡推算的方案中，應用比較多的是零速修正算法。該算法利用步行者腳部著地時的零速區間獲得運動觀測量而對運動參數進行修正，抑制陀螺儀漂移而提高定位精度。由於零速修正算法對零速區間檢測的精度要求很高，因此零速檢測方法直接決定了系統的整體性能。
傳統的零速檢測方法基於補償後的加速度計與陀螺儀測量數據，利用聯合閾值的方式進行判斷，但由於所採用的閾值為固定的經驗值，無法適應人的各種形態運動而導致精度較差；基於Neyman-Pearson準則的方法採用極大似然估計方法對零速狀態進行估計，但實現過於複雜難以在裝置中獲得實際應用；目前應用的閾值自適應調整方法一般為對人的行走與跑步兩種狀態做判斷，並以固定比例設置閾值，但該比例仍為提前設定，因此對快走與慢跑等複雜步態存在誤判的可能，從而導致精度降低。
技術實現要素：
鑑於上述的分析，本發明旨在提供一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法及裝置，可完全或者至少部分地解決上述問題。
為解決上述問題，本發明主要是通過以下技術方案實現的：
本發明一方面提供了一種閾值自適應調整零速檢測步行者軌跡的推算方法，該方法包括：獲取步行者腳部的加速度數據、角速度數據以及磁場數據，對所述加速度數據、所述角速度數據以及所述磁場數據進行補償，並對補償後的加速度數據進行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；
根據前一步的採樣區間對補償後的角速度數據，計算對應的第一角速度能量均值，並根據當前步的滑動窗口對補償後的角速度數據，計算對應的第二角速度能量均值；
根據計算得到的步伐頻率計算步伐頻率對應的比例係數；
根據所述第一角速度能量均值與所述比例係數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定。
進一步地，所述對補償後的加速度數據進行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率，具體包括：
計算加速度能量其中，ax,ay,az分別為加速度在x軸、y軸以及z軸的分量；
對加速度能量Eg進行低通濾波，得到濾波後的加速度能量E′g；
對低通濾波後的加速度能量E′g進行波峰與波谷初步判決，得到初步判決後的波峰與波谷序列；
對初步判決後的波谷進行二次判決，確定有效波谷和無效波谷；
對有效波谷間隔取倒數得到步行者的步伐頻率vf。
進一步地，在初步判決後的波谷進行二次判決，確定有效波谷和無效波谷，具體包括：
在初步判決後的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。
進一步地，根據前一步的採樣區間對補償後的角速度數據，計算對應的第一角速度能量均值，並據根據當前步的滑動窗口對補償後的角速度數，計算對應的第二角速度能量均值，具體包括：
計算第i個採樣點的角速度模值||wi||為：其中，wx,wy,wz分別為角速度在x軸、y軸以及z軸的分量；
設(m,n)為第s步的採樣區間，計算第s步的角速度能量均值為：
計第s+1步內的第i個採樣點角速度模值為||wi||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內角速度能量均值為：
進一步地，根據所述第一角速度能量均值與所述比例係數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定，具體包括：
對步行者的運動狀態做判定：當T＝1時，認為當前處於零速狀態，比例係數Rp＝k0·vf+α0，其中，k0與α0為提前預設的經驗值。
本發明另一方面還提供了一種自適應閾值零速檢測步行者軌跡的推算裝置，該裝置包括：
第一計算單元，用於獲取步行者腳部的加速度數據、角速度數據以及磁場數據，對所述加速度數據、所述角速度數據以及所述磁場數據進行補償，並對補償後的加速度數據進行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；
第二計算單元，用於根據前一步的採樣區間對補償後的角速度數據，計算對應的第一角速度能量均值，並根據當前步的滑動窗口對補償後的角速度數據，計算對應的第二角速度能量均值，根據計算得到的步伐頻率計算步伐頻率對應的比例係數；
第三計算單元，用於根據所述第一角速度能量均值與所述比例係數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定。
進一步地，所述第一計算單元還用於，計算加速度能量其中，ax,ay,az分別為加速度在x軸、y軸以及z軸的分量；對加速度能量Eg進行低通濾波，得到濾波後的加速度能量E′g；對低通濾波後的加速度能量E′g進行波峰與波谷初步判決，得到初步判決後的波峰與波谷序列；對初步判決後的波谷進行二次判決，確定有效波谷和無效波谷；對有效波谷間隔取倒數得到步行者的步伐頻率vf。
進一步地，所述第一計算單元還用於，在初步判決後的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。
進一步地，所述第一計算單元還用於，計算第i個採樣點的角速度模值||wi||為：其中，wx,wy,wz分別為角速度在x軸、y軸以及z軸的分量；設(m,n)為第s步的採樣區間，計算第s步的角速度能量均值為：計第s+1步內的第i個採樣點角速度模值為||wi||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內角速度能量均值為：
進一步地，所述第三計算單元還用於，對步行者的運動狀態做判定：當T＝1時，認為當前處於零速狀態，比例係數Rp＝k0·vf+α0，其中，k0與α0為提前預設的經驗值。
本發明有益效果如下：
本發明通過對原始數據進行補償，獲取可靠的原始數據，通過對補償後的加速度數據進行低通濾波以及二次判決，進行步伐頻率的提取，然後將步伐頻率作為輸入量對角速度能量均值的閾值進行調整，從而達到閾值平滑自適應調整的目的。與目前通用的閾值自適應調整方法相比，可進行閾值平滑調整，從而適應步行者的各類步伐頻率的行動，克服了對快走與慢跑等情況誤判的問題，從而提高系統定位精度。
本發明的其他特徵和優點將在隨後的說明書中闡述，並且部分的從說明書中變得顯而易見，或者通過實施本發明而了解。本發明的目的和其他優點可通過在所寫的說明書、權利要求書、以及附圖中所特別指出的結構來實現和獲得。
附圖說明
圖1為本發明實施例的一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法的流程示意圖；
圖2為本發明實施例的另一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法的流程示意圖；
圖3為本發明實施例的再一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法的流程示意圖；
圖4為應用本發明實施例的所述方法得到的沿運動場運動的軌跡圖；
圖5為應用本發明實施例的所述方法得到的另一種沿運動場運動的軌跡圖；
圖6為本發明實施例的一種對閾值平滑自適應調整的零速修正步行者軌跡的推算裝置的結構示意圖。
具體實施方式
下面結合附圖來具體描述本發明的優選實施例，其中，附圖構成本申請一部分，並與本發明的實施例一起用於闡釋本發明的原理。為了清楚和簡化目的，當其可能使本發明的主題模糊不清時，將省略本文所描述的器件中已知功能和結構的詳細具體說明。
本發明提供了一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法，本發明通過對原始數據進行補償，獲取可靠的原始數據，通過對補償後的加速度數據進行低通濾波以及二次判決，進行步伐頻率的提取，然後將步伐頻率作為輸入量對角速度能量均值的閾值進行調整，從而達到閾值自適應調整的目的。與目前通用的閾值自適應調整方法相比，可進行閾值平滑調整，從而適應步行者的各類步伐頻率的行動，克服了對快走與慢跑等情況誤判的問題，從而提高系統定位精度。以下結合附圖以及幾個實施例，對本發明進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本發明，並不限定本發明。
本發明實施例提供了一種對閾值平滑自適應調整的零速修正步行者軌跡的推算方法，參見圖1，該方法包括：
S101、獲取步行者腳部的加速度數據、角速度數據以及磁場數據，對所述加速度數據、所述角速度數據以及所述磁場數據進行補償，並對補償後的加速度數據進行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；
S102、根據前一步的採樣區間對補償後的角速度數據，計算對應的第一角速度能量均值，並根據當前步的滑動窗口對補償後的角速度數據，計算對應的第二角速度能量均值；
S103、根據計算得到的步伐頻率計算步伐頻率對應的比例係數；
S104、根據所述第一角速度能量均值與所述比例係數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定。
也就是說，本發明通過對原始數據進行補償，獲取可靠的原始數據，通過對補償後的加速度數據進行低通濾波以及二次判決，進行步伐頻率的提取，然後將步伐頻率作為輸入量對角速度能量均值的閾值進行調整，從而達到閾值自適應調整的目的。與目前通用的閾值自適應調整方法相比，可進行閾值平滑調整，從而適應步行者的各類步伐頻率的行動，克服了對快走與慢跑等情況誤判的問題，從而提高系統定位精度。
具體來說，本發明實施例是通過位對準誤差補償來對獲取步行者腳部的加速度數據、角速度數據以及磁場數據進行補償。當然，本領域的技術人員也可以通過其他的方法來對獲取的各個數據進行補償。
具體實施是，本發明所述對補償後的加速度數據進行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率，具體包括：
計算加速度能量(其中，ax,ay,az分別為加速度在x軸、y軸以及z軸的分量)；
對加速度能量Eg進行低通濾波，得到濾波後的加速度能量E′g；
對低通濾波後的加速度能量E′g進行波峰與波谷初步判決，得到初步判決後的波峰與波谷序列；
對初步判決後的波谷進行二次判決，確定有效波谷和無效波谷；
對有效波谷間隔取倒數得到步行者的步伐頻率vf。
本發明實施例在初步判決後的波谷進行二次判決，確定有效波谷和無效波谷，具體包括：
在初步判決後的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。
本發明實施例所述根據前一步的採樣區間對補償後的角速度數據，計算對應的第一角速度能量均值，並據根據當前步的滑動窗口對補償後的角速度數，計算對應的第二角速度能量均值，具體包括：
計算第i個採樣點的角速度模值||wi||為：(其中，wx,wy,wz分別為角速度在x軸、y軸以及z軸的分量)；
設(m,n)為第s步的採樣區間，計算第s步的角速度能量均值為：
計第s+1步內的第i個採樣點角速度模值為||wi||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內角速度能量均值為：
本發明實施例所述根據所述第一角速度能量均值與所述比例係數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定，具體包括：
對步行者的運動狀態做判定：當T＝1時，認為當前處於零速狀態，比例係數Rp＝k0·vf+α0，其中，k0與α0為提前預設的經驗值。
如圖2所示，本發明提供一種自適應閾值零速檢測的步行者軌跡推算方法，該方法的主要步驟包括：在步行者腳部固定微機電系統(MEMS)慣性傳感元件，包括加速度計、陀螺儀以及磁力計；系統開始工作後，首先選定起始工作點(原點)，設定初始狀態；然後獲取由所述MEMS傳感器所提供的傳感數據，並進行補償；隨後，利用自適應閾值零速檢測方法，判定步行者是否處於零速狀態；若是，則基於擴展卡爾曼濾波對系統狀態進行校正，並進行系統狀態遞推；若否，則直接進行系統狀態遞推；循環執行步行者零速狀態判定、基於擴展卡爾曼濾波對系統狀態進行更新、系統狀態遞推三個步驟；最後，輸出定位結果。
如圖3所示，本發明所提供的步行者零速狀態判定方法，首先需要進行步頻計算，主要步驟包括：
計算加速度能量
對加速度能量Eg進行低通濾波，得到E′g；
對低通濾波後的E′g進行波峰與波谷判決，得到初步判決後的波峰與波谷序列；
對初步判決後的波峰與波谷序列進行二次判決，判決方法為：在初步判決後的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定此波谷為有效波谷；否則，視為無效波谷；
對有效波谷間隔取倒數，即可得到步頻vf。
計算得到步頻vf之後，以其為參考計算閾值比例係數；並基於角速度能量均值，以前一步角速度能量均值與所述比例係數的乘積為閾值，進行零速狀態檢測，具體步驟包括：
計算第i個採樣點的角速度模值||wi||為：
設(m,n)為第s步的採樣區間，計算第s步的角速度能量均值為：
計第s+1步內的第i個採樣點角速度模值為||wi||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內角速度能量均值為：
計算比例係數Rp：Rp＝k0·vf+α0(其中，k0與α0為提前預設的經驗值)；
對運動狀態做判定：
當T＝1時，認為當前處於零速狀態。
總體來說，本發明實施例所述的方法包括：
在步行者腳部固定MEMS傳感器，獲取腳部的加速度、角速度以及磁場等原始傳感數據；
所述原始傳感數據需經過相關補償方法進行補償；
補償後的加速度數據需進行能量計算、低通濾波與兩次判決過程，以確定步行者的步伐頻率；
補償後的角速度數據需根據前一步的採樣區間，計算對應的角速度能量均值；
補償後的角速度數據需根據當前步的滑動窗口，計算對應的角速度能量均值；
依據所計算的步伐頻率，計算對應的比例係數；
依據前一步角速度能量均值與所述比例係數的乘積計算自適應閾值；
比較當前步滑動窗口內的角速度能量均值與所述自適應閾值，進行步行者的零速狀態判定。
圖5和圖6為利用本發明提供的方法對步行者軌跡進行推算的結果圖，本發明採用了荷蘭Xsens公司的MTI 100模塊進行了兩組步行實驗。圖5對應的是實驗的場地為威海北洋電氣集團股份有限公司的籃球場，沿籃球場白線行走，總路程約505米，回到起始點，始終點誤差為0.87米，定位精度為0.17％。圖6對應的是實驗的場地為山東大學(威海)體育場，沿跑道行走，回到起始點，總路程>800米，始終點誤差為2.89米，定位精度為0.36％。
總體來說，通過MEMS傳感器的應用，可以在室內、叢林等衛星定位系統受遮擋而失效的環境中實現定位；通過對傳感器原始數據進行補償、對補償後的加速度數據進行低通濾波以及二次判決、角速度能量均值計算、閾值自適應調整等過程，可以在步行者行走、跑步、快走、慢跑等各類行動條件下實現零速狀態的準確判決，從而有效提高定位精度。
與圖1所示的方法相對應的，本發明實施例還提供了一種對閾值平滑自適應調整的零速修正步行者軌跡的推算裝置，參見圖6，該裝置包括：
第一計算單元，用於獲取步行者腳部的加速度數據、角速度數據以及磁場數據，對所述加速度數據、所述角速度數據以及所述磁場數據進行補償，並對補償後的加速度數據進行能量計算、低通濾波與兩次判決，確定步行者的步伐頻率；
第二計算單元，用於根據前一步的採樣區間對補償後的角速度數據，計算對應的第一角速度能量均值，並根據當前步的滑動窗口對補償後的角速度數據，計算對應的第二角速度能量均值，根據計算得到的步伐頻率計算步伐頻率對應的比例係數；
第三計算單元，用於根據所述第一角速度能量均值與所述比例係數的乘積計算得到自適應閾值，根據所述第二角速度能量均值與所述自適應閾值進行步行者的零速狀態判定。
具體實施時，本發明實施例所述第一計算單元還用於，計算加速度能量(其中，ax,ay,az分別為加速度在x軸、y軸以及z軸的分量)；對加速度能量Eg進行低通濾波，得到濾波後的加速度能量E′g；對低通濾波後的加速度能量E′g進行波峰與波谷初步判決，得到初步判決後的波峰與波谷序列；對初步判決後的波谷進行二次判決，確定有效波谷和無效波谷；對有效波谷間隔取倒數得到步行者的步伐頻率vf。
進一步地，本發明實施例所述第一計算單元還用於，在初步判決後的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定該波谷為有效波谷；否則，為無效波谷。
進一步地，本發明實施例所述第一計算單元還用於，計算第i個採樣點的角速度模值||wi||為：(其中，wx,wy,wz分別為角速度在x軸、y軸以及z軸的分量)；設(m,n)為第s步的採樣區間，計算第s步的角速度能量均值為：計第s+1步內的第i個採樣點角速度模值為||wi||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內角速度能量均值為：
具體實施時，本發明實施例所述第三計算單元還用於，對步行者的運動狀態做判定：當T＝1時，認為當前處於零速狀態，比例係數Rp＝k0·vf+α0，其中，k0與α0為提前預設的經驗值。
本實施例提供一種自適應零速點檢測的步行者軌跡推算系統，通過固定在步行者腳部的MEMS慣性傳感模塊進行定位，其中，所述的MEMS慣性傳感模塊包括三軸加速度計、三軸陀螺儀以及三軸磁力計。
具體實施時，本發明對所述步行者軌跡推算的裝置也可包括慣性傳感模塊、初始狀態設置模塊、傳感數據補償模塊、判斷模塊、估計模塊、系統遞推模塊以及輸出模塊。
本發明涉及的方法，包括如下步驟：
將所述的MEMS慣性傳感模塊固定於步行者的腳部；
所述初始狀態設置模塊用於設置系統初始狀態；
所述傳感數據補償模塊按照既定的數據補償方法，對慣性傳感模塊的原始數據進行補償，並利用基於橢球假設的最小二乘法對磁力計數據進行三維校正；
所述判斷模塊用於對步行者的運動狀態進行判斷，以確定其是否處於零速狀態，首先進行步伐頻率計算，具體步驟包括：1)計算加速度能量2)對加速度能量Eg進行低通濾波，得到E′g；2)對低通濾波後的E′g進行波峰與波谷判決，得到初步判決後的波峰與波谷序列；3)對初步判決後的波峰與波谷序列進行二次判決，判決方法為：在初步判決後的波谷周圍尋找初步判決波峰，若存在初步判決波峰，則判定此波谷為有效波谷；否則，視為無效波谷；4)對有效波谷間隔取倒數，得到步伐頻率vf。得到步伐頻率後，進行並基於角速度能量均值，並以前一步角速度能量均值與所述比例係數的乘積為閾值，進行零速狀態檢測，具體步驟包括：1)計算第i個採樣點的角速度模值||wi||為：2)設(m,n)為第s步的採樣區間，計算第s步的角速度能量均值為：3)計第s+1步內的第i個採樣點角速度模值為||wi||，陀螺儀噪聲方差為滑動窗口寬度為W，計算在滑動窗口(n,n+W-1)內角速度能量均值為：4)計算比例係數Rp：Rp＝k0·vf+α0(其中，k0與α0為提前預設的經驗值)。最後，對運動狀態做判定：當T＝1時，認為當前處於零速狀態。
所述估計模塊基於擴展卡爾曼濾波器對位置信息誤差進行估計，具體方法如下：
建立卡爾曼濾波的系統狀態方程為選取系統狀態變量為其中：φE、φN、φU為俯仰角誤差、橫滾角誤差和方位角誤差；δVE、δVN、δVU分別為東向、北向和天向速度誤差；δλ為緯度、經度誤差；εX、εY、εZ分別為陀螺東北天向的零漂；分別為加速度計東北天向的偏置；W(t)＝[wgx wgy wgz wax way waz]T高斯白噪聲，且E[W(t)]＝0，cov[Wk,Wj]＝E[WkWjT]Qkδkj(Qk為系統噪聲方差陣)。
其中，
FA為8×8階矩陣，其中非零元素如下：
F(4,2)＝-fU
F(4,3)＝fN
F(5,1)＝fU
F(5,3)＝-fE
F(6,1)＝-fN
F(6,2)＝fE
2)觀測方程為
yk＝Hzk+νk
其中，H為觀測矩陣，νk為噪聲矩陣。
當運用本發明提供的零速檢測方案檢測出零速狀態時，採用磁力計航向角、零速度和零角速度作為觀測量對系統狀態進行修正，系統的觀測量為zk＝[ΔφU,δVE,δVN,δVU,εX,εY,εZ]，觀測矩陣為
當檢測出非零速狀態時，僅利用磁力計航向角信息對系統狀態進行修正，系統的觀測量為，觀測矩陣為
H＝[[0,0,1] 01×3 01×2 01×3 01×3]。
以上所述，僅為本發明較佳的具體實施方式，但本發明的保護範圍並不局限於此，任何熟悉本技術領域的技術人員在本發明揭露的技術範圍內，可輕易想到的變化或替換，都應涵蓋在本發明的保護範圍之內。因此，本發明的保護範圍應該以權利要求書的保護範圍為準。