CN106996780A - 一种航向误差修正方法与装置及磁场检测方法与装置 - Google Patents

Abstract

本发明提供一种航向误差修正方法，包括如下步骤：(a)采集磁场测量数据、陀螺仪测量数据、加速度计测量数据；(b)计算检验统计量；(c)判定在人员行进中第s步到第s+m‑1步之间对应的时间区间内磁场是否处于稳态；(d)若磁场处于稳态，则将磁场解算航向作为观测量，利用卡尔曼滤波对航向角误差进行估计，利用航向角误差对人员的航向进行修正。本发明还提供一种航向误差修正装置，包括加速度计、陀螺仪、磁力计、信号采集单元、信号处理单元。本发明还提供一种磁场检测方法与装置。

Description

一种航向误差修正方法与装置及磁场检测方法与装置

技术领域

本发明涉及一种航向误差修正方法，尤其涉及一种陀螺仪航向漂移误差修正方法。本发明属于室内人员自主定位技术领域，具体说是一种利用稳态磁场修正陀螺仪航向漂移误差的方法与装置及磁场检测方法与装置。

背景技术

在基于微惯导的人员自主定位技术中，利用固定在人员足部微机电系统(Micro-electromechanical Systems,MEMS)MEMS的惯性测量单元(包括微型加速度计和微型陀螺仪)对人体运动进行测量，积分推演获得人员运动轨迹。该技术无需在定位场所预先安装任何设备，具有自主性、低功耗及便携性等优势，已成为卫星导航定位不可靠情况下重要的“补盲”手段。

受制造工艺水平的限制，微型陀螺仪在实际的应用中会产生漂移，导致陀螺仪解算航向误差随时间不断积累，导致定位误差不断增大。地磁场是一种稳定的地理信息，通常被用于辅助修正陀螺仪航向误差。但在室内场景中，由于地磁场受墙体、电子设备等的干扰比较严重，导致地磁场解算航向存在较大误差。

考虑到地磁场在某些地点或者时间段受到的干扰很小，受到干扰很小的磁场称为稳态磁场。在稳态磁场中，解算出的航向误差可以忽略，因此可用来修正陀螺仪航向误差。

从受干扰磁场中提取稳态磁场依赖于稳态磁场检测方法。传统的稳态磁场检测方法通过磁场幅度的变换率作为判断磁场受干扰程度的依据，当磁场幅度变化率小于某一阈值时，判定该时刻磁场处于稳态。若磁场在短时间内受到比较稳定的干扰，则该方法很容易造成虚警检测，即误将受干扰磁场判定为稳态磁场。若用其解算航向值修正陀螺仪航向，反而会引入额外的误差，极大地影响测量精度。在受到干扰的情况下，磁场在导航坐标系(东-北-天)三个轴向的受干扰程度往往是不相同的，即各轴向磁场变化比例不同，导致解算出来的航向存在较大的波动，而这样的磁场航向角无法有效的被用来修正陀螺仪航向角的长期漂移特性。

发明内容

本发明要解决的问题是针对现有技术中利用稳态磁场的解算航向值修正陀螺仪航向会引入额外的误差的问题，提供一种新的航向误差修正方法与装置及磁场检测方法与装置。

为解决上述技术问题，本发明采用的技术方案是：一种航向误差修正方法，包括如下步骤：

(a)人员行进中，以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据；

(b)利用下式计算检验统计量T

其中，α₁、α₂、α₃、α₄分别为第一权值、第二权值、第三权值、第四权值，且α₁、α₂、α₃、α₄的取值范围均为[0,1]，且α₁+α₂+α₃+α₄＝1，m为人员行进的步数间隔，m＞2，s为人员步数，B_i、θ_i、ξ_i分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差，分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差，B_i、θ_i、ξ_i、均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到；

(c)若T＜γ，则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态，并跳转到步骤(d)，否则跳转到步骤(a)，其中，γ为磁场稳态检测门限；

(d)在由步骤(c)得到的人员行进中第s步到第s+m-1步之间对应的时间区间内，将磁场解算航向作为观测量，利用卡尔曼滤波对航向角误差进行估计，利用航向角误差对人员的航向进行修正。

