CN112461224A

CN112461224A - 一种基于已知姿态角的磁力计标定方法

Info

Publication number: CN112461224A
Application number: CN202011247295.4A
Authority: CN
Inventors: 旷俭; 李泰宇; 牛小骥; 刘韬
Original assignee: Wuhan University WHU
Current assignee: Wuhan University WHU
Priority date: 2020-11-10
Filing date: 2020-11-10
Publication date: 2021-03-09
Anticipated expiration: 2040-11-10
Also published as: CN112461224B

Abstract

本发明提供一种基于已知姿态角的磁力计标定方法。基于某一固定位置的磁场总强度不变及在同一坐标系下磁场强度投影分量不变的假设，依据磁力计测量模型，使用已知姿态角将磁力计观测值投影到当地坐标系，并通过最小二乘迭代解得磁力计标定参数。本发明与现有标定技术相比，算法简单，效率较高。同时实现了磁力计的软、硬磁效应标定，并将磁力计坐标系与惯性传感器坐标系对齐，使得磁力计和惯性传感器的信息放在一个公共框架中，可以在多源传感器融合定位、传感器阵列中发挥重要作用。

Description

一种基于已知姿态角的磁力计标定方法

技术领域

本发明属于磁力计标定领域，特别涉及姿态角已知时磁力计标定方法。

背景技术

磁力计是用来测量磁感应强度的传感器，它可以提供对磁北的参考，是低成本高性能导航***中用于姿态估计的关键信息源。磁力计由于加工工艺的限制，会存在零偏误差、交轴耦合误差、比例因子误差等问题，影响磁力计的测量精度；而且磁力计容易受到周围环境的磁干扰，这些干扰分为软磁干扰和硬磁干扰。因此为测量更准确的磁场信息，需对磁力计进行标定。此外，在多源融合传感器定位中，将磁力计和惯性传感器的信息放在一个公共框架中是实现高精度定位的基础，但由于安装工艺、软磁干扰等原因，磁力计与惯性传感器的轴系难以实现完全一致，故磁力计与惯性传感器进行交叉对准标定是一个关键点和亟需解决的问题。

目前根据国内外的文献来看，基于极大似然估计法的椭球拟合是目前最常用的磁力计内在校准方法，它的标定与姿态无关，利用了磁力计在局部位置或均匀磁场中的测量值无论方向如何都是恒定的这一事实。对于传感器之间的交叉标定，主要通过依靠局部重力信息完成，但这种标定方法依赖加速计测量值。

综上，在磁力计标定时，如何快速完成磁力计内在标定及交叉标定是需要解决的关键问题。本发明以最小二乘法，包括但不限于此应用场景，无需依赖姿态、重力信息，提出一种简单有效的磁力计标定及与惯性传感器交叉标定方法。

发明内容

针对磁力计快速校准的需求以及磁力计轴系与IMU惯性传感器轴系不对齐的问题，本发明提出了一种在姿态角已知的情况下在基于最小二乘原理完成磁力计的内在标定以及IMU惯性传感器的交叉标定，即可以快速实现标定磁力计交轴耦合误差、比例因子误差、零位偏置误差、软磁误差、硬磁误差，并实现磁力计与IMU惯性传感器轴系对齐。本发明无需借助任何外界设备，无需参数设置，简单可行，具有很好的普适性，同时能够满足现场快速(20s左右)标定的要求，具有良好的标定精度。

本发明采用如下的技术方案：一种基于最小二乘方法完成磁力计的内在标定以及与IMU惯性传感器的轴系对齐，主要是通过旋转智能手机完成标定，在基于某一位置的地磁场强度在当地坐标系下三轴磁场分量不变及磁场总强度不变的假设下，通过对磁力计的测量模型进行建模，使用泰勒展开及最小二乘方法，得到磁力计标定参数；所述的技术方案包括以下步骤：

步骤1，在室外空旷区域选择任意选择一个位置，并根据位置的经纬度坐标查询地球磁场参考模型，得到该位置的磁场总强度参考值；

