CN112539777A - 一种九轴传感器的误差参数标定方法 - Google Patents

Abstract

本发明提供一种九轴传感器的误差参数标定方法。基于标定过程中位置不变及单一位置下环境磁场不变的假设，以惯导机械编排方程和磁力计观测模型为系统状态方程，使用伪位置观测和单一位置下磁场投影分量不变约束构建量测方程，通过卡尔曼滤波得到传感器标定参数。本发明与现有标定技术相比，算法简单，效率较高。不仅可以得到陀螺仪、加速度计的零偏和比例因子误差，而且实现了磁力计的软、硬磁效应标定，并将磁力计坐标系与惯性传感器坐标系对齐。本发明可以在无专业设备工具、不依赖外部信息的情况下，通过简单的标定动作可以获取较为准确的传感器标定结果。

Description

一种九轴传感器的误差参数标定方法

技术领域

本发明属于微机电系统技术领域，特别涉及一种九轴传感器的误差标定方法。

背景技术

微机电系统MEMS(Micro-Electro Mechanical Systems)具有尺寸小、重量轻、可靠性高等优点，近年来，随着MEMS技术的发展，MEMS传感器被广泛应用于手机、游戏设备等。由于制造工艺、加工精度等的影响，MEMS传感器会存在零偏、比例因子等误差，并且这些误差会随着温度、时间发生变化。虽然在出厂时厂家给了传感器误差标定结果，但还会存在传感器误差，这些误差将会对设备的定位、定姿性能等产生较大影响。此外，磁力计由于软铁材料的影响使得磁力计坐标系发生畸变，导致磁强计和陀螺仪/加速度计之间的交叉畸变。

传统的九轴传感器标定算法将陀螺仪/加速度计和磁力计进行分开标定，效率低且操作复杂。因此，为提高其定姿、定位性能，简化标定过程，九轴传感器同时完成陀螺仪/加速度和磁力计的内在标定和交叉标定是一个亟需解决的问题。

综上，本发明基于卡尔曼滤波器，包括但不限于此应用场景，无需依赖姿态、重力信息，无需专业设备工具，提出一种简单有效的九轴传感器同时标定方法。

发明内容

针对九轴传感器的内在标定以及由于软磁效应造成磁力计轴系与陀螺仪/加速度计的交叉畸变，本发明提出了一种使用卡尔曼滤波器同时完成九轴传感器的标定方法，既可以快速实现标定陀螺仪/加速度计的零位偏置误差、比例因子误差，磁力计的零位偏置误差、交轴耦合误差、比例因子误差、软磁误差、硬磁误差，并可以实现磁力计与陀螺仪/加速度计轴系对齐。本发明无需借助任何外界设备，无需参数设置，简单可行，具有很好的普适性，同时能够满足现场快速(20s左右)标定的要求，具有良好的标定精度。

本发明的技术方案为一种九轴传感器的误差参数标定方法，具体包括如下步骤：

步骤1，以九轴传感器的测量中心为旋转中心，使其绕着传感器坐标系的X、Y、Z三轴分别旋转多圈；

步骤2，建立九轴传感器中陀螺仪、加速度计、磁力计的测量模型，确定标定参数，以方便构建误差状态卡尔曼滤波进行传感器标定；

步骤3，使用捷联惯导算法解算九轴传感器在标定过程中各个时刻的位置、速度及姿态，结合当地坐标系下磁场强度与磁力计原始观测值磁场强度的关系，构建卡尔曼滤波器中的系统误差状态方程；

步骤4，标定过程中，基于位置不变和单一位置下环境磁场不变的假设，构建卡尔曼滤波器的量测方程，完成传感器误差参数标定。

进一步的，步骤2中陀螺仪的测量模型构建如下，

对陀螺仪的测量模型进行建模时，考虑比例因子误差、零位偏置误差及传感器噪声，陀螺仪测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；

表示陀螺仪实际的角速度测量输出；ω^b表示陀螺仪在b系下的理论角速度；b_g为陀螺仪零偏；S_g为陀螺仪比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；w_g为陀螺仪传感器噪声，为高斯白噪声。

进一步的，步骤2中加速度计测量模型建模如下，

加速度计的测量模型进行建模时，考虑比例因子误差、零位偏置误差及传感器噪声，加速度计测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；