在受到干扰的情况下，磁场在导航坐标系(东-北-天)三个轴向的受干扰程度往往是不相同的，即各轴向磁场变化比例不同，导致解算出来的航向存在较大的波动，而这样的磁场航向角无法有效的被用来修正陀螺仪航向角的长期漂移特性。本发明中，利用构造检验统计量T检验磁场是否为稳态，即判断磁场解算航向的可信度，若磁场为稳态，则利用磁场航向角修正陀螺仪航向角的漂移。

若α₄＝0，则利用磁场解算航向的相对变化、磁场幅度的相对变化、陀螺仪短时航向的相对变化构造检验统计量，则对直线行走的效果较好；若α₁、α₂、α₃、α₄均不为0，则由于加入了磁场解算航向与陀螺仪解算航向的偏差值的变化量Δξ_s，因此可以在连续转弯环境下确定磁场角度连续变化以及陀螺连续角度变化的之间的差异，如果磁场解算航向与陀螺仪解算航向两者之间的相对角度变化偏差是稳定的，则对于连续转弯的行走线路的磁场的稳态策略仍然可以对陀螺起到修正作用，因此对曲线行走线路甚至是连续曲线的行走线路的修正较好。

上述技术方案中，所述步骤(b)中，B_i、θ_i分别为从人员行进中第i步的静止时间区间中任取的时刻在导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向，其中，第i步的静止时间区间为人员行进中第i步对应的人员足部与地接触的时间区间。

上述技术方案中，所述步骤(b)中，第i步的静止时间区间通过如下步骤计算：

(b1)计算人员足部在k时刻的加速度模值其中(a_x(k),a_y(k),a_z(k))为k时刻人员足部在导航坐标系下的加速度；

(b2)利用下式计算偏差σ_k和方差λ_k，

其中M₁的取值范围为3≤M₁≤10，M₂的取值范围为3≤M₂≤10，且M₁、M₂均为整数；

(b3)设置偏差门限η_BIAS、方差门限η_v，若式子λ_k≥η_v和σ_k≥η_BIAS中至少有一项成立，则判断人员足部在k时刻为运动状态，反之，则判断人员足部在k时刻为静止状态；

(b4)若检测到人员足部在时间区间[k_1i,k_2i]内的每个时刻均为静止状态，而检测到人员足部在k_1i-1时刻、k_2i+1时刻均为运动状态，则认为时间区间[k_1i,k_2i]为人员行进中的第i步对应的静止时间区间。

上述技术方案中，所述步骤(b)中，θ_i通过姿态四元数计算得到，B_i、ξ_i通过下式计算得到

其中，(B_i,x,B_i,y,B_i,z)为人员行进中的第i步时人员足部在导航坐标系下的磁场强度，D为地磁偏角。

上述技术方案中，所述的步骤(b)中，m＝5。

上述技术方案中，所述的步骤(c)中，磁场稳态检测门限γ通过接收器操作特性曲线确定。

上述技术方案中，所述步骤(d)中，利用航向角误差对人员行进中第s+m-1步对应的航向进行修正或对第s步到第s+m-1步中对应的各个航向进行修正。

本发明还提供一种实现上述航向误差方法的航向误差修正装置，包括设置于人员足部且具有相同载体坐标系的加速度计、陀螺仪、磁力计，还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元，还包括与所述信号采集单元连接的信号处理单元。

本发明还提供一种磁场检测方法，在人员足部设置具有相同载体坐标系的加速度计、陀螺仪、磁力计，所述磁场检测方法包括如下步骤：

(a)人员行进中，以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据；

(b)利用下式计算检验统计量T

其中，α₁、α₂、α₃、α₄分别为第一权值、第二权值、第三权值、第四权值，且α₁、α₂、α₃、α₄的取值范围均为[0,1]，且α₁+α₂+α₃+α₄＝1，m为人员行进的步数间隔，m＞2，s为人员步数，B_i、θ_i、ξ_i分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差，分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差，B_i、θ_i、ξ_i、均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到；

(c)若T＜γ，则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态。

本发明还提供一种磁场检测装置，包括设置于人员足部且具有相同载体坐标系的加速度计、陀螺仪、磁力计，还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元，还包括与所述信号采集单元连接的信号处理单元。

