CN114353825B - 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及系统 - Google Patents

Abstract

本发明请求保护基于无迹卡尔曼滤波的磁力计在线校准算法，包括以下步骤：初始阶段，在空间中任意旋转磁力计，采集一段时间的数据；根据采集的磁力计数据进行椭球拟合；根据磁力计误差模型与拟合结果得到误差参数矩阵，完成磁力计初始校准；在行进过程中使用无迹卡尔曼滤波实时估计误差，对磁力计进行校准；利用校准后的磁力计输出进行航向估计，并对陀螺仪解算的航向进行修正。实验结果表明，本发明的磁力计校准方法，与磁力计未校准和仅进行初始校准磁力计的行人导航算法相比，闭环误差分别下降了6.17％和2.8％，有效地校准了磁力计，并抑制了行人导航中的航向角发散，提升了导航精度。

Description

基于无迹卡尔曼滤波的磁力计在线校准算法、介质及系统

技术领域

本发明属于惯性导航定位技术领域，特别是涉及基于无迹卡尔曼滤波的磁力计在线校准算法。

背景技术

惯性导航定位技术是一种不依赖于外界信息的自主导航定位技术，通过采集人体的角速度和加速度信息完成位置解算。但由于传感器误差和解算原理，航向角误差会随时间累积，导致定位精度下降。

磁力计可以通过测量磁场强度和方向来确定航向，不存在累积误差问题，可以用来修正航向角。但由于磁力计存在误差，因此需要在使用前对其进行校准。

对于磁力计的校准，前人已提出多种方法。庞鸿锋等人[庞鸿锋,潘孟春,王伟,等.基于高斯牛顿迭代算法的三轴磁强计校正[J].仪器仪表学报,2013,34(7):67-72.]使用高斯牛顿迭代法求解非线性方程得到磁力计误差参数，使各个轴的误差降低了两个数量级0。Deng等人[Deng Yang,Zheng You,Bin Li,et al.Complete tri-axis magnetometercalibration with a gyro auxiliary[J].Sensors,2017,17(6):1223-1243]使用陀螺仪辅助，对磁力计输出进行线性插值来计算误差参数，经标定后，磁力计计算的航向误差降至0.5°。孙伟等人[孙伟,杨一涵,王野.基于椭球拟合的磁力计误差校正方法研究[J].传感技术学报,2018,31(09):77-80.]提出了八位置系统误差，以此求取误差矩阵系数，能够满足一般应用的需求0。李冰等人[李冰,雷泷杰,陈超.基于椭圆拟合的双轴磁传感器标定方法[J].探测与控制学报,2020,42(03):20-23.]提出了基于椭圆拟合的标定方法，对磁力计输出进行椭圆拟合，以此求出标定参数，能有效降低其测量误差分布。上述算法大都在实验室环境下进行验证，或使用转台辅助，无法消除行人行走过程由动态环境产生的磁误差，因此不利于实际使用。

专利公开号为CN 109781084 A，《一种校准磁力计的装置和方法》，将磁力计电路板安装在电机上，通过电机旋转校准磁力计，且在飞行过程也可以触发校准装置，应用于航空领域，使用电机辅助，不满足行人导航的便携性。

专利公开号为CN 106289243 B，《一种磁力计自动校准方法及系统》，通过陀螺仪确定N组角度值，并通过每组角度值对应的磁力计数据生成校准矩阵，使用校准矩阵对磁力计进行校准，无法消除动态环境产生的误差。

发明内容

本发明旨在解决以上现有技术的问题。提出了基于无迹卡尔曼滤波的磁力计在线校准算法、存储介质及系统。本发明的技术方案如下：

基于无迹卡尔曼滤波的磁力计在线校准算法，其包括以下步骤：

采集一段时间内任意旋转磁力计的磁力计数据；

对采集的磁力计数据进行椭球拟合，即将样本点以椭球方程为模型进行拟合，确定椭球拟合结果；