表示加速度计实际的加速度测量输出；f^b表示加速度计在b系下的理论加速度；b_a为加速度计零偏；S_a为加速度计比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；w_a为加速度计传感器噪声，为高斯白噪声。

进一步的，步骤2中磁力计测量模型建模如下，

对磁力计的测量模型进行建模时，考虑磁力计比例因子误差、交轴耦合误差、零位偏置误差及传感器噪声；磁力计的测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；上标m'表示校正过后的标准磁力计传感器坐标系；m为磁力计原始的不规则坐标系；m^b为当地磁场强度在载体坐标b系的真实投影分量，

为磁力计观测值，b_m为磁力计零偏，n_m为磁力计传感器噪声；

为磁力计标定坐标系与惯性传感器坐标系的旋转矩阵,

为磁力计标定矩阵，包含比例因子和交轴耦合w_m为高斯白噪声；

为简化模型，将

和

的乘积作为一个待估矩阵

其定义为磁力计原始的不规则坐标系到惯性传感器坐标系的转换矩阵，简化后的模型如下：

进一步的，步骤3的实现方式包括以下子步骤，

31)通过惯导算法和磁场预测算法建立系统状态方程

其中，由于MEMS九轴传感器器件精度低，地球自转对捷联惯导推算造成的误差影响很小，故不考虑地球自转，系统状态方程如下：

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

为第k个历元时n系下的位置向量；

为第k个历元时n系下的速度向量；

为第k个历元时b系到n系的方向余弦转换矩阵；

为第k个历元时磁场强度在n系的投影；

表示第k个历元加速度计的原始输出；gⁿ表示重力加速度在n系下的投影向量；Δt＝t_k-t_k-1为第k-1个第k个观测历元的时间间隔；

表示第k个历元陀螺仪计的原始输出；

为磁力计传感器观测坐标系到IMU传感器坐标系的转换矩阵；

为第k个历元时磁力计原始输出；b_m为磁力计零偏；(·×)为向量叉乘形式；

32)建立离散时间系统误差状态微分方程

卡尔曼滤波的状态向量包含导航误差状态和传感器误差，30维的连续误差状态向量定义如下：

式中，δx表示误差状态向量；δrⁿ、δvⁿ、Ψ分别为n系下位置误差、速度误差和姿态误差向量；δb_g、δb_a、δb_m分别为陀螺、加速度计、磁力计的零偏误差向量；δS_g、δS_a分别为陀螺、加速度计比例因子误差向量；

为b系到m系的转换矩阵误差；δmⁿ表示估计的n系磁场强度分量误差；δx中各个分量均为时间的函数，为方便书写，省略了时间变量t的书写，离散后的系统误差模型表示如下：

δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1 (6)

式中，下标k-1和k分别表示时刻t_k-1和t_k；Φ_k/k-1表示t_k-1到t_k时刻的状态一步转移矩阵；Γ_k-1为系统噪声驱动阵；w_k-1为系统激励噪声；

为得到离散后的系统误差模型，首先需要得到δx的连续时间微分方程，具体形式为：

式中，

表示各个状态向量误差的微分方程；F(t)为各个状态向量误差微分方程的系数矩阵；G(t)为驱动噪声系数矩阵；w(t)为各个状态向量误差的驱动噪声；

各个系统误差状态微分方程如下：

式中，

分别表示n系下位置误差、速度误差和姿态误差、陀螺仪零偏误差、加速度计零偏误差、陀螺仪比例因子误差、加速度计比例因子误差、磁力计零偏误差、磁力计转换矩阵误差、n系磁场强度分量误差的微分方程；δrⁿ、δvⁿ、Ψ分别为n系下位置误差、速度误差和姿态误差向量；δb_g、δb_a、δb_m分别为陀螺、加速度计、磁力计的零偏误差向量；δS_g、δS_a分别为陀螺、加速度计比例因子误差向量；