本发明具有的优点和积极效果是：本发明可以极大改善利用磁力计航向角修正惯导航向误差的精度，降低室内外环境因素对磁力计干扰的影响；新的检测策略在保证有效磁场向角检测，降低检测误检率的同时，通过融合修正抑制了惯导航向的累积误差。本发明直线行走的效果较好，而且对曲线行走线路甚至是连续曲线的行走线路的修正也有较好的效果。相比较传统检测方法，本发明可大幅降低稳态磁场的检测虚警率，提高检测精度。采用本发明的方法，导航定位轨迹的航向误差明显降低。

附图说明

图1为本发明的航向误差修正方法的处理流程示意图；

图2为本发明的航向误差修正装置的结构示意图；

图3为本发明的磁场检测方法处理流程示意图；

图4为检测统计量曲线，图中横坐标为步数，纵坐标为检测统计量，图中采用的参数为：M₁、M₂取值为5，σ_θ＝0.56rad/s,

图5为定位效果对比图，图中外围实线表示墙体，实心方形曲线为采用传统检测方法解算轨迹，空心圆形曲线为采用本发明的航向误差修正方法的解算轨迹；

图中，1、人员足部，2、加速度计，3、陀螺仪，4、磁力计，5、信号采集单元，6、信号处理单元，7、数据传输单元。

具体实施方式

下面结合附图对本发明作进一步说明。

本发明中，若判定人员行进过程中某一步的磁场为稳态磁场，则对相应的静止时间区间内的各个时刻的磁场数据求出的航向角进行修正。

稳态磁场是对地磁场稳定性的判断性描述，由于在复杂的室内或者室外环境下地磁场会受到不同程度的污染干扰，从而导致依据其计算出来的航向角存在干扰，并间接的影响后续陀螺航向角修正。稳态磁场中，在固定时间间隔内，磁传感器获取的多个当地地磁场总强度采样值保持稳定，或者说磁传感器获取到的磁场强度随时间的变化率约等于零，磁传感器可采用磁力计。

本发明的基本思路是：在受到干扰的情况下，磁场在导航坐标系(东-北-天)三个轴向的受干扰程度往往是不相同的，即各轴向磁场变化比例不同，导致解算出来的航向存在较大的波动，而这样的磁场航向角无法有效的被用来修正陀螺仪航向角的长期漂移特性。因此本发明利用磁场解算航向的相对变化、磁场幅度的相对变化以及陀螺仪短时航向的相对变化共同作为磁场解算航向可信度的一个判断，即发明中提出的稳态磁场检测。

如图1—图5所示，将足部模块安装在鞋尖处，其中足部模块融合了加速度计2、陀螺仪3、磁力计4，且加速度计2、陀螺仪3、磁力计4具有相同方向的载体坐标系和导航坐标系。测量坐标系即为载体坐标系。其中载体指人员足部1。实验中，人员沿着某办公楼一楼内墙行走一圈后回到出发点，路径总长约626米。本专利实验所用模块采样率为400Hz。磁场测量数据即为磁力计4测量得到的数据。磁场解算航向即为由磁场测量数据解算得到的航向。利用其它磁场测量装置测量得到磁场测量数据也可实现本发明的方法。

本发明的航向误差修正方法包括以下处理步骤：

步骤(a)：数据采集

人员行进中，设置采样周期，以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据，采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的固定周期可根据实际需要或由各个产品的实际参数确定，例如可设置为0.001-5秒的任意数值或其他数值，k为采样时刻，k＝0,1,2,3……；s＝1,2,3……；三轴加速度计在k时刻测得人员足部1在载体坐标系下的加速度计测量数据为(a_x(k),a_y(k),a_z(k))，磁力计4在k时刻测得人员足部1部在载体坐标系下的磁场强度为(h_x(k),h_y(k),h_z(k))，k时刻人员足部1在导航坐标系下的磁场强度为(B_x(k),B_y(k),B_z(k))；

步骤(b)：参数计算

