CN114279426A - 一种六轴优化的磁力计在线校准方法 - Google Patents

Abstract

本发明公开了一种六轴优化的磁力计在线校准方法，包括以下步骤：获取加速度计的值、陀螺仪的值和上一时刻的磁力计的值；根据测量得到的加速度值和测量得到角速度值进行卡尔曼滤波，得到优化后稳定的陀螺仪角速度值；根据优化后的角速度和上一时刻磁力计的值，计算理论磁力计的值；根据理论磁力计值和实际测量的磁力计值构建扩展卡尔曼滤波器；利用扩展卡尔曼滤波器，根据测量的加速度计值和测量得到的角速度值，计算磁力计动态校准后的值。本发明利用扩展卡尔曼实现了磁力计的动态校准，使得校准结果更准确、实时和稳定；通过利用卡尔曼滤波器将加速度计和陀螺仪的数据进行融合滤波，有效抑制扩展卡尔曼动态校准磁力计的漂移。

Description

一种六轴优化的磁力计在线校准方法

技术领域

本发明属于AHRS技术领域，具体涉及一种六轴优化的磁力计在线校准方法。

背景技术

在AHRS领域，航向角的稳定是很重要的，仅靠六轴数据融合得到的航向角会由于陀螺仪的积分误差无法补偿而随时间漂移；所以确定准确的航向角需要用磁力计测量地球磁场作为绝对参考，而磁力计的实际使用极易受到环境磁场的干扰，包括硬磁干扰和软磁干扰。磁场校准就是为了计算硬磁干扰和软磁干扰的参数，并对磁场数据进行这两者的修正补偿，从而得到校准后稳定的磁力计数据。

相关研究中，椭球拟合法在一般环境下能取得很好的校准效果，但对磁力计数据的要求高，需要磁场数据点均匀地分布在椭球表面，实际使用中需要执行特殊数据采集动作，这对某些场景(如二维场景割草机器人、行走机器人)中很难自主执行采集动作；而且在磁力计使用环境发生变化后需要重新校准，所以它不具备便捷和实时性。还有使用EKF融合陀螺仪和磁力计数据，实时计算了硬磁干扰和软磁干扰，实现了实时、稳定、无需重复校准和便捷。但由于陀螺仪数据的漂移，得到校准后计数据也不是很稳，也会存在漂移。

发明内容

本发明针对以上问题，提出了一种六轴优化的磁力计在线校准方法。

本发明的技术方案是：

获取加速度计的值、陀螺仪的值和上一时刻的磁力计的值；

根据测量得到的加速度值和测量得到角速度值进行卡尔曼滤波，得到优化后稳定的陀螺仪角速度值；

根据优化后的角速度和上一时刻磁力计的值，计算理论磁力计的值；

根据理论磁力计值和实际测量的磁力计值构建扩展卡尔曼滤波器；

利用扩展卡尔曼滤波器，根据测量的加速度计值和测量得到的角速度值，计算磁力计动态校准后的值。

可选地，所述根据测量得到的加速度值和测量得到角速度值进行卡尔曼滤波，得到优化后稳定的陀螺仪角速度值，包括：根据所述加速度计的值计算出旋转角度作为卡尔曼滤波的观测量，根据所述陀螺仪的值计算出旋转角度和陀螺仪偏移量作为卡尔曼滤波二维状态量。

可选地，二维状态转移方程和观测方程为：

为陀螺仪的测量值计算的旋转角度值，b为陀螺仪偏移量，w为包含固定偏差的陀螺仪输出角速度，

为加速度计经处理后得到的角度值，w_g为陀螺仪测量噪声，w_a为加速度计测量噪声，w_g和w_a相互独立，此处假设二者为满足正态分布的白色噪声，令T为系统采样周期，得到离散系统的状态转移方程和测量方程：

z(k)＝[1 0]x(k)+w_a

其中，

是K时刻的状态量，x(k-1)是K-1时刻的状态量，w(k-1)是K-1时刻包含固定偏差的陀螺仪角速度测量值，

为K时刻加速度计经处理后得到的旋转角度值，通过上面构建的卡尔曼滤波器，经过预测和更新五个公式递推迭代后，得到优化后的陀螺仪角速度值w_J＝w+b。