为b系到m系的转换矩阵误差；δmⁿ表示估计的n系磁场强度分量误差；w_a为加速度计传感器噪声，为高斯白噪声；w_g为陀螺仪传感器噪声，为高斯白噪声；τ_bg、τ_ba、τ_sg、τ_sa分别为陀螺仪零偏相关时间、加速度计零偏相关时间、陀螺比例因子相关时间、加速度计比例因子相关时间。w_bg、w_ba、w_sg、w_sa分别为陀螺仪零偏驱动白噪声、加速度计零偏驱动白噪声、陀螺比例因子驱动白噪声、加速度计比例因子驱动白噪声。

33)构建误差状态的系统误差状态方程

根据32)中得到的系统误差状态微分方程，得到系统误差状态方程的一步转移矩阵，即δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1中的Φ_k/k-1；

Φ_k/k-1＝exp{F(t_k-1)Δt}≈Ι+F(t_k)Δt (9)

式中，I为单位阵，至此，卡尔曼滤波器的系统误差状态方程建立完成。误差状态向量每次滤波反馈后都需要进行置零处理，因此不对误差状态向量预测，只对误差状态量的协方差进行预测。进一步的，步骤4的实现方式包括以下子步骤，

41)由于在标定过程中，九轴传感器绕着传感器中心点旋转，故假设位置保持不变，故以下等式恒成立：

式中，

为某一假定的固定位置；

为推算的位置，易得

式中，

为惯导实际推算得到的位置；

为假定的位置；n_r为位置观测噪声。

故位置观测方程如下：

δz^r＝H_rδx-n_r (12)

式中，H_r为位置观测方程的观测矩阵，具体形式为：

H_r＝[Ι₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃]

42)基于单一位置下环境磁场不变的假设，使用磁力计观测值构建量测方程，存在以下等式恒成立：

式中，i表示第i个历元，k为历元总数；

为0时刻地磁场的投影分量，

为第i个历元地磁场的投影分量；由此，可得预测的磁力计观测值为

式中，

为预测的磁力计观测值；

为b系到磁力计坐标的旋转矩阵与磁力计标定矩阵的乘积；

为n系到b系的姿态旋转矩阵；

为当地坐标系n系下的磁场投影分量；n_m为磁力计坐标系下磁力计传感器噪声；

对其进行扰动分析可得，

式中，m^m为磁力计的理论输出值；b_m为磁力计零偏；

为n系到磁力计坐标系m系的姿态旋转矩阵；mⁿ为n系下的磁场投影分量；Ψ为姿态误差向量；δb_m为磁力计零偏误差；Ι₃为3维的单位矩阵；

为克罗内克积；δmⁿ为n系下的磁场投影分量误差；

联立(14)、(15)两式，移项处理可得

基于观测值与预测值两者的差异为零向量的约束，故磁力计观测方程如下：

式中，n_m为磁力计坐标系下磁力计传感器噪声；H_m为磁力计观测方程的观测矩阵，具体形式为：

至此，完成九轴传感器的标定。

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

(1)本发明通过卡尔曼滤波器完成了九轴传感器的的内在标定及磁力计与陀螺仪/加速度计的交叉标定，可以有效补偿陀螺仪、加速度计、磁力计的传感器误差，标定效果较好，有效提升了定位定姿性能。

(2)本发明操作简单，易于实现，操作时间短。只需以九轴传感器的中心为旋转中心，绕着载体坐标系的X、Y、Z三轴各旋转几圈即可完成标定工作，对姿态无要求，无需依赖外部设备，标定效率高。

(3)本发明实现了将磁力计坐标系与惯性传感器坐标系对齐，使得磁力计和惯性传感器的信息放在一个公共框架中，可以进一步提升定位定姿性能。

附图说明

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

图2为使用卡尔曼滤波器进行传感器标定的流程图。

具体实施方式

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

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

步骤2，建立九轴传感器中陀螺仪、加速度计、磁力计的测量误差模型确定标定参数，以方便构建误差状态卡尔曼滤波进行传感器标定(本发明认为载体坐标系与惯性传感器坐标系轴系相同)。使用卡尔曼滤波完成传感器标定过程如图2所示。

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

21)对陀螺仪的原始观测模型进行建模。受制造工艺、加工精度等的影响，陀螺仪存在交轴耦合误差、比例因子误差、零位偏置误差及传感器噪声。考虑到交轴耦合误差难以在线标定，陀螺仪的原始观测模型不考虑交轴耦合误差。陀螺仪测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；