利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算人员行进中第i步在导航坐标系下的磁场幅度B_i、磁场解算航向陀螺仪解算航向θ_i以及人员行进中第i步的导航坐标系下的磁场幅度B_i、磁场解算航向陀螺仪解算航向θ_i、磁场解算航向与陀螺仪解算航向的偏差ξ_i以及磁场幅度的噪声方差磁场解算航向的噪声方差陀螺仪解算航向的噪声方差磁场解算航向与陀螺仪解算航向的偏差的方差其中s为人员步数，s＝1,2,3……，i∈[s,s+m-1]。本实施例中，利用步态静止检测方法确定人员各个步子的静止时间区间，然后利用第i步的静止时间区间中任取的时刻在导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向得到B_i、θ_i、ξ_i。实际过程中，也可不计算静止时间区间，通过其他类似方法得到B_i、θ_i、ξ_i、等各个参数。本实施例的静止检测方法中，将固定长度时间窗口内的加速计模值和模值方差作为检测依据，且时间窗口随着时间变化滑动。第i步的静止时间区间为人员行进中第i步对应的足部与地接触的时间区间。

(b1)计算人员足部1在k时刻的加速度模值

(b2)利用下式计算偏差σ_k和方差λ_k，将方差λ_k取为短时间计算区间(k-M₁,k+M₂)，从而避免了判断延迟，

其中M₁的取值范围为3≤M₁≤10，M₂的取值范围为3≤M₂≤10，且M₁、M₂均为整数，本实施例中，M₁、M₂的优选值为5；

(b3)设置偏差门限η_BIAS、方差门限η_v，若式子λ_k≥η_v和σ_k≥η_BIAS中至少有一项成立，则判断人员足部1在k时刻为运动状态，反之，则判断人员足部1在k时刻为静止状态。本步骤中，考虑了人员行走的规律，判断偏差和方差须同时满足条件，才能判断人员足部为静止状态。偏差门限η_BIAS和方差门限η_v根据加速度计测量数据的统计特性来确定，例如采集加速度计测量数据，通过试验确定偏差门限η_BIAS和方差门限η_v的取值。此处的加速度统计特性是依据实际应用是所使用的加速度传感器的型号不同，它的硬件性能不同，以及器件安装使用的不同，而确定加速计统计特性，本领域的人员可以理解；所述偏差门限η_BIAS和方差门限η_v通过加速计测量数据的统计特性计算得到，本实施例中，方差门限η_v可设置为0.05(m/s²)²到0.8(m/s²)²之间的数值，偏差门限η_BIAS可设置为小于0.15m/s²的数值；

(b4)若检测到人员足部1在时间区间[k_1i,k_2i]内的每个时刻均为静止状态，而检测到人员足部1在k_1i-1时刻、k_2i+1时刻均为运动状态，则认为时间区间[k_1i,k_2i]为人员行进中的第i步对应的静止时间区间，同理即可得到人员行进中每一步对应的静止时间区间，第i步的静止时间区间即为人员行进中第i步对应的足部与地接触的时间区间，其中k_1i＜k_2i；

(b5)将静止时间区间中的磁场数据由载体坐标系转换到导航坐标系

设k时刻磁力计的载体坐标系与导航坐标系之间的欧拉角为(r(k),p(k),y(k))，其中r(k)为横滚角，p(k)为俯仰角，y(k)为航向角。在足部着地时的静止时间区间内，r(k)和p(k)可由人员足部1的加速度数据计算得出，即：

其中，g为重力加速度。

由于在k时刻测得的人员足部1的磁场强度(h_x(k),h_y(k),h_z(k))是在载体坐标系中的数据，而通常定位是在导航坐标系下进行，因此，需在进行解算航向前将载体坐标系下的磁场数据(h_x(k),h_y(k),h_z(k))转换为导航坐标系下的磁场强度(B_x(k),B_y(k),B_z(k))，转换通过下式进行：

(b6)计算静止时间区间内磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差

磁场幅度的噪声方差磁场解算航向的噪声方差陀螺仪解算航向的噪声方差磁场解算航向与陀螺仪解算航向的偏差可通过长时间进行实验，利用统计方法计算得出，可将足部模块静止放置，连续采集两小时的磁场幅度的噪声数据、磁场解算航向的噪声数据及陀螺仪解算航向的噪声数据，经过计算得到本实施例中的采集时间不对本发明作出限制，数据的采集时间根据实际需要确定。根据磁场解算航向的噪声数据及陀螺仪解算航向的噪声数据计算，因为这个变量是与这两个变量相关的。