可选地，所述根据优化后的角速度和上一时刻磁力计的值，计算理论磁力计的值，包括：

B_C,k+1＝(I+[w_J,k]_xT)B_C,k

其中，B_C,k+1为理论磁力计数据，[w_J,k]_x是由w_J构成的反对称矩阵，B_C,k是上一时刻的磁力计的值，I是单位矩阵，T为系统采样周期。

可选地，所述根据理论磁力计值和实际测量的磁力计值构建扩展卡尔曼滤波器，包括：根据所述理论磁力计数据与测量磁力计数据构建状态转移矩阵和量测矩阵；利用所述状态转移矩阵和量测矩阵构建扩展卡尔曼滤波器。

可选地，所述用理论磁力计数据与测量磁力计数据构建状态转移矩阵和量测矩阵，包括：建立硬磁干扰和软磁干扰参数矩阵；根据所述理论磁力计数据、软磁参数矩阵和硬磁参数矩阵得到当前时刻状态量；根据所述当前时刻状态值和磁力计量测值得到所述状态转移矩阵和量测矩阵。

可选地，所述利用扩展卡尔曼滤波器，根据测量的加速度计值和测量得到的角速度值，计算磁力计动态校准后的值，包括：将上一时刻测量的加速度计值和测量得到的角速度值通过卡尔曼滤波器优化后的角速度值、上一时刻测量得到的磁力计的值输入到所述扩展卡尔曼滤波器，得到校准后当前时刻状态值和当前时刻状态值的方差估计值，所述校准后当前时刻状态值中包含所述磁力计数据动态校准值。

本发明提出的一种六轴优化的磁力计动态校准方法，至少具有如下有益效果：

1、利用扩展卡尔曼实现了磁力计的动态校准，使得校准结果更准确、实时和稳定；

2、无需操作者实施特殊采集动作，校准磁力计更为便捷、快速，特别是在二维场景下也会有很好的效果；

3、通过利用卡尔曼滤波器将加速度计和陀螺仪的数据进行融合滤波，有效抑制扩展卡尔曼动态校准磁力计的漂移，使得本发明可以有更多应用场景。

附图说明

图1是本发明六轴优化磁力计在线校准的流程图。

图2是卡尔曼滤波优化前后陀螺仪角速度值的对比图。

图3是本发明六轴优化磁力计在线校准后的效果示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施案例，并参照附图，对本发明进一步详细说明。

图1展示出了本发明的流程图。

参阅图1，同时结合图2和图3，对图1所示方法进行详细说明。如图1所示，该磁力计数据的动态校准方法包括操作S1-操作S5。

S1，获取加速度计的值、陀螺仪的值和上一时刻的磁力计的值。

本发明中的加速度计、陀螺仪的值可以通过六轴传感器芯片得到，磁力计的值可以通过三轴磁传感器芯片得到，同时负责处理的单片机可以保存上一时刻的数据。将X轴原始加速度计的值处理成旋转角度

(以X轴举例)，其公式为：

其中，Ax、Ay、Az是加速度计的原始测量值。

S2，根据测量得到的加速度值和测量得到角速度值进行卡尔曼滤波融合，得到优化后的陀螺仪角速度值；

根据角度和角速度之间关系得到离散系统的状态方程和测量方程为：

z(k)＝[1 0]x(k)+w_a

其中，

为K时刻加速度计经处理后得到的旋转角度值；

是K时刻的状态量，x(k-1)是K-1时刻的状态量，

为陀螺仪的测量值计算的旋转角度值，b为陀螺仪偏移量，w(k-1)是K-1时刻包含固定偏差的陀螺仪角速度测量值，T为系统采样周期，w_g为陀螺仪测量噪声，w_a为加速度计测量噪声，_g和w_a相互独立，此处假设二者为满足正态分布的白色噪声。

其滤波器可根据卡尔曼滤波的五个经典公式解算：

其中，

是状态转移矩阵,

是控制矩阵,U_k-1＝w(k-1)是控制量；H＝[1 0]是量测矩阵。

和

分别是K时刻的先验估计值和K-1时刻的后验状态估计值；