根据磁力计误差模型与椭球拟合结果得到误差参数矩阵，完成磁力计初始校准；

在人体佩戴磁力计设备的行进过程中使用无迹卡尔曼滤波实时估计误差，根据状态方程与量测方程持续更新磁力计误差的状态值与量测值，结合初始校准后的磁力计输出对磁力计进行在线校准；利用校准后的磁力计输出进行航向估计，并对陀螺仪解算的航向进行修正。

进一步的，所述对采集的磁力计数据使用最小二乘法进行椭球拟合，得到椭球方程的各个参数，椭球方程为ax²+by²+cz²+2fyz+2gxz+2hxy+2px+2qy+2rz+d＝0，椭球参数为[a b c d f g h p q r d]^T，x、y、z分别表示空间直角坐标系的横轴、纵轴与竖轴，a、b、c、d、f、g、h、p、q、r、d分别表示方程中各项的系数。

进一步的，所述根据磁力计误差模型与椭球拟合结果得到误差参数矩阵，完成磁力计初始校准，具体包括：所述磁力计误差模型为：

D＝K₁K₂K₃ (2)

b＝K₁K₂b₂+b₁ (3)

其中，B为载体坐标系下无误差的磁力计输出，为载体坐标系下的含误差的磁力计输出，D和b为误差参数矩阵，分别为3×3的矩阵和三维列向量，ε为满足均值为0，方差为的高斯噪声，K₁为磁力计比例因子误差，K₂为非正交误差，K₃为软磁误差，b₁为零偏误差，b₂为硬磁误差。

进一步的，在行进过程中磁力计误差的状态方程与量测方程为：

其中，x＝[w_x w_y w_z]^T，为待估计的三轴零偏状态，M_x、M_y、M_z分别表示初始校准后的三维磁力计输出，为x的一阶导数，y为观测值，ω和v为高斯噪声；

计算步骤如下：

(1)系统离散化：将状态方程与观测方程进行离散化处理；

(2)计算Sigma采样点：根据k-1时刻状态估计值与协方差生成采样点；

(3)时间更新：将采样点通过状态转移函数生成新的采样点集，并计算转移后的均值与协方差，并将新的采样点通过量测函数转移，并计算量测值的均值；

(4)量测更新：计算量测值的方差及协方差，并求出滤波增益，完成状态估计与方差估计；

(5)磁力计修正：使用估计出的零偏对磁力计进行修正。

进一步的，所述将状态方程与观测方程进行离散化处理具体公式为：

所述计算Sigma采样点具体公式为：：根据k-1时刻状态估计值与协方差生成采样点

其中，为k-1时刻第i个采样点，/>为x_k-1的均值，P_x,k-1为k-1时刻x的方差，κ为尺度参数，n为采样点数。

进一步的，所述将采样点通过状态转移函数生成新的采样点集，并计算转移后的均值与协方差，并将新的采样点通过量测函数转移，并计算量测值的均值，具体公式为：

其中，为状态预测后的采样点，/>和/>为状态预测后的均值和方差，W_i ^m与W_i ^c分别为均值权值与方差权值，Q_k为系统噪声的协方差矩阵，h(·)表示量测函数，/>为通过量测函数转移后的采样点，/>为其均值。

进一步的，所述量测更新具体包括以下步骤：计算量测值的方差及协方差，并求出滤波增益，完成状态估计与方差估计：

P_x,k＝P_x,k-KP_y,kK^T (16)

其中，P_y,k为量测值的方差，P_xy,k为状态量与量测量的协方差，K为滤波增益，和P_x,k为x的状态估计与方差估计，R_k表示观测噪声的协方差矩阵。

所述磁力计修正具体为：使用估计出的零偏对磁力计进行修正，即

其中，B′、B″分别表示初始校准后的磁力计输出与在线校准后的磁力计输出。