在人员足部1行进中某一步的静止时间区间内，人员足部1的磁场强度基本保持稳定，即磁场强度基本不发生变化，因此，计算该步磁场幅度时，可取静止时间区间中的任意采样点或根据一小段采样点的平均数据得到。若在人员行进中第i步的静止时间区间[k_1i,k_2i]内选取采样点，则可任取时刻k_3i，其中k_3i∈[k_1i,k_2i]。若在人员行进中第i步的静止时间区间[k_1i,k_2i]内选取一小段采样点的平均值，则可选取时间区间[k_11i,k_12i]并将各个采样点求平均值，其中k_1i、k_12i均为采样时刻，且k_11i≥k_1i、k_12i≤k_2i。

当下一步(第i+1步)静止时间区间到来时，用相同方法计算第i+1步的陀螺仪航向、磁场幅度及磁场解算航行，并分别计算相邻两步(第i和第i+1步之间)的变化量。

人员行进过程中，每一步都对应静止时间区间和运动时间区间，且两步分别对应的静止时间区间之间的时间区间即为人员的运动时间区间。在本发明中，人员开始行进，即利用上述步骤(b1)-(b4)检测各个静止时间区间，由此可以将人员行进中第i步与第i步对应的静止时间区间相对应。

人员行进中第i步的导航坐标系下的磁场幅度B_i由下式计算：

同理计算得到第i+1步的磁场幅度B_i+1，则相邻两步(第i和第i+1步之间)的磁场幅度变化量ΔB_i为：

ΔB_i＝|B_i+1-B_i|，

初始值ΔB₁＝0。人员行进中第i步的导航坐标系下的磁场解算航向由下式计算：

其中D为地磁偏角，可通过查阅资料获得。

同理计算得到第i+1步的磁场解算航向则相邻两步(第i+1和第i步之间)的磁场解算航向变化量为：

人员行进中，利用陀螺仪测量数据，通过姿态四元数解算获取陀螺航向角，人员行进中姿态四元数的计算本领域的人员是可以理解的。人员行进中第i步的陀螺仪解算航向表示为θ_i。

同理计算得到第i+1步的陀螺仪解算航向θ_i+1，则相邻两步(第i+1和第i步之间)的陀螺仪解算航向变化量Δθ_i为：

Δθ_i＝|θ_i+1-θ_i|

人员行进中第i步的磁场解算航向与陀螺仪解算航向的偏差表示为

同理计算得到第i+1步的磁场解算航向与陀螺仪解算航向的偏差ξ_i+1，则相邻两步(第i+1和第i步之间)的磁场解算航向与陀螺仪解算航向的偏差变化量Δξ_i为：

Δξ_i＝|ξ_i+1-ξ_i|。

(b7)计算检验统计量

结合似然比检测，令检测统计量T为

为提高鲁棒性，在上式的磁场稳态检测中计算相邻m步的磁场变化量，其中m的取值范围为m＞2，优选取值范围为2＜m≤7，m优选取值为5。s为人员步数。

α₁、α₂、α₃、α₄分别为第一权值、第二权值、第三权值、第四权值，且α₁、α₂、α₃、α₄的取值范围均为[0,1]，且α₁+α₂+α₃+α₄＝1，α₁、α₂、α₃、α₄为分别与磁场幅度变化量、磁场解算航向变化量、陀螺仪解算航向变化量、磁场解算航向与陀螺仪解算航向的偏差变化量对应的权值。

α₁、α₂、α₃、α₄根据实际试验或实际工程需要进行计算，或可根据实际效果进行调整，或可利用现有的数学方法，对α₁、α₂、α₃、α₄的最优值进行计算。

若α₄＝0，则检验统计量T仅由磁场幅度变化量、磁场解算航向变化量、陀螺仪解算航向变化量决定。