步骤2，以传感器测量中心为旋转中心，使其绕着载体坐标系的X、Y、Z三轴各旋转多圈，并获取标定过程中每个历元的姿态角(即俯仰角、横滚角、航向角)及磁场信息；

步骤3，建立磁力计测量模型，确定待估参数，将磁力计的测量模型进行泰勒展开，使用最小二乘法迭代求得待估参数。

进一步的，对磁力计的测量模型进行建模时，考虑磁力计受到软磁误差、比例因子误差、交轴耦合误差、零位偏置误差、硬铁误差及传感器噪声，载体坐标系下磁力计的测量模型如下：

对磁力计原始观测数据m_{b_init}进行均值滤波，降低磁力计噪声n_M，得到m_b；将

的乘积作为一个待估矩阵，记为

将b_M和b_HI之和作为一个待估矩阵，记为B_M，模型简化后如下式，待估参数为

和B_M；

其中，b表示载体坐标系b系，b系是以惯性传感器IMU相位中心为原点，x轴指向载体前进方向，y轴垂直于x轴指向载体右侧，z轴与x轴和y轴垂直并构成右手系；

表示载体坐标系下磁场强度的真实值；

表示磁力计坐标系到惯性传感器IMU坐标系的姿态旋转矩阵，为3×3方阵；C_SI表示软磁效应变化矩阵；C_NO是磁力计三轴由非正交转化为正交的变化矩阵，为3×3方阵，主对角线元素为0；S_M为比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；m_{b_init}表示磁力计原始观测数据；b_M表示磁力计零位偏置；b_HI表示硬磁效应误差；n_M为磁力计噪声矢量。

进一步的，对磁力计测量模型进行迭代计算时，首先以m_b作为初次迭代计算时载体坐标系下磁场强度参考真值，即

之后迭代计算时

通过

求得，然后求得当地坐标系下的磁场强度真值

计算关系如下式：

式中，k表示第k个历元，j表示历元的总个数；

表示第k个历元载体坐标系到当地坐标系的姿态旋转矩阵，

表示第k个历元载体坐标系下磁场强度参考真值；n表示当地坐标n系，n系是以惯性传感器IMU相位中心为原点，x轴平行于当地水平面指向正北，y轴平行于当地水平面指向正东，z轴垂直于当地水平面向下，三者构成右手系；

在基于磁强总强度不变的假设下，根据M_{n_CGRF}求得其磁场总强度||M_{n_CGRF}||，通过计算出

与||M_{n_CGRF}||的比例关系，将

各轴的数据等比例变化，得到M_{n_ture}，将其作为当地坐标系下的磁场真值，进而得到载体坐标系下磁场强度参考真值

M_{n_ture}及

计算过程如下：

其中，

表示当地坐标系到载体坐标系的姿态旋转矩阵；

进一步的，通过一阶泰勒展开将简化模型

线性化，根据最小二乘法迭代求得待估参数，具体实现方式如下：

待估参数

为3×3方阵，待估参数B_M为3×1矩阵，具体表现为：

将式子

展开，

待求参数共计12个，记为X，

X＝[a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ b₁ b₂ b₃]^T

由于该式子为非线性，按泰勒公式展开，舍弃二阶项和高阶项，上标i表示迭代次数，如下式：

其中Δ表示残差，令：

令：xⁱ＝Xⁱ-X^i-1，

式(7)简化为：

令

将式(8)写为残差方程的形式，

通过最小二乘法即可求式(9)的解算结果，各观测值等权，P为单位阵，求得

x＝(H^TPH)^-1H^TPz (10)

Xⁱ＝xⁱ+X^i-1 (11)

式中，Xⁱ为磁力计原始观测数据m_{b_in}进行均值滤波后得到的m_b，通过

更新

重复该迭代过程直至收敛，得到参数结果。

进一步的，最小二乘法的初值通过以下方式获取，

对于

磁力计和惯性传感器坐标系之间的旋转角为小角度，故