进一步的，所述航向修正方法具体为：使用相邻步数陀螺仪解算出的航向之差Δψ_g作为阈值条件，使用陀螺仪解算出的航向ψ_g与磁力计解算出的航向ψ_m进行加权得到当前步伐的航向，如下式所示：

其中，α、β为基于实验得到的加权系数，λ为阈值。

一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如任一项所述的磁力计在线校准方法。

一种基于任一项所述方法的磁力计在线校准系统，其包括：

采集单元：用于采集一段时间内任意旋转磁力计的磁力计数据；

拟合单元：用于对采集的磁力计数据进行椭球拟合，即将样本点以椭球方程为模型进行拟合，确定椭球拟合结果；

初始校准单元：用于根据磁力计误差模型与椭球拟合结果得到误差参数矩阵，完成磁力计初始校准；

校准单元：在人体佩戴磁力计设备的行进过程中使用无迹卡尔曼滤波实时估计误差，根据状态方程与量测方程持续更新磁力计误差的状态值与量测值，结合初始校准后的磁力计输出对磁力计进行在线校准；

航向调整单元：用于利用校准后的磁力计输出进行航向估计，并对陀螺仪解算的航向进行修正。

本发明的优点及有益效果如下：

无需使用外部设备辅助，仅需使用磁力计本身采集的数据进行处理从而完成校准；

建立了磁力计动态误差的状态方程与量测方程，以磁力计的三轴零偏作为估计状态，利用在某一特定环境下，磁场强度恒定这一特点，以磁场强度的模值作为观测量，使用无迹卡尔曼滤波算法实时更新状态值与协方差，对动态环境产生的磁误差进行实时估计，完成磁力计的实时校准；

通过校准后的磁力计解算出磁航向，对陀螺解算出的航向进行条件加权，使累积误差权重减小，真实航向权重增大，能够在一定程度上消除陀螺由于误差积分累计的航向误差，从而抑制行人导航中的航向角发散。

附图说明

图1是本发明提供优选实施例磁力计校准的流程图；

图2是本发明的算法流程图；

图3本发明磁力计初始校准前的结果图；

图4本发明磁力计初始校准后的结果图

图5是验证算法的路线图；

图6是不同算法下的结果图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

基于无迹卡尔曼滤波的磁力计在线校准算法，使用实验室自主研发的导航设备，其集成了三轴陀螺仪、三轴加速度计和磁力计，进行磁力计校准，之后进行导航定位。附图1给出了本发明磁力计校准的流程图。包括以下步骤：

步骤1，初始阶段，在空间中任意旋转磁力计，采集一段时间的数据；

步骤2，根据采集的磁力计数据进行椭球拟合；

步骤3，根据磁力计误差模型与拟合结果得到误差参数矩阵，完成磁力计初始校准；

步骤4，在行进过程中使用无迹卡尔曼滤波实时估计误差，对磁力计进行校准；

步骤5，利用校准后的磁力计输出进行航向估计，并对陀螺仪解算的航向进行修正。

进一步的，所述步骤3中，磁力计误差模型为：

D＝K₁K₂K₃ (2)

b＝K₁K₂b₂+b₁ (3)

其中，B为载体坐标系下无误差的磁力计输出，为载体坐标系下的含误差的磁力计输出，D和b为误差参数矩阵，分别为3×3的矩阵和三维列向量，ε为满足均值为0，方差为的高斯噪声，K₁为磁力计比例因子误差，K₂为非正交误差，K₃为软磁误差，b₁为零偏误差，b₂为硬磁误差。

进一步的，所述步骤4中，磁力计误差的状态方程与量测方程为：

其中，x＝[w_x w_y w_z]^T，为待估计的三轴零偏状态，M_x、M_y、M_z分别表示初始校准后的三维磁力计输出，为x的一阶导数，y为观测值，ω和v为高斯噪声。

计算步骤如下：

(1)系统离散化：将状态方程与观测方程进行离散化处理。