若α₁、α₂、α₃、α₄均不为0，则由于加入了磁场解算航向与陀螺仪解算航向的偏差值的变化量Δξ_s，则可以在连续转弯环境下确定磁场角度连续变化以及陀螺连续角度变化的之间的差异，如果磁场解算航向与陀螺仪解算航向两者之间的相对角度变化偏差是稳定的，则对于连续转弯的环境下磁场的稳态策略依旧可以对陀螺起到修正作用，因此对曲线行走线路甚至是连续曲线的行走线路的修正较好。

检测统计量曲线如图2所示。图中横坐标为步数，纵坐标为检测统计量，其中M₁、M₂取值为5，σ_θ＝0.56rad/s,γ＝554mGs。

步骤(c)：磁场稳态检测

磁场稳态检测门限γ可通过接收器操作特性曲线(Receiver OperatingCharacteristic curve,或称ROC曲线)确定，接收器操作特性曲线通过改变磁场稳态检测门限，计算该磁场稳态检测门限下样本数据的检测概率和虚警概率，以虚警概率为横坐标，检测概率为纵坐标绘制得出的曲线。磁场稳态检测门限γ的值不恒定。γ的单位为毫高斯(mGs)。本实施例中，γ的优选取值为850mGs，此值根据所在实验环境，考虑了周围磁场干扰的情况下确定的磁场稳态检测门限γ。在实际专利应用中，可以根据需要改变γ值，但γ的变动幅度应参考标准地球磁场标准值。

将T与磁场稳态检测门限比较γ，若T＜γ，则判定磁场处于稳态。若磁场处于稳态，则跳转到步骤(d)，否则跳转到步骤(a)，继续进行数据采集。

步骤(d)：利用磁场解算航向对陀螺仪航向误差进行修正

若在步骤(b)中检测得到磁场处于稳态，则将步骤(b)中的磁场解算航向作为零速更新辅助的卡尔曼滤波的观测量对陀螺仪航向误差进行修正。状态转移矩阵Φ_k、过程噪声w_k-1、观测矩阵H、观测噪声n_k、速度误差v_k的计算为本领域公知常识。在本专利中，使用现有的卡尔曼滤波方法或卡尔曼滤波方法的变形也可以实现。通过本专利的卡尔曼滤波方法，可以求解得到陀螺和加速度误差，并在后续的计算加以修正。本专利中，在满足磁场稳态条件的人员行进中第s步到第s+m-1步之间的静止时间区间(足部与地接触的时间区间)内进行卡尔曼滤波，卡尔曼滤波是一直进行的过程。

令卡尔曼滤波的状态向量δx_k|k为：

其中δφ_k为人员足部1的姿态角误差，人员足部1的姿态角误差包括横滚角误差、俯仰角误差、航向角误差，δω_k为人员足部1的三轴角速度误差，δr_k为人员足部1的三维位置误差，δv_k为人员足部1的三轴速度误差，δa_k为人员足部1的三轴加速度误差。卡尔曼滤波的状态方程为：

δx_k|k-1＝Φ_kδx_k-1|k-1+w_k-1，

其中δx_k|k-1为一步预测误差状态向量，δx_k-1|k-1为上一步滤波后状态向量，Φ_k为状态转移矩阵，w_k-1为过程噪声，噪声协方差矩阵

卡尔曼滤波量测方程为：

z_k＝Hδx_k|k+n_k，

其中z_k为预测误差观测，H为观测矩阵，n_k为观测噪声，其协方差矩阵当磁场稳态检测结果为稳态时，磁场解算航向可作为参考校准陀螺仪解算误差。此时真实误差观测为：

m_k＝[Δψ_k,v_k]，

其中v_k为速度误差，Δψ_k为陀螺仪解算航向误差。其中 为陀螺仪解算航向，θ_k为磁场解算航向值。

卡尔曼滤波的估计误差协方差矩阵P_k|k-1可表示为：

令G_k为卡尔曼滤波增益，则有：

G_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1；

估计误差状态量可通过下式更新：

δx_k|k＝δx_k|k-1+G_k·[m_k-Hδx_k|k-1]；

其中，δx_k|k包含航向角误差估计，因此获得δx_k|k后可对航向角进行校正。