表示陀螺仪实际的角速度测量输出；ω^b表示陀螺仪在b系下的理论角速度；b_g为陀螺仪零偏；S_g为陀螺仪比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；w_g为陀螺仪传感器噪声，为高斯白噪声。

22)对加速度计的原始观测模型进行建模。受制造工艺、加工精度等的影响，加速度计存在交轴耦合误差、比例因子误差、零位偏置误差及传感器噪声。考虑到交轴耦合误差难以在线标定，加速度计的原始观测模型不考虑交轴耦合误差。加速度计测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；

表示加速度计实际的加速度测量输出；f^b表示加速度计在b系下的理论加速度；b_a为加速度计零偏；S_a为加速度计比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；w_a为加速度计传感器噪声，为高斯白噪声。

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

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；上标m'表示校正过后的标准磁力计传感器坐标系(由于磁力计受到软铁误差、交轴耦合误差、比例因子误差等，造成磁力计坐标系为不规则坐标系，需进行校正)；m为磁力计原始的不规则坐标系；m^b为当地磁场强度在载体坐标b系的真实投影分量，

为磁力计观测值，b_m为磁力计零偏，n_m为磁力计传感器噪声；

为磁力计标定坐标系与惯性传感器坐标系的旋转矩阵,

为磁力计标定矩阵(即软磁效应)，包含比例因子和交轴耦合w_m为高斯白噪声。

为简化模型，将

和

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

其定义为磁力计原始的不规则坐标系到惯性传感器坐标系的转换矩阵，简化后的模型如下：

步骤3，使用捷联惯导算法解算九轴传感器在标定过程中各个时刻的位置、速度及姿态，结合当地坐标系下磁场强度与磁力计原始观测值的关系，构建卡尔曼滤波器中的系统误差状态方程。

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

31)通过惯导算法和磁场预测算法建立系统状态方程。由于MEMS九轴传感器器件精度低，地球自转对捷联惯导推算造成的误差影响很小，故不考虑地球自转。系统状态方程如下：

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

为第k个历元时n系下的位置向量；

为第k个历元时n系下的速度向量；

为第k个历元时b系到n系的方向余弦转换矩阵；

为第k个历元时磁场强度在n系的投影；

表示第k个历元加速度计的原始输出；gⁿ表示重力加速度在n系下的投影向量；Δt＝t_k-t_k-1为第k-1个第k个观测历元的时间间隔；

表示第k个历元陀螺仪计的原始输出；

为磁力计传感器观测坐标系到IMU传感器坐标系的转换矩阵；

为第k个历元时磁力计原始输出；b_m为磁力计零偏；(·×)为向量叉乘形式。

32)建立离散时间系统误差状态微分方程。

为解决系统非线性问题，常采用误差状态卡尔曼滤波来处理。卡尔曼滤波的状态向量包含导航误差状态和传感器误差，30维的连续误差状态向量定义如下：

式中，δx表示误差状态向量；δrⁿ、δvⁿ、Ψ分别为n系下位置误差、速度误差和姿态误差向量；δb_g、δb_a、δb_m分别为陀螺、加速度计、磁力计的零偏误差向量；δS_g、δS_a分别为陀螺、加速度计比例因子误差向量；

为b系到m系的转换矩阵误差；δmⁿ表示估计的n系磁场强度分量误差。δx中各个分量均为时间的函数，为方便书写，省略了时间变量t的书写。离散后的系统误差模型表示如下：

δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1 (6)

式中，下标k-1和k分别表示时刻t_k-1和t_k；Φ_k/k-1表示t_k-1到t_k时刻的状态一步转移矩阵；Γ_k-1为系统噪声驱动阵；w_k-1为系统激励噪声。

为得到离散后的系统误差模型，首先需要得到δx的连续时间微分方程，具体形式为：

式中，

表示各个状态向量误差的微分方程；F(t)为各个状态向量误差微分方程的系数矩阵；G(t)为驱动噪声系数矩阵；w(t)为各个状态向量误差的驱动噪声。

各个系统误差状态微分方程如下：

式中，