(2)计算Sigma采样点：根据k-1时刻状态估计值与协方差生成采样点。

其中，为k-1时刻第i个采样点，/>为x_k-1的均值，P_x,k-1为k-1时刻x的方差，κ为尺度参数，n为采样点数。

(3)时间更新：将采样点通过状态转移函数生成新的采样点集，并计算转移后的均值与协方差，并将新的采样点通过量测函数转移，并计算量测值的均值。

其中，为状态预测后的采样点，/>和/>为状态预测后的均值和方差，W_i ^m与W_i ^c分别为均值权值与方差权值，Q_k为系统噪声的协方差矩阵，h(·)表示量测函数，/>为通过量测函数转移后的采样点，/>为其均值。

(4)量测更新：计算量测值的方差及协方差，并求出滤波增益，完成状态估计与方差估计。

P_x,k＝P_x,k-KP_y,kK^T(16)

其中，P_y,k为量测值的方差，P_xy,k为状态量与量测量的协方差，K为滤波增益，和P_x,k为x的状态估计与方差估计，R_k表示观测噪声的协方差矩阵。

(5)磁力计修正：使用估计出的零偏对磁力计进行修正，即

其中，B′、B″分别表示初始校准后的磁力计输出与在线校准后的磁力计输出。

进一步的，所述步骤5中航向修正方法为使用相邻步数陀螺仪解算出的航向之差Δψ_g作为阈值条件，使用陀螺仪解算出的航向ψ_g与磁力计解算出的航向ψ_m进行加权得到当前步伐的航向，如下式所示：

其中，α、β为基于实验得到的加权系数，λ为阈值。

附图2为算法流程图，图3和图4分别为磁力计初始校准前和校准后的结果图。

根据附图5所示路线进行实验验证，最终得到导航定位结果如附图6所示。通过对数据进行分析处理，得到闭环误差如表1。从表中可以看出，本发明的磁力计校准方法，与磁力计未校准和仅进行初始校准磁力计的行人导航算法相比，闭环误差分别下降了6.17％和2.8％，有效地校准了磁力计，并抑制了行人导航中的航向角发散，提升了导航精度。

表1不同方案误差比较

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (6)

1.基于无迹卡尔曼滤波的磁力计在线校准算法，其特征在于，包括以下步骤：

采集一段时间内任意旋转磁力计的磁力计数据；

对采集的磁力计数据进行椭球拟合，即将样本点以椭球方程为模型进行拟合，确定椭球拟合结果；

根据磁力计误差模型与椭球拟合结果得到误差参数矩阵，完成磁力计初始校准；

在人体佩戴磁力计设备的行进过程中使用无迹卡尔曼滤波实时估计磁力计的误差，根据状态方程与量测方程持续更新磁力计误差的状态值与量测值，结合初始校准后的磁力计输出对磁力计进行在线校准；

利用校准后的磁力计输出进行航向估计，并对陀螺仪解算的航向进行修正；

在行进过程中磁力计误差的状态方程与量测方程为：

其中，x＝[w_x w_y w_z]^T，为待估计的三轴零偏状态，为x的一阶导数，M_x、M_y、M_z分别表示初始校准后的三维磁力计输出，y为观测值，ω和v为高斯噪声；

计算步骤如下：

(1)系统离散化：将状态方程与观测方程进行离散化处理；

(2)计算Sigma采样点：根据k-1时刻状态估计值与协方差生成采样点；

(3)时间更新：将采样点通过状态转移函数生成新的采样点集，并计算转移后的均值与协方差，并将新的采样点通过量测函数转移，并计算量测值的均值；

(4)量测更新：计算量测值的方差及协方差，并求出滤波增益，完成状态估计与方差估计；

(5)磁力计修正：使用估计出的零偏对磁力计进行修正；

所述将状态方程与观测方程进行离散化处理具体公式为：

所述计算Sigma采样点具体公式为：根据k-1时刻状态估计值与协方差生成采样点