本发明还提供一种实现上述航向误差修正方法的航向误差修正装置，航向误差修正装置包括设置于人员足部1且具有相同载体坐标系的加速度计2、陀螺仪3、磁力计4，还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元5，还包括与所述信号采集单元5连接的信号处理单元6。可利用数据传输单元7将信号处理单元6处理得到的数据进行传输。在信号处理单元6中执行本发明中的航向误差修正方法。信号处理单元6可由单片机、DSP、FPGA等构成。

本发明中，利用稳态磁场航向角对惯导航向角进行修正时，可仅修正人员行进中第s+m-1步对应的惯导航向角，也可修正第s步到第s+m-1步中对应的人员的航向。例如，若人员行进中第4-7步时，磁场为稳态，且第3步为非稳态，则可仅修正第7步对应的人员的航向，即可满足要求。如果需要达到更高的定位精度，也可对第4、5、6、7步对应的人员的各个航向均进行修正，也可仅修正第6、7步或第5、6、7步对应的人员的各个航向。实际工作过程中，根据运算效率或实际需要决定。本发明提出的航向误差方法可用于室内定位，也可应用于其他类似的磁场易受干扰的环境的定位中。

图4为实施例的检测统计量的曲线图。从图4中可以看出,检测统计量的波动很大，从侧面反映出室内磁场的干扰比较严重。另外，由图中结果可知，磁场受干扰较小的情况同样存在，可用来修正陀螺仪航向误差。

图5为实施例的定位效果对比图。图中，矩形方框分别代表建筑物中的各个房间结构。从图5中可以看出，采用了传统检测方法解算轨迹航向发散较严重，定位已出现偏差，与真实运动区域完全偏离，定位结果已经不可信，从图中可以看出纯惯导运动轨迹相对实际运动建筑物存在较大的误差，解算轨迹已穿透墙体，定位误差达到4m，航向误差超过11°。采用本发明的航向误差修正方法解算轨迹航向误差明显减小，定位轨迹和建筑物走向重合度明显提高，解算轨迹与实际行走路径基本吻合，达到终点时定位误差不超过0.5m，航向误差仅为2°，且定位的轨迹未出现穿过墙体的情况。

本发明还提供一种磁场检测方法，在人员足部1设置具有相同载体坐标系的加速度计、陀螺仪、磁力计，所述磁场检测方法包括如下步骤：

(a)人员行进中，以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据；

(b)利用下式计算检验统计量T

其中，α₁、α₂、α₃、α₄分别为第一权值、第二权值、第三权值、第四权值，且α₁、α₂、α₃、α₄的取值范围均为[0,1]，且α₁+α₂+α₃+α₄＝1，m为人员行进的步数间隔，m＞2，s为人员步数，B_i、θ_i、ξ_i分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差，分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差，B_i、θ_i、ξ_i、均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到；

(c)若T＜γ，则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态。

本发明还提供一种实现上述磁场检测方法的磁场检测装置，包括设置于人员足部1且具有相同载体坐标系的加速度计2、陀螺仪3、磁力计4，还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元5，还包括与所述信号采集单元5连接的信号处理单元6。

以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。

Claims (10)

1.一种航向误差修正方法，其特征在于：包括如下步骤：

(a)人员行进中，以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据；

(b)利用下式计算检验统计量T

其中，α₁、α₂、α₃、α₄分别为第一权值、第二权值、第三权值、第四权值，且α₁、α₂、α₃、α₄的取值范围均为[0,1]，且α₁+α₂+α₃+α₄＝1，m为人员行进的步数间隔，m＞2，s为人员步数，B_i、θ_i、ξ_i分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差，分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差，B_i、θ_i、ξ_i、均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到；

(c)若T＜γ，则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态，并跳转到步骤(d)，否则跳转到步骤(a)，其中，γ为磁场稳态检测门限；

(d)在由步骤(c)得到的人员行进中第s步到第s+m-1步之间对应的时间区间内，将磁场解算航向作为观测量，利用卡尔曼滤波对航向角误差进行估计，利用航向角误差对人员的航向进行修正。

2.根据权利要求1所述的航向误差修正方法，其特征在于：所述步骤(b)中，B_i、θ_i分别为从人员行进中第i步的静止时间区间中任取的时刻在导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向，其中，第i步的静止时间区间为人员行进中第i步对应的人员足部(1)与地接触的时间区间。

