[发明专利]基于改进型互补滤波的室内行人导航磁航向解算方法

摘要

本发明公开了一种基于改进型互补滤波的室内行人导航磁航向解算方法，从时域角度设计互补滤波器参数，建立改进型互补滤波器算法模型，构建行人运动模态辨识算法模型，进而利用磁传感器二维椭圆标定实现室内行人磁航向角的实时解算，采用低精度的消费级MEMS传感器芯片，无论室内、室外，航向角均能保证较高的精度，实用性较强，采用非固连模式安装，安装灵活性较高，使用环境不受限制，适用场景广阔，使用方式简易便捷，短时间内即能够保证较高精度的输出。

主权项

1.基于改进型互补滤波的室内行人导航磁航向解算方法，其特征在于：具体包含如下步骤，步骤1，从时域角度获得互补滤波器参数k₁和k₂，具体计算如下：k₁＝1‑k₂其中，是陀螺仪积分获得的角度的方差，是加速度获得的角度的方差，k₁为陀螺仪的权重，k₂为加速度计的权重；步骤2，根据步骤1获取的互补滤波器参数建立改进型互补滤波器算法模型，进而计算出姿态角，具体计算如下：f_k1＝f_k‑Δf_k‑1θ＝k₁*θ_w+k₂*θ_f又因为：则其中θ为滤波后姿态角，Δf_k‑1为k‑1时刻行人引入的加速度，f_k为k时刻实测加速度计数据，f_k1为修正误差后的加速度数据,θ_w为陀螺仪积分解算出的角度，θ_f为加速度计解算出来的角度；T为采样周期，w为陀螺仪输出的角速度，θ_k为k时刻滤波后的姿态角，为陀螺仪在k‑1时刻通过积分解算出的角度，为加速度计在k时刻解算出来的角度；步骤3，利用步骤2计算出的姿态角构建投影矩阵，将磁传感器数据投影到水平面：利用天向陀螺每秒绝对值积分进行模式判别，若行人步行、跑步时的模态为非稳态状态，则直接置航向角为零，不输出磁航向角；若行人步行、跑步时的模态为稳态状态，则补偿磁传感器误差，计算出精确地磁航向角，并更新Δf_k‑1：其中为k‑1时刻坐标转换矩阵，为重力矢量；行人手持载体在步行、跑步场景下，磁航向角解算主要分为两种模态，一种模态为行人行走、跑步时人体手臂摆动下的非稳态状态，一种模态为行人行走、跑步时的稳态查看状态；通过观测z轴陀螺每秒角速度绝对值积分对两种模态进行判别，可以有效辨别出磁航向角解算的必要性；在非稳态状态下，磁航向角计算无意义；稳态状态下，磁航向角实时输出；模式判别如下所示：其中，w为z轴方向陀螺仪输出的角速度，T为采样周期，对陀螺仪输出的角速度绝对值进行积分可以避免人体摆动周期性相消；当满足θ_z＜ε此时判为手持稳态查看状态，否则为非稳态状态,其中ε为通过统计学方法计算出来的参数值,θ_z为z轴方向每秒绝对值积分计算出的角度；在步行、跑步前，在水平方向上静止，利用最优加权平均估计法设计出互补滤波器参数，然后实时采集陀螺仪加速度计数据，通过对每秒z轴陀螺绝对值进行积分，判断此时行人步行、跑步时的模态，如果判为非稳态状态，则直接置航向角为0，此时输出无意义，同时人为引入的额外加速度不进行更新；如果判为稳态状态，则每秒利用改进型互补滤波器获得此时姿态角，对磁传感器数据进行坐标投影，并利用最小二乘法标定好的二维椭圆参数，补偿磁传感器误差，实时输出磁航向角；磁航向角由下式获得：其中，为磁航向角，Y为补偿后的横向磁传感器信息，X为补偿后的纵向磁传感器信息。