接近单位阵；C_SI各个元素值都比较小；C_NO主对角线元素为0，其余元素也较小；S_M只有主对角线元素，各元素数值接近1；故

的初值为单位阵I_3×3；

对于B_M的初值，只考虑磁力计受零偏误差影响，通过进一步简化模型，即

将磁力计原始观测数据m_{b_init}作为第一次迭代计算时载体坐标系下的磁场真值，即

根据每个历元的

(

表示每个历元的载体坐标系到当地坐标系的姿态旋转矩阵)，求得当地坐标系下各轴的磁场强度的均值

作为当地坐标系下的磁场强度的真值。在基于磁强总强度不变的假设下，根据M_{n_CGRF}求得其磁场总强度||M_{n_CGRF}||。通过计算出

与||M_{n_CGRF}||的比例关系，将

各轴的数据等比例变化，得到M_{n_ture}，将其作为当地坐标系下的磁场真值。进而得到载体坐标系下磁场强度参考真值

将原始数据m_{b_init}与

求差得到每个历元的零偏，再将其求平均得到B_M；

当本次迭代求得的B_M和上次迭代求得的B_M各轴之差小于0.1mGauss时，认为零偏标定完成，否则更新

重复上述过程，最终求得B_M，将其作为泰勒展开中的初值。

本发明具有以下有益效果：

(1)本发明在获取高精度姿态角的基础上完成了磁力计的内在标定及与惯性传感器的交叉标定，可以减少零偏误差、交轴耦合误差、比例因子误差、软磁干扰和硬磁干扰以及磁力计与惯性传感器的轴系不对齐误差，标定效果较好。

(2)本发明操作简单，易于实现，操作时间短。只需以智能手机(即载体)的中心为旋转中心，绕着载体坐标系的X、Y、Z三轴各旋转720°即可完成标定工作，对姿态无要求，标定效率较高。

(3)本发明实现了将磁力计坐标系与惯性传感器坐标系对齐，使得磁力计和惯性传感器的信息放在一个公共框架中，可以在传感器阵列发挥重要作用。

附图说明

图1为手机标定动作示意图。

图2为迭代求解磁力计标定参数的流程图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步说明。

步骤1，选择室外某一空旷位置作为标定位置，获取该位置的经纬度坐标及高度，通过查询国际地磁参考场IGRF(International Geomagnetic Reference Field)模型获取该位置的磁场强度M_{n_IGRF}；

步骤2，如图1所示，以智能手机(即载体)的中心为旋转中心，使其绕着载体坐标系的X、Y、Z三轴各旋转720°；

步骤3，通过专利“MEMS陀螺自动标定方法”的方法，通过伪位置约束和反向平滑获取该标定过程中高精度的每个历元的载体坐标系到当地坐标系的姿态旋转矩阵

(姿态旋转矩阵的作用是实现矢量由一个坐标系投影变换到另一个坐标系)；

步骤4，建立磁力计测量模型，建立当地坐标系下磁场强度与载体坐标系下磁场强度之间的关系，通过对磁力计测量模型的迭代计算得到标定参数，计算流程如图2所示；

而且，步骤4的实现方式包括以下子步骤，

41)对磁力计的测量模型进行建模。受制造工艺、加工精度等的影响，磁力计存在交轴耦合误差、比例因子误差、零位偏置误差及传感器噪声。此外，磁力计容易受到周围环境的磁干扰，这些干扰分为软磁干扰和硬磁干扰，它们的表现形式与交轴耦合误差、比例因子误差、零位偏置误差相同。故需同时完成上述误差的标定。此外，还存在磁力计轴系与惯性传感器轴系不对齐误差。本发明认为载体坐标系与惯性传感器坐标系轴系相同，故在载体坐标系下磁力计的测量模型如下：

式中，b表示载体坐标系b系，b系是以惯性传感器IMU相位中心为原点，x轴指向载体前进方向，y轴垂直于x轴指向载体右侧，z轴与x轴和y轴垂直并构成右手系；n表示当地坐标n系，n系是以惯性传感器IMU相位中心为原点，x轴平行于当地水平面指向正北，y轴平行于当地水平面指向正东，z轴垂直于当地水平面向下，三者构成右手系；