是K时刻的先验估计协方差；P_k和P_k-1分别是K时刻和K-1时刻的后验估计协方差；K_k是滤波增益矩阵；Q是过程激励噪声协方差；R是测量噪声协方差；

根据卡尔曼滤波得到优化后的陀螺仪角速度值w_J＝w+b，滤波优化效果对比如图2所示。

S3，根据优化后的角速度和上一时刻磁力计的值，计算理论磁力计的值，包括：

B_C,k+1＝(I+[w_J,k]_xT)B_C,k

其中，B_C,k+1为理论磁力计数据，[w_J,k]_x是由w_J构成的反对称矩阵，g_C,k是上一时刻的磁力计的值，I是单位矩阵，T为系统采样周期。

S4，根据理论磁力计值和实际测量的磁力计值构建扩展卡尔曼滤波器，包括：

建立状态转移方程，状态量有：磁场数据的3个值B_C,k＝(B_C,xB_C,yB_C,z)^T，硬磁干扰的3个值v_k＝(v_xv_yv_z)，软磁干扰矩阵中虽然有9个值，但是因为它是对称的，只需要6个值就可以，记为w_e,k＝(w₁₁w₂₂w₃₃w₁₂w₁₃w₂₃)，故12维状态量为：

则有状态方程为x_k＝F_k-1·x_k-1，其中状态转移矩阵为：

其中陀螺仪测量噪声σ_g得到的协方差矩阵为：

其中，D为控制参数，在实际情况中调整。

根据磁力计实际测量数据作为量测方程：B_P,k＝ω_kB_C,k+v_k+ε_m，B_P,k为k时刻的未校准磁力计读数，ω_k和v_k分别是k时刻的软磁干扰矩阵和硬磁干扰矢量，ε_m是磁力计量测噪声。则量测方程可写为：Z_k＝H_kx_k+J_k，由于是非线性的方程，其量测雅可比矩阵H_k为：

其中，软磁干扰矩阵

I_3*3是3*3的单位矩阵,J_k＝ε_m磁力计量测噪声，x_k,1、x_k,2、x_k,3分别为状态量x_k的前三个值。

S5，利用扩展卡尔曼滤波器，根据测量的加速度计值和测量得到的角速度值，计算磁力计动态校准后的值，包括：

将上一时刻测量的加速度计值和测量得到的角速度值通过卡尔曼滤波器优化后的角速度值、上一时刻测量得到的磁力计的值输入到所述扩展卡尔曼滤波器，得到校准后当前时刻状态值和当前时刻状态值的方差估计值，所述校准后当前时刻状态值中包含所述磁力计数据动态校准值。

具体地，将优化后的角速度的值代入构成的扩展卡尔曼滤波器中，根据状态转移方程和量测方程经过下面五个经典公式得到磁力计校准后的结果。

预测阶段：

x_k|k-1＝F_k-1x_k-1

更新阶段：

x_k＝x_k|k-1+K_k(Z_k-H_kx_k|k-1)

P_k＝(I-K_kH_k)P_k|k-1

其中，F_k-1是K-1时刻的状态转移矩阵；x_k-1|是K-1时刻的状态量；x_k|k-1是状态量从k-1时刻到k时刻的一步预测值；P_k-1是K-1时刻状态的方差估计值；Q_k-1是状态转移过程中K-1时刻的噪声方差矩阵；P_k|k-1是状态值方差估计值从K-1时刻到K时刻的预测值；K_k是滤波增益矩阵；H_k是K时刻的量测矩阵；R_k是由J_k构成的K时刻的测量噪声协方差矩阵；Z_k是K时刻磁力计的测量值；x_k|和P_k|是通过EKF算法求得的k时刻的状态量估计值和状态的方差估计值。

两个需要赋予初值的量：

x₀＝[B_p,0111000000]^T

其中，B_p,0是磁力计的初始测量值，最后循环递推滤波后得到的x_k中包含K时刻需要磁力计校准后的值。

图3展示了本发明六轴优化磁力计在线校准后的效果展示图，可以看出本发明能有效抑制校准后磁力计数据随时间的漂移，能得到更稳定的磁力计校准数据。