分别表示n系下位置误差、速度误差和姿态误差、陀螺仪零偏误差、加速度计零偏误差、陀螺仪比例因子误差、加速度计比例因子误差、磁力计零偏误差、磁力计转换矩阵误差、n系磁场强度分量误差的微分方程；δrⁿ、δvⁿ、Ψ分别为n系下位置误差、速度误差和姿态误差向量；δb_g、δb_a、δb_m分别为陀螺、加速度计、磁力计的零偏误差向量；δS_g、δS_a分别为陀螺、加速度计比例因子误差向量；

为b系到m系的转换矩阵误差；δmⁿ表示估计的n系磁场强度分量误差；w_a为加速度计传感器噪声，为高斯白噪声；w_g为陀螺仪传感器噪声，为高斯白噪声；τ_bg、τ_ba、τ_sg、τ_sa分别为陀螺仪零偏相关时间、加速度计零偏相关时间、陀螺比例因子相关时间、加速度计比例因子相关时间。w_bg、w_ba、w_sg、w_sa分别为陀螺仪零偏驱动白噪声、加速度计零偏驱动白噪声、陀螺比例因子驱动白噪声、加速度计比例因子驱动白噪声。

33)构建误差状态的系统误差状态方程。

根据32)中得到的系统误差状态微分方程，得到系统误差状态方程的一步转移矩阵，即δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1中的Φ_k/k-1。

Φ_k/k-1＝exp{F(t_k-1)Δt}≈Ι+F(t_k)Δt (9)

式中，I为单位阵。至此，卡尔曼滤波器的系统误差状态方程建立完成。每次滤波反馈后都需要进行置零处理，因此不对误差状态向量预测，只对误差状态量的协方差进行预测。

步骤4，通过位置不变约束和固定位置磁场投影不变约束，构建卡尔曼滤波器的量测方程，通过卡尔曼滤波器不断的预测和更新，完成对九轴传感器进行标定。

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

41)由于在标定过程中，九轴传感器绕着传感器中心点旋转，故假设位置保持不变，故以下等式恒成立：

式中，

为某一假定位置；

为推算的位置，易得

式中，

为实际推算得到的位置；

为假定的位置；n_r为位置观测噪声。

故位置观测方程如下：

δz^r＝H_rδx-n_r (12)

式中，H_r为位置观测方程的观测矩阵，具体形式为：

H_r＝[Ι₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃]

42)本发明基于单一位置下环境磁场不变的假设，使用磁力计观测值构建量测方程，存在以下等式恒成立：

式中，i表示第i个历元，k为历元总数；

为0时刻地磁场的投影分量，

为第i个历元地磁场的投影分量。由此，可得预测的磁力计观测值为

式中，

为预测的磁力计观测值；

为b系到磁力计坐标的旋转矩阵与磁力计标定矩阵的乘积；

为n系到b系的姿态旋转矩阵；

为当地坐标系n系下的磁场投影分量；n_m为磁力计坐标系下磁力计传感器噪声。

对其进行扰动分析可得，

式中，m^m为磁力计的理论输出值；b_m为磁力计零偏；

为n系到磁力计坐标系m系的姿态旋转矩阵；mⁿ为n系下的磁场投影分量；Ψ为姿态误差向量；δb_m为磁力计零偏误差；Ι₃为3维的单位矩阵；

为克罗内克积；δmⁿ为n系下的磁场投影分量误差。

联立(14)、(15)两式，移项处理可得

基于观测值与预测值两者的差异为零向量的约束，故磁力计观测方程如下：

式中，n_m为磁力计坐标系下磁力计传感器噪声；H_m为磁力计观测方程的观测矩阵，具体形式为

至此，完成九轴传感器的标定。

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

Claims (6)

1.一种九轴传感器的误差参数标定方法，其特征在于，包括如下步骤：

步骤1，以九轴传感器的测量中心为旋转中心，使其绕着传感器坐标系的X、Y、Z三轴分别旋转多圈；

步骤2，建立九轴传感器中陀螺仪、加速度计、磁力计的测量模型，确定需要标定的参数；

步骤3，使用捷联惯导算法解算九轴传感器在标定过程中各个时刻的位置、速度及姿态，结合当地坐标系下磁场强度与磁力计原始观测值的关系，构建卡尔曼滤波器中的系统误差状态方程；

步骤4，标定过程中，基于位置不变和单一位置下环境磁场不变的假设，构建卡尔曼滤波器的量测方程，完成传感器误差参数标定。