表示载体坐标系下磁场强度的真实值；

表示磁力计坐标系到惯性传感器IMU坐标系的姿态旋转矩阵，为3×3方阵；C_SI表示软磁效应变化矩阵；C_NO是磁力计三轴由非正交转化为正交的变化矩阵，为3×3方阵，主对角线元素为0；S_M为比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；m_{b_init}表示磁力计原始观测数据，b_M表示磁力计零位偏置；b_HI表示硬磁效应误差；n_M为磁力计噪声矢量。

42)对磁力计原始观测数据m_{b_init}进行均值滤波，降低磁力计噪声n_M，得到m_b。

43)将式(1)中

的乘积作为一个待估矩阵，记为

用m_b代替m_{b_init}和n_M之和；将b_M和b_HI之和作为一个待估矩阵，记为B_M。磁力计测量模型简化后如下式，待估参数为

和B_M。

44)以m_b作为初次迭代计算时载体坐标系下磁场强度参考真值，即

之后迭代计算时

通过

求得，然后求得当地坐标系下的磁场强度真值

计算关系如下式：

式中，k(k＝1,2...j)表示第k个历元，j表示历元的总个数；

表示第k个历元载体坐标系到当地坐标系的姿态旋转矩阵，

表示第k个历元载体坐标系下磁场强度参考真值。

45)在基于磁强总强度不变的假设下，根据M_{n_CGRF}求得其磁场总强度||M_{n_CGRF}||.通过计算出

与||M_{n_CGRF}||的比例关系，将

各轴的数据等比例变化，得到M_{n_ture}。将其作为当地坐标系下的磁场真值，进而得到载体坐标系下磁场强度参考真值

M_{n_ture}及

计算过程如下：

其中，

表示当地坐标系到载体坐标系的姿态旋转矩阵；

46)对简化后的模型(式2)进行一阶泰勒展开，完成对模型的线性化处理，然后用最小二乘法求出待估参数

和B_M，具体实现方式如下，

待估参数

为3×3方阵，待估参数B_M为3×1矩阵，具体表现为：

将式子

展开，

待求参数共计12个，记为X，

X＝[a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ b₁ b₂ b₃]^T

其中Δ表示残差，令：

令：xⁱ＝Xⁱ-X^i-1，

式(7)简化为：

令

将式(8)写为残差方程的形式，

通过最小二乘法即可求式(9)的解算结果，本发明认为各观测值等权，P为单位阵，求得

x＝(H^TPH)^-1H^TPz (10)

Xⁱ＝xⁱ+X^i-1 (11)

Xⁱ的值即为步骤42)中的m_b；通过

更新

重复步骤43)～46)，直至迭代收敛，得到待估参数结果。

进一步的，最小二乘法的初值通过如下方式获得，

由于

磁力计和惯性传感器坐标系之间的旋转角为小角度，故

接近单位阵，C_SI各个元素值都比较小，C_NO主对角线元素为0，其余元素也较小，S_M只有主对角线元素，各元素数值接近1，故

的初值为单位阵I_3×3；

对于B_M，其初值的给定方法如下：

对于手机磁力计而言，零偏造成的误差远大于交轴耦合和比例因子等的误差，因此在进行泰勒展开获取初值时，只考虑磁力计受零偏误差影响，故进一步简化磁力计的测量模型为：

根据每个历元的

求得当地坐标系下各轴的磁场强度的均值，作为当地坐标系下的磁场强度的真值，设共有j个历元的数据，计算过程如下：

与||M_{n_CGRF}||的比例关系，将

M_{n_ture}及

计算过程如下：

根据每个历元的

求得每个历元的载体坐标系下的磁场强度，在只考虑零偏误差的条件下，将所有历元的载体坐标系下的磁场强度有差异的原因是由于存在零偏造成的，由于标定动作为绕轴旋转，通过平均即可得到载体坐标系下的磁场强度真值，计算过程如下：