综上所述，本发明一种六轴优化的磁力计在线校准方法，利用卡尔曼滤波算法融合加速度计和陀螺仪的数据，得到更稳定的陀螺仪角速度值，然后配合使用扩展卡尔曼滤波器来融合磁力计的值，实现了磁力计的动态校准，更便捷，不再局限于三维场景，在二维场景下也能广泛使用。而且稳定的角速度值可以有效抑制单一扩展卡尔曼滤波器校准的漂移，实现了便捷、稳定的动态磁力计校准。

Claims (6)

1.一种六轴优化的磁力计在线校准方法，其特征在于该方法包括以下步骤：

获取加速度计的值、陀螺仪的值和上一时刻的磁力计的值；

根据测量得到的加速度值和测量得到角速度值进行卡尔曼滤波，得到优化后稳定的陀螺仪角速度值；

根据优化后的角速度和上一时刻磁力计的值，计算理论磁力计的值；

根据理论磁力计值和实际测量的磁力计值构建扩展卡尔曼滤波器；

利用扩展卡尔曼滤波器，根据测量的加速度计值和测量得到的角速度值，计算磁力计动态校准后的值。

2.根据权利要求1所述的一种六轴优化的磁力计在线校准方法，其特征在于，所述加速度计和陀螺仪的值通过六轴传感器芯片得到，磁力计的值通过三轴磁传感器芯片得到。

3.根据权利要求1所述的一种六轴优化的磁力计在线校准方法，其特征在于，根据测量得到的加速度值和测量得到角速度值进行卡尔曼滤波，得到优化后稳定的陀螺仪角速度值，具体是：

根据所述加速度计的值计算出旋转角度作为卡尔曼滤波的观测量，根据陀螺仪的测量值计算旋转角度和陀螺仪偏移量作为卡尔曼滤波的二维状态量，构建的状态转移方程和测量方程为：

z(k)＝[1 0]x(k)+w_a

其中，

是K时刻的状态量，x(k-1)是K-1时刻的状态量，

为陀螺仪的测量值计算的旋转角度值，b为陀螺仪偏移量，w(k-1)是K-1时刻包含固定偏差的陀螺仪角速度测量值，T为系统采样周期，

为K时刻加速度计经处理后得到的旋转角度值，w_g为陀螺仪测量噪声，w_a为加速度计测量噪声，w_g和w_a相互独立，此处假设二者为满足正态分布的白色噪声，通过上面构建的卡尔曼滤波器，经过预测和更新迭代后，得到优化后的陀螺仪角速度值w_J＝w+b。

4.根据权利要求1所述的一种六轴优化的磁力计在线校准方法，其特征在于，根据优化后的角速度和上一时刻磁力计的值，计算理论磁力计的值，具体是：得到优化后的角速度w_J，通过已知上一时刻的磁力计的值B_C，k得到磁力计的理论值B_C，k+1：

B_C，k+1＝(I+[w_J，k]_xT)B_C，k

其中，I是单位矩阵，[w_J，k]_x是由w_J构成的反对称矩阵，T为系统采样周期。

5.根据权利要求1所述的一种六轴优化的磁力计在线校准方法，其特征在于，根据理论磁力计值和实际测量的磁力计值构建扩展卡尔曼滤波器，具体是：建立硬磁干扰和软磁干扰参数的多维状态量，建立依据磁力计测量值的量测方程，构建得到扩展卡尔曼滤波器的状态方程和量测方程为：

x_k＝F_k-1·x_k-1

Z_k＝H_kx_k+J_k

其中，x_k和x_k-1分别是K时刻和K-1时刻的状态量；F_k-1是K-1时刻的状态转移矩阵；Z_k是磁力计的测量值；H_k是K时刻的测量矩阵，J_k是测量噪声。

6.根据权利要求1所述的一种六轴优化的磁力计在线校准方法，其特征在于，利用扩展卡尔曼滤波器，根据测量的加速度计值和测量得到的角速度值，计算磁力计动态校准后的值，具体是：

将上一时刻测量的加速度计值和测量得到的角速度值通过卡尔曼滤波器优化后的角速度值、上一时刻测量得到的磁力计的值输入到所述扩展卡尔曼滤波器，得到校准后当前时刻状态值和当前时刻状态值的方差估计值，所述校准后当前时刻状态值中包含所述磁力计数据动态校准值。

Images