2.根据权利要求1所述的一种九轴传感器的误差参数标定方法，其特征在于：步骤2中陀螺仪的测量模型构建如下，

对陀螺仪的测量模型进行建模时，考虑比例因子误差、零位偏置误差及传感器噪声，陀螺仪测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；

表示陀螺仪实际的角速度测量输出；ω^b表示陀螺仪在b系下的理论角速度；b_g为陀螺仪零偏；S_g为陀螺仪比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；w_g为陀螺仪传感器噪声，为高斯白噪声。

3.根据权利要求1所述的一种九轴传感器的误差参数标定方法，其特征在于：步骤2中加速度计测量模型建模如下，

加速度计的测量模型进行建模时，考虑比例因子误差、零位偏置误差及传感器噪声，加速度计测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；

表示加速度计实际的加速度测量输出；f^b表示加速度计在b系下的理论加速度；b_a为加速度计零偏；S_a为加速度计比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；w_a为加速度计传感器噪声，为高斯白噪声。

4.根据权利要求1所述的一种九轴传感器的误差参数标定方法，其特征在于：步骤2中磁力计测量模型建模如下，

对磁力计的测量模型进行建模时，考虑磁力计比例因子误差、交轴耦合误差、零位偏置误差及传感器噪声；磁力计的测量模型如下：

式中，上标b表示载体坐标系b系，b系以惯性传感器的测量中心为原点，x轴指向平均春分日点，z轴平行于地球自转轴且指向北极点，y轴与x轴和z轴正交并构成右手系，其中惯性传感器包含陀螺仪和加速度计，陀螺仪和加速度计的坐标原点与惯性传感器测量中心重合且轴向一致；上标m'表示校正过后的标准磁力计传感器坐标系；m为磁力计原始的不规则坐标系；m^b为当地磁场强度在载体坐标b系的真实投影分量，

为磁力计观测值，b_m为磁力计零偏，n_m为磁力计传感器噪声；

为磁力计标定坐标系与惯性传感器坐标系的旋转矩阵,

为磁力计标定矩阵，包含比例因子和交轴耦合w_m为高斯白噪声；

为简化模型，将

和

的乘积作为一个待估矩阵

其定义为磁力计原始的不规则坐标系到惯性传感器坐标系的转换矩阵，简化后的模型如下：

5.根据权利要求1所述的一种九轴传感器的误差参数标定方法，其特征在于：步骤3的实现方式包括以下子步骤，

31)惯导算法和磁场预测算法建立系统状态方程

其中，由于MEMS九轴传感器器件精度低，地球自转对捷联惯导推算造成的误差影响很小，故惯导算法中不考虑地球自转，惯导算法和磁场预测算法具体如下：

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

为第k个历元时n系下的位置向量；

为第k个历元时n系下的速度向量；

为第k个历元时b系到n系的方向余弦转换矩阵；

为第k个历元时磁场强度在n系的投影；

表示第k个历元加速度计的原始输出；gⁿ表示重力加速度在n系下的投影向量；Δt＝t_k-t_k-1为第k-1个第k个观测历元的时间间隔；

表示第k个历元陀螺仪计的原始输出；

为磁力计传感器观测坐标系到IMU传感器坐标系的转换矩阵；

为第k个历元时磁力计原始输出；b_m为磁力计零偏；(·×)为向量叉乘形式；

32)建立离散时间系统误差状态微分方程

卡尔曼滤波的状态向量包含导航误差状态和传感器误差，30维的连续误差状态向量定义如下：

式中，δx表示状态向量误差；δrⁿ、δvⁿ、Ψ分别为n系下位置误差、速度误差和姿态误差向量；δb_g、δb_a、δb_m分别为陀螺、加速度计、磁力计的零偏误差向量；δS_g、δS_a分别为陀螺、加速度计比例因子误差向量；

为b系到m系的转换矩阵误差；δmⁿ表示估计的n系磁场强度分量误差；δx中各个分量均为时间的函数，为方便书写，省略了时间变量t的书写，离散后的系统误差模型表示如下：

δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1 (6)

式中，下标k-1和k分别表示时刻t_k-1和t_k；Φ_k/k-1表示t_k-1到t_k时刻的状态一步转移矩阵；Γ_k-1为系统噪声驱动阵；w_k-1为系统激励噪声；

