CN106123900B - 基于改进型互补滤波的室内行人导航磁航向解算方法 - Google Patents

Abstract

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

Description

基于改进型互补滤波的室内行人导航磁航向解算方法

技术领域

本发明涉及一种室内行人导航磁航向解算算法，尤其涉及一种基于改进型互补滤波的室内行人导航磁航向解算方法，属于室内行人导航技术领域。

背景技术

磁航向传感器凭借对地球磁场的测量，可以长期有效地解算出当前载体的航向角，被广泛的应用于各个领域。目前，室内行人导航较为流行，磁航向传感器则是其中的关键模块，但是当下的磁传感器均采取与人体固连模式(穿戴式)，大大约束了其适用条件，实用性不强。对于捷联惯性导航系统，姿态解算是其重要环节，但当前捷联惯性导航姿态解算算法(包括欧拉角法、方向余弦法、四元数法等等)存在三角函数计算问题和方程奇异现象等，复杂性较高。本专利提出一种改进型互补滤波器，从时域角度研究利用最优加权平均参数估计完成对姿态角的解算，同时研究剔除行人自带引入加速度误差；在行人稳态与非稳态辨识上，引入天向陀螺每秒绝对值积分进行模式判别。本方法适用于行人导航使用低精度的惯性传感器，在室内仍能够较为有效及较强适应性的磁航向解算算法，具有极高的工程应用和商业价值。

发明内容

本发明所要解决的技术问题是针对背景技术的不足提供了一种基于改进型互补滤波的室内行人导航磁航向解算方法。

本发明为解决上述技术问题采用以下技术方案

基于改进型互补滤波的室内行人导航磁航向解算方法，具体包含如下步骤，

步骤1，从时域角度获得互补滤波器参数k₁和k₂，具体计算如下：

k₁＝1-k₂

其中，是陀螺仪积分获得的角度的方差，是加速度计获得的角度的方差，k₁为陀螺仪的权重，k₂为加速度计的权重；

步骤2，根据步骤1获取的互补滤波器参数建立改进型互补滤波器算法模型，进而计算出姿态角，具体计算如下：

f_k1＝f_k-Δf_k-1

其中Δf_k-1为k-1时刻行人引入的加速度，f_k为k时刻实测加速度计数据，f_k1为修正误差后的加速度数据,T为采样周期，w为陀螺仪输出的角速度，θ_k为k时刻滤波后的姿态角，为陀螺仪在k-1时刻通过积分解算出的角度，加速度计在k时刻解算出来的角度；

步骤3，利用步骤2计算出的姿态角构建投影矩阵，将磁传感器数据投影到水平面：利用天向陀螺每秒绝对值积分进行模式判别，若行人步行、跑步时的模态为非稳态状态，则直接置航向角为零，不输出磁航向角；若行人步行、跑步时的模态为稳态状态，则补偿磁传感器误差，计算出精确地磁航向角，并更新Δf_k-1：

其中为k-1时刻坐标转换矩阵，为重力矢量。

作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案，在步骤1中，从时域角度利用最优加权平均估计分析获得互补滤波器参数k₁和k₂。

作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案，在步骤3中，行人手持载体在步行、跑步时的模态判别如下：

若θ_z<ε

则此时判为行人步行、跑步时的模态为稳态状态，否则为非稳态状态；

其中，w为z轴方向陀螺仪输出的角速度，T为采样周期，θ_z为z轴方向每秒绝对值积分计算出得角度，ε为通过统计学方法计算出来的参数值。

作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案，在步骤3中，利用最小二乘法标定好的二维椭圆参数补偿磁传感器误差。

作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案，在步骤3中，利用递推的思想，若行人步行、跑步时的模态为稳态状态，则实时更新人为引入的加速度，利用修正误差后的加速度计算姿态角。

作为本发明基于改进型互补滤波的室内行人导航磁航向解算方法的进一步优选方案，若行人步行、跑步时的模态为稳态状态，则补偿磁传感器误差，计算出磁航向角，磁航向角的具体计算如下：

其中，为磁航向角，Y为补偿后的横向磁传感器信息，X为补偿后的纵向磁传感器信息。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1.本发明采用低精度的消费级MEMS传感器芯片，无论室内、室外，航向角均能保证较高的精度，实用性较强；

2.本发明采用非固连模式安装，安装灵活性较高，使用环境不受限制，适用场景广阔；

3.本发明使用方式简易便捷，短时间内即能够保证较高精度的输出。

附图说明

图1为改进型互补滤波器原理图；

图2为基于改进型互补滤波的室内行人导航磁航向解算方法原理图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

如图2所示，基于改进型互补滤波的室内行人导航磁航向解算方法，具体包含如下步骤，

步骤1，从时域角度获得互补滤波器参数k₁和k₂，具体计算如下：

