[發明專利]一種基于磁干擾檢測的步行者軌跡推算方法及裝置

有效

• 申請號: 201611196323.8
• 申請日: 2016-12-22
• 公開（公告）號: CN107014388B
• 公開（公告）日: 2020-08-07
• 發明（設計）人: 趙坤鵬;于吉剛;伍凱
• 申請（專利權）人: 威海北洋電氣集團股份有限公司
• 主分類號: G01C21/34
• 分類號: G01C21/34;G01R33/00;G01R35/00
• 代理公司: 威海科星專利事務所 37202
• 代理人: 初姣姣

1.一種基于磁干擾檢測的步行者軌跡推算方法，其特征在于，包括：

獲取步行者的加速度、角速度，以及步行者所處的地磁矢量：

根據卡爾曼偏整估計結果對獲取的步行者的加速度、角速度進行偏差補償，對獲取的所述地磁矢量進行校正，判斷所述地磁矢量是否存在干擾，確定當前時刻的磁場干擾檢測結果：

根據加速度和角速度數據零速檢測方法對步行者的運動狀態進行判斷：

在地理坐標系下利用已經進行偏差補償的角速度進行姿態矩陣遞推：

去除重力加速度影響，進行速度遞推和位置遞推：

根據磁場干擾檢測結果和步行者的運動狀態確定步行者的軌跡：

對獲取的所述地磁矢量進行校正，具體包括：

建立磁力計測量的通用誤差模型

根據通用誤羞模型對獲取的所述地磁矢量進行校正：

其中，為磁力計的測量值，B為真實的磁場矢量，A為未對準誤差、增益誤差和軟磁干擾引起的誤差項，b為未對準、軟硬磁干擾和傳感器偏差引起的誤差項，ε_w為測量噪聲；

所述判斷所述地磁矢量是否存在干擾，具體包括：

條件1，當前時刻的磁場強度判定，判定模型為：

其中，||·||₂為向量的二范數，表示磁場強度大小，flag_條件1為磁干擾判定標志位，等于1時為有磁干擾，B_r為步行者活動區域的參考地磁場強度大小，根據活動區域的經緯度，由世界地磁場模型查詢獲得，參數α和參數β預設的經驗參數：

和/或，

條件2，當前時刻磁場強度的滑動方差判定，判定模型為：

其中，參數s為正整數，決定滑動窗口大小，由經驗給定，flag_條件2為條件2的磁干擾判定標志位，等于1時為有磁干擾，為滑動窗口各時刻的磁場強度的數學期望，ζ為滑動方差的判定閾值；

當條件1或條件2判定存在有磁干擾，則判定為有磁干擾；

所述根據磁場干擾檢測結果和步行者的運動狀態確定步行者的軌跡，具體包括：

當判定存在外部磁干擾且步行者為非靜止狀態，則直接通過系統狀態方程進行狀態傳播獲得下一時刻的軌跡狀態；

當判定存在外部磁干擾且步行者為靜止狀態，通過零速校正原理對步行者的軌跡進行修正，卡爾曼濾波的觀測量為零速狀態量，觀測方程為：

y_k＝Hz_k+v_k；

其中，H為觀測矩陣，v_k為噪聲矩陣；

觀測模型觀測量其中，Δv_k＝v_k-[0 0 0]^T，為k時刻經過偏差補償后的角速度，v_k為k時刻步行者速度，相應的觀測矩陣為

當判定無外部磁干擾且步行者為非靜止狀態，通過磁力計測量地磁場所得航向角信息對步行者的軌跡進行卡爾曼濾波修正，卡爾曼濾波的觀測量為k時刻姿態矩陣所得航向角與由地磁場所得航向角的差量，觀測模型的觀測量為：

z_k＝Δψ；

其中，ψ_k為由k時刻姿態矩陣解算的航向角，為由k時刻測量的地磁矢量解算的航向角，相應的觀測矩陣為：H＝[[0，0，1]0_1×3 0_1×3 0_1×3 01_×3]；

當判定無外部磁干擾且步行者為靜止狀態，通過磁力計測量地磁場所得航向角信息與零速校正原理對步行者的軌跡進行卡爾曼濾波修正，觀測模型的觀測量為：

相應的觀測矩陣為：