为得到离散后的系统误差模型，首先需要得到δx的连续时间微分方程，具体形式为：

式中，

表示各个误差状态向量的微分方程；F(t)为各个状态向量误差微分方程的系数矩阵；G(t)为驱动噪声系数矩阵；w(t)为各个状态向量误差的驱动噪声；

各个系统误差状态微分方程如下：

式中，

分别表示n系下位置误差、速度误差和姿态误差、陀螺仪零偏误差、加速度计零偏误差、陀螺仪比例因子误差、加速度计比例因子误差、磁力计零偏误差、磁力计转换矩阵误差、n系磁场强度分量误差的微分方程；δrⁿ、δvⁿ、Ψ分别为n系下位置误差、速度误差和姿态误差向量；δb_g、δb_a、δb_m分别为陀螺、加速度计、磁力计的零偏误差向量；δS_g、δS_a分别为陀螺、加速度计比例因子误差向量；

为b系到m系的转换矩阵误差；δmⁿ表示估计的n系磁场强度分量误差；w_a为加速度计传感器噪声，为高斯白噪声；w_g为陀螺仪传感器噪声，为高斯白噪声；τ_bg、τ_ba、τ_sg、τ_sa分别为陀螺仪零偏相关时间、加速度计零偏相关时间、陀螺比例因子相关时间、加速度计比例因子相关时间；w_bg、w_ba、w_sg、w_sa分别为陀螺仪零偏驱动白噪声、加速度计零偏驱动白噪声、陀螺比例因子驱动白噪声、加速度计比例因子驱动白噪声；

33)构建误差状态的系统误差状态方程

根据32)中得到的系统误差状态微分方程，得到系统误差状态方程的一步转移矩阵，即δx_k＝Φ_k/k-1δx_k-1/k-1+Γ_k-1w_k-1中的Φ_k/k-1；

Φ_k/k-1＝exp{F(t_k-1)Δt}≈Ι+F(t_k)Δt (9)

式中，I为单位阵，至此，卡尔曼滤波器的系统误差状态方程建立完成；误差状态向量每次滤波反馈后都需要进行置零处理，因此不对误差状态向量预测，只对误差状态量的协方差进行预测。

6.根据权利要求5所述的一种九轴传感器的误差参数标定方法，其特征在于：步骤4的实现方式包括以下子步骤，

41)由于在标定过程中，九轴传感器绕着传感器中心点旋转，故假设位置保持不变，故以下等式恒成立：

式中，

为某一假定的固定位置；

为推算的位置，易得

式中，

为实际推算得到的位置；

为假定的位置；n_r为位置观测噪声；

故位置观测方程如下：

δz^r＝H_rδx-n_r (12)

式中，H_r为位置观测方程的观测矩阵，具体形式为：

H_r＝[Ι₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃ 0₃]

42)基于单一位置下环境磁场不变的假设，使用磁力计观测值构建量测方程，存在以下等式恒成立：

式中，i表示第i个历元，k为历元总数；

为0时刻地磁场的投影分量，

为第i个历元地磁场的投影分量；由此，可得预测的磁力计观测值为

式中，

为预测的磁力计观测值；

为b系到磁力计坐标的旋转矩阵与磁力计标定矩阵的乘积；

为n系到b系的姿态旋转矩阵；

为当地坐标系n系下的磁场投影分量；n_m为磁力计坐标系下磁力计传感器噪声；

对式(14)进行扰动分析可得，

式中，m^m为磁力计的理论输出值；b_m为磁力计零偏；

为n系到磁力计坐标系m系的姿态旋转矩阵；mⁿ为n系下的磁场投影分量；Ψ为姿态误差向量；δb_m为磁力计零偏误差；Ι₃为3维的单位矩阵；

为克罗内克积；δmⁿ为n系下的磁场投影分量误差；

联立(14)、(15)两式，移项处理可得

基于观测值与预测值两者的差异为零向量的约束，故磁力计观测方程如下：

式中，n_m为磁力计坐标系下磁力计传感器噪声；H_m为磁力计观测方程的观测矩阵，具体形式为：

至此，完成九轴传感器的标定。

Images