k₁＝1-k₂

其中，是陀螺仪积分获得的角度的方差，是加速度获得的角度的方差，k₁为陀螺仪的权重，k₂为加速度计的权重；

步骤2，根据步骤1获取的互补滤波器参数建立改进型互补滤波器算法模型，进而计算出姿态角，具体计算如下：

f_k1＝f_k-Δf_k-1

其中Δf_k-1为k-1时刻行人引入的加速度，f_k为k时刻实测加速度计数据，f_k1为修正误差后的加速度数据,T为采样周期，w为陀螺仪输出的角速度，θ_k为k时刻滤波后的姿态角，为陀螺仪在k-1时刻通过积分解算出的角度，加速度计在k时刻解算出来的角度；

步骤3，利用步骤2计算出的姿态角构建投影矩阵，将磁传感器数据投影到水平面：利用天向陀螺每秒绝对值积分进行模式判别，若行人步行、跑步时的模态为非稳态状态，则直接置航向角为零，不输出磁航向角；若行人步行、跑步时的模态为稳态状态，则补偿磁传感器误差，计算出精确地磁航向角，并更新Δf_k-1：

其中为k-1时刻坐标转换矩阵，为重力矢量。

具体步骤如下：

步骤一，从时域角度获得互补滤波器参数k₁和k₂：

假设陀螺仪与加速度计在水平静止状态下同时测量一段时间，由陀螺仪积分获得的角度与加速度计获得的角度，其方差分别为和

假设陀螺仪的权重为k₁，加速度计的权重为k₂，则对真值的估计为：

且

k₁+k₂＝1

则估计值的误差(方差)为

对其求偏导，可以求得最小值

此时，最优解参数为

通过最优参数可以求得此刻最优姿态角。

步骤二：建立改进型互补滤波器参数模型

利用加速度计采集的信息按下面公式所示可以解算出姿态角，陀螺仪通过积分也可以解算出姿态角。

其中，θ、γ分别为俯仰角与横滚角。

其中k₁，k₂为从时域角度利用最优加权平均估计分析获得的权重参数，则可得姿态角：

θ＝k₁*θ_w+k₂*θ_f

其中θ为滤波后姿态角，θ_w为陀螺仪积分解算出的角度，θ_f为加速度计解算出来的角度。

又因为：

则

其中T为采样周期，此即为姿态角互补滤波方程。

如图1所示，改进型互补滤波器即如下表述：由k-1时刻计算得到的横滚角、俯仰角、航向角构成投影矩阵将重力矢量投影到机体三轴，计算其与k-1时刻加速度计输出的差值，即为k-1时刻人为引入的三轴加速度。在k时刻，将k时刻加速度计三轴输出量与k-1时刻人为引入的的加速度的差值作为k时刻互补滤波器加速度计的输入量计算得到k时刻的横滚角与俯仰角，然后计算k时刻航向角，如此循环。由此，消除了行人自带加速度的干扰。即

f_k＝f_k-Δf_k-1

步骤三：构建行人运动模态辨识算法

行人手持载体在步行、跑步场景下，磁航向角解算主要分为两种模态，一种模态为行人行走、跑步时人体手臂摆动下的非稳态状态，一种模态为行人行走、跑步时的稳态查看状态。通过观测z轴陀螺每秒角速度绝对值积分对两种模态进行判别，可以有效辨别出磁航向角解算的必要性。在非稳态状态下，磁航向角计算无意义；稳态状态下，磁航向角实时输出。模式判别如下所示：

其中，w为z轴方向陀螺仪输出的角速度，T为采样周期。对陀螺仪输出的角速度绝对值进行积分可以避免人体摆动周期性相消。

当满足

θ_z<ε

此时判为手持稳态查看状态，否则为非稳态状态,其中ε为通过统计学方法计算出来的参数值,θ_z为z轴方向每秒绝对值积分计算出得角度。

步骤四：构建磁航向实时解算算法：

在步行、跑步前，在水平方向上静止，利用最优加权平均估计法设计出互补滤波器参数，然后实时采集陀螺仪加速度计数据，通过对每秒z轴陀螺绝对值进行积分，判断此时行人步行、跑步时的模态，如果判为非稳态状态，则直接置航向角为0，此时输出无意义，同时人为引入的额外加速度不进行更新；如果判为稳态状态，则每秒利用改进型互补滤波器获得此时姿态角，对磁传感器数据进行坐标投影，并利用最小二乘法标定好的二维椭圆参数，补偿磁传感器误差，实时输出磁航向角。

磁航向角可由下式获得：

其中，为磁航向角，Y为补偿后的横向磁传感器信息，X为补偿后的纵向磁传感器信息。

Claims (1)

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为补偿后的纵向磁传感器信息。