3.根据权利要求2所述的航向误差修正方法，其特征在于：所述步骤(b)中，第i步的静止时间区间通过如下步骤计算：

(b1)计算人员足部(1)在k时刻的加速度模值其中(a_x(k),a_y(k),a_z(k))为k时刻人员足部(1)在导航坐标系下的加速度；

(b2)利用下式计算偏差σ_k和方差λ_k，

$\left\{\begin{array}{c}{\mathrm{\σ}}_k=\left|\right|a_k|-|g\left|\right|\\ {\mathrm{\λ}}_k=\mathrm{var}iance\left(\right|a_{k-M_1}|,...,|a_k|,...,|a_{k+M_2}\left|\right)\end{array}\right.,$

其中M₁的取值范围为3≤M₁≤10，M₂的取值范围为3≤M₂≤10，且M₁、M₂均为整数；

(b3)设置偏差门限η_BIAS、方差门限η_v，若式子λ_k≥η_v和σ_k≥η_BIAS中至少有一项成立，则判断人员足部(1)在k时刻为运动状态，反之，则判断人员足部(1)在k时刻为静止状态；

(b4)若检测到人员足部(1)在时间区间[k_1i,k_2i]内的每个时刻均为静止状态，而检测到人员足部(1)在k_1i-1时刻、k_2i+1时刻均为运动状态，则认为时间区间[k_1i,k_2i]为人员行进中的第i步对应的静止时间区间。

4.根据权利要求1-3中任一项所述的航向误差修正方法，其特征在于：所述步骤(b)中，B_i、ξ_i通过下式计算得到

其中，(B_i,x,B_i,y,B_i,z)为人员行进中的第i步时人员足部(1)在导航坐标系下的磁场强度，D为地磁偏角。

5.根据权利要求1-3中任一项所述的航向误差修正方法，其特征在于：所述的步骤(b)中，m＝5。

6.根据权利要求1-3中任一项所述的航向误差修正方法，其特征在于：所述的步骤(c)中，磁场稳态检测门限γ通过接收器操作特性曲线确定。

7.根据权利要求1-3中任一项所述的航向误差修正方法，其特征在于：所述步骤(d)中，利用航向角误差对人员行进中第s+m-1步对应的航向进行修正或对第s步到第s+m-1步对应的各个航向进行修正。

8.一种实现权利要求1-7中任一项所述的航向误差修正方法的航向误差修正装置，其特征在于：包括设置于人员足部(1)且具有相同载体坐标系的加速度计(2)、陀螺仪(3)、磁力计(4)，还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元(5)，还包括与所述信号采集单元(5)连接的信号处理单元(6)。

9.一种磁场检测方法，其特征在于：包括如下步骤：

(a)人员行进中，以固定周期采集磁场测量数据、陀螺仪测量数据、加速度计测量数据；

(b)利用下式计算检验统计量T

其中，α₁、α₂、α₃、α₄分别为第一权值、第二权值、第三权值、第四权值，且α₁、α₂、α₃、α₄的取值范围均为[0,1]，且α₁+α₂+α₃+α₄＝1，m为人员行进的步数间隔，m＞2，s为人员步数，B_i、θ_i、ξ_i分别为人员行进中第i步的导航坐标系下的磁场幅度、磁场解算航向、陀螺仪解算航向、磁场解算航向与陀螺仪解算航向的偏差，分别为磁场幅度的噪声方差、磁场解算航向的噪声方差、陀螺仪解算航向的噪声方差、磁场解算航向与陀螺仪解算航向的偏差的方差，B_i、θ_i、ξ_i、均利用步骤(a)中的磁场测量数据、陀螺仪测量数据计算得到；

(c)若T＜γ，则判定在人员行进中第s步到第s+m-1步之间对应的时间区间内磁场处于稳态。

10.一种实现权利要求9所述的磁场检测方法的磁场检测装置，其特征在于：包括设置于人员足部(1)且具有相同载体坐标系的加速度计(2)、陀螺仪(3)、磁力计(4)，还包括用于采集磁场测量数据、陀螺仪测量数据、加速度计测量数据的信号采集单元(5)，还包括与所述信号采集单元(5)连接的信号处理单元(6)。