将原始数据m_{b_init}与上式中的

求差得到每个历元的零偏，再将其求平均得到B_M

当本次迭代求得的B_M和上次迭代求得的B_M各轴之差小于0.1mGauss时，认为零偏标定完成，否则根据式(12)更新

重复上述过程。最终求得B_M，将其作为泰勒展开中的初值。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims

1.一种基于已知姿态角的磁力计标定方法，其特征在于，包括如下步骤：

步骤2，以传感器测量中心为旋转中心，使其绕着载体坐标系的X、Y、Z三轴各旋转多圈，并获取标定过程中每个历元的姿态角及磁场信息；

2.根据权利要求1所述一种基于已知姿态角的磁力计标定方法，其特征在于：对磁力计的测量模型进行建模时，考虑磁力计受到软磁误差、比例因子误差、交轴耦合误差、零位偏置误差、硬铁误差及传感器噪声，载体坐标系下磁力计的测量模型如下：

的乘积作为一个待估矩阵，记为

将b_M和b_HI之和作为一个待估矩阵，记为B_M；模型简化后如下式，待估参数为

和B_M；

表示载体坐标系下磁场强度的真实值；

3.根据权利要求2所述一种基于已知姿态角的磁力计标定方法，其特征在于：对磁力计测量模型进行迭代计算时，首先以m_b作为初次迭代计算时载体坐标系下磁场强度参考真值，即

之后迭代计算时

通过

求得，然后求得当地坐标系下的磁场强度真值

计算关系如下式：

式中，k表示第k个历元，j表示历元的总个数；

表示第k个历元载体坐标系到当地坐标系的姿态旋转矩阵，

与||M_{n_CGRF}||的比例关系，将

M_{n_ture}及

计算过程如下：

其中，

表示当地坐标系到载体坐标系的姿态旋转矩阵。

4.根据权利要求3所述一种基于已知姿态角的磁力计标定方法，其特征在于：通过一阶泰勒展开将简化模型

待估参数

为3×3方阵，待估参数B_M为3×1矩阵，具体表现为：

将式子

展开，

待求参数共计12个，记为X，

X＝[a₁ a₂ a₃ a₄ a₅ a₆ a₇ a₈ a₉ b₁ b₂ b₃]^T

其中Δ表示残差，令：

令：xⁱ＝Xⁱ-X^i-1，

式(7)简化为：

令

将式(8)写为残差方程的形式，

通过最小二乘法即可式(9)的解算结果，各观测值等权，P为单位阵，求得

x＝(H^TPH)^-1H^TPz (10)

Xⁱ＝xⁱ+X^i-1 (11)

式中，Xⁱ为磁力计原始观测数据m_{b_init}进行均值滤波后得到的m_b，通过

更新

重复该迭代过程直至收敛，得到参数结果。

5.根据权利要求4所述一种基于已知姿态角的磁力计标定方法，其特征在于：最小二乘法的初值通过以下方式获取，

对于

磁力计和惯性传感器坐标系之间的旋转角为小角度，故

的初值为单位阵I_3×3；

根据每个历元的

表示每个历元的载体坐标系到当地坐标系的姿态旋转矩阵，求得当地坐标系下各轴的磁场强度的均值

作为当地坐标系下的磁场强度的真值；在基于磁强总强度不变的假设下，根据M_{n_CGRF}求得其磁场总强度||M_{n_CGRF}||，通过计算出

与||M_{n_CGRF}||的比例关系，将

将原始数据m_{b_init}与

求差得到每个历元的零偏，再将其求平均得到B_M；

当本次迭代求得的B_M和上次迭代求得的B_M各轴之差小于0.1mGauss时，认为零偏标定完成，否则根据

更新

重复上述过程，最终求得B_M，将其作为泰勒展开中的初值。

6.根据权利要求1所述一种基于已知姿态角的磁力计标定方法，其特征在于：步骤2中，使传感器绕着载体坐标系的X、Y、Z三轴各旋转720度，所述姿态角包括俯仰角、横滚角、航向角。