其中，为k-1时刻第i个采样点，/>为x_k-1的均值，P_x,k-1为k-1时刻x的方差，κ为尺度参数，n为采样点数；

所述将采样点通过状态转移函数生成新的采样点集，并计算转移后的均值与协方差，并将新的采样点通过量测函数转移，并计算量测值的均值，具体公式为：

其中，为状态预测后的采样点，/>和/>为状态预测后的均值和方差，W_i ^m与W_i ^c分别为均值权值与方差权值，Q_k为系统噪声的协方差矩阵，h(·)表示量测函数，/>为通过量测函数转移后的采样点，/>为其均值；

所述量测更新具体包括以下步骤：计算量测值的方差及协方差，并求出滤波增益，完成状态估计与方差估计：

P_x,k＝P_x,k-KP_y,kK^T (16)

其中，P_y,k为量测值的方差，P_xy,k为状态量与量测量的协方差，K为滤波增益，和P_x,k为x的状态估计与方差估计，R_k表示观测噪声的协方差矩阵；

所述磁力计修正具体为：使用估计出的零偏对磁力计进行修正，即

其中，B′、B″分别表示初始校准后的磁力计输出与在线校准后的磁力计输出。

2.根据权利要求1所述的基于无迹卡尔曼滤波的磁力计在线校准算法，其特征在于，所述对采集的磁力计数据使用最小二乘法进行椭球拟合，得到椭球方程的各个参数，椭球方程为ax²+by²+cz²+2fyz+2gxz+2hxy+2px+2qy+2rz+d＝0，椭球参数为[a b c d f g h p q rd]^T，x、y、z分别表示空间直角坐标系的横轴、纵轴与竖轴，a、b、c、d、f、g、h、p、q、r、d分别表示方程中各项的系数。

3.根据权利要求1所述的基于无迹卡尔曼滤波的磁力计在线校准算法，其特征在于，所述根据磁力计误差模型与椭球拟合结果得到误差参数矩阵，完成磁力计初始校准，具体包括：

所述磁力计误差模型为：

D＝K₁K₂K₃ (2)

b＝K₁K₂b₂+b₁ (3)

其中，B为载体坐标系下无误差的磁力计输出，为载体坐标系下的含误差的磁力计输出，D和b为误差参数矩阵，分别为3×3的矩阵和三维列向量，ε为满足均值为0，方差为/>的高斯噪声，K₁为磁力计比例因子误差，K₂为非正交误差，K₃为软磁误差，b₁为零偏误差，b₂为硬磁误差。

4.根据权利要求1述的基于无迹卡尔曼滤波的磁力计在线校准算法，其特征在于，所述航向修正方法具体为：使用相邻步数陀螺仪解算出的航向之差Δψ_g作为阈值条件，使用陀螺仪解算出的航向ψ_g与磁力计解算出的航向ψ_m进行加权得到当前步伐的航向，如下式所示：

其中，α、β为基于实验得到的加权系数，λ为阈值。

5.一种计算机可读存储介质，其特征在于，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如权利要求1-4一项所述的磁力计在线校准算法。

6.一种基于权利要求1-4一项所述算法的磁力计在线校准系统，其特征在于，包括：

采集单元：用于采集一段时间内任意旋转磁力计的磁力计数据；

拟合单元：用于对采集的磁力计数据进行椭球拟合，即将样本点以椭球方程为模型进行拟合，确定椭球拟合结果；

初始校准单元：用于根据磁力计误差模型与椭球拟合结果得到误差参数矩阵，完成磁力计初始校准；

校准单元：在人体佩戴磁力计设备的行进过程中使用无迹卡尔曼滤波实时估计误差，根据状态方程与量测方程持续更新磁力计误差的状态值与量测值，结合初始校准后的磁力计输出对磁力计进行在线校准；

航向调整单元：用于利用校准后的磁力计输出进行航向估计，并对陀螺仪解算的航向进行修正。