CN105180968A - 一种imu/磁强计安装失准角在线滤波标定方法 - Google Patents

Abstract

本发明公开了一种IMU/磁强计安装失准角在线滤波标定方法，属于惯性技术领域。本发明采用三位置路径设计，包括转动轴、转动顺序和转动角度等；应用卡尔曼滤波方法得到捷联惯导系统IMU相对磁强计安装失准角全部误差参数；利用六面体或其它相似的可翻转装置即可完成现场标定试验，克服了传统实验室标定的不足，提高了系统实际使用精度。

Description

一种IMU/磁强计安装失准角在线滤波标定方法

技术领域

本发明属于惯性技术领域，涉及一种IMU/磁强计安装失准角的在线标定方法，可在使用现场没有转台和地理基准的情况下标定出IMU与磁强计安装失准角参数。

背景技术

磁强计是测量磁场强度的传感器，在组合导航中应用广泛，具有隐蔽性能好、即开即用、误差不随时间积累等特点。捷联惯性导航系统（SINS）与磁强计组合系统为飞行器姿态测量及控制提供磁航向信息，可以弥补惯性导航长期误差积累的不足，越来越多地被应用于导航技术中。由于磁强计受到安装误差和外界干扰的影响大，因而引起的航向误差相对较大。虽然针对误差补偿技术的方法研究较多，但都存在着不足。

在导航应用中，地磁场信息用来测量飞行器航向，在使用磁强计与惯性导航系统组合之前，必须先对磁强计相对于惯组的安装失准角进行估计。在磁强计标定完成之后，结合惯导系统导航信息，采用三位置方法估计安装失准角。结果证明，该方法效果良好，方便可靠。

对于惯性测量单元（IMU）与磁强计安装失准角的估计，目前在工程上应用最为广泛的是基于转台的实验室标定方法，该方法对转台设备的依赖性较高，一般只能在实验室进行，但是能标定出系统较为全面的误差参数，是SINS/磁强计组合导航系统投入使用的重要前提。

参考文献[1]（Calibrationofamagnetometerincombinationwithinertialsensors[J]）本文也对IMU与磁强计之间的安装失准角进行估计，利用加速度计测量的重力场矢量，与磁强计测得的地磁场矢量，它们的内积为一常数，本文依此应用ML法对安装失准角进行了估计。但是该方法计算出来的安装失准角严格来说是相对于加速度计轴向的，而且本方法参数估计的精度取决于相关参数初始值的估计。

发明内容

本发明的目的在于提供一种磁强计相对于惯组的安装失准角在线标定的方法，减小磁强计/IMU组合导航误差，并提高其实际使用精度。本发明采用三位置路径设计，包括转动轴、转动顺序和转动角度等；应用卡尔曼滤波方法得到捷联惯导系统相对磁强计安装失准角全部误差参数；利用六面体或其它相似的可翻转装置即可完成现场标定试验。具体方法步骤如下：

第一步：将IMU和磁强计通过工装固定在六面体装置上，锁紧。连接系统、电源、采集计算机之间的线缆，并检查正确；

第二步：将六面体装置置于桌面或平整的地面上，手动调整六面体装置，上电预热使系统达到热平衡状态；

第三步：向SINS/磁强计系统装订标定现场的地理参数，包括初始的经度、纬度和高度，然后捷联惯导系统进行1min的静态初始对准，以获取导航解算的初始姿态角，并采集磁强计的数据；

第四步：将SINS/磁强计系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据；

第五步：再次将SINS/磁强计系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据。

磁强计\IMU安装失准角在线滤波法估计包含以下几个步骤：

步骤1：建立惯性器件误差模型；

惯性器件误差模型包括光纤陀螺误差模型和加速度计误差模型，分别对应如下：

式中为陀螺仪的误差输出矢量；为由陀螺误差引起的误差角速度。分别表示三轴陀螺测量值；分别表示三轴陀螺仪标度因数误差；分别表示陀螺仪各轴间安装误差角；分别表示陀螺仪三轴零偏误差；

式中为加速度计的误差输出矢量；分别表示由加速度计误差引起的误差加速度；分别表示三轴加速度计测量值；分别为三轴标度因数误差；分别为加速度计三轴零偏；分别表示加速度计各轴间安装误差角。

步骤2：地磁场强度归一化处理

通过查询当地地磁模型可以得到磁倾角、磁偏角和地理坐标系下各方向磁场强度分量，而通过磁强计椭球拟合标定得到的模型，其补偿后的磁场测量值大小与当地磁场真实强度存在比例关系，因此，需要先将拟合后的磁场强度和当地磁场模型一同进行归一化处理，再进行安装失准角的估计。即：。

步骤3：建立磁强计\IMU安装失准角模型

基本思路是经过标定补偿后的磁强计的输出，结合地磁模型辅助补偿惯导姿态角误差，同时得到惯组与磁强计的安装误差；

其中b系为惯导载体坐标系，为磁强计坐标系，n系为导航坐标系，系为计算得到的导航坐标系，φ为姿态角，μ为安装失准角

记；；

易知rank(A)=rank(A|b)=2<3;方程有解析，没有唯一解；X初值任意取时只能估计出一个参数，当X初值取值和设定的参数值一致时，才能解出全部参数；通过不同位置提高可观测度；两位置即可全部解出全部参数，得到正确的参数。

步骤4：建立卡尔曼滤波器模型

选取地理坐标系东北天为导航坐标系，建立系统状态方程和量测方程分别如下：

式中表示系统状态的微分，表示状态矩阵、表示系统状态向量、表示系统噪声、表示系统量测矢量、表示观测矩阵、表示量测噪声矢量；

其中，系统状态向量，表示姿态角误差,表示俯仰角误差、表示横滚角误差、表示航向角误差；表示速度误差，表示东向速度误差、北向速度误差、表示天向速度误差。表示位置误差，表示纬度误差、表示经度误差，表示高度误差。表示陀螺标定参数误差、分别表示加速度计标定参数误差；表示IMU与磁强计之间的安装失准角；

分别表示三轴陀螺仪标度因数误差；、、、、、分别表示陀螺仪各轴间安装误差角；分别表示陀螺仪三轴零偏误差；分别为三轴标度因数误差；分别表示三轴加速度计零偏；分别表示加速度计各轴间安装误差角；

状态矩阵

式中表示地球自转角速率；表示系统所在位置的地理纬度；表示系统所在位置的海拔高度；表示当地子午面主曲率半径；表示当地卯酉面主曲率半径；、和分别表示系统东向、北向和天向速度；、和分别表示导航系下系统的比力信息；、和分别表示三轴陀螺测量值；分别表示各轴向陀螺测量值；

系统噪声，分别表示三轴陀螺在导航系下的零均值白噪声，分别表示三轴加速度计在导航系下的零均值白噪声；

由

得

卡尔曼滤波器对状态量建模时，是一个常量，故；

其观测量取：

其量测矩阵取：

系统量测值，式中为系统输出的东北天速度信息，为速度观测信息，静止状态下为0，为系统输出的纬度、经度和高度信息，为位置观测信息，静止状态下位初始位置信息，为当地的磁场分量，根据地磁模型可得磁偏角和磁倾角，为磁强计测得的磁场强度，为导航解算得到的带有姿态误差的捷联矩阵；系统观测矩阵，式中表示六阶单位阵。

步骤5：对系统状态方程进行离散化

采用泰勒级数展开：

其中为状态一步转移矩阵，为30阶单位阵，为状态转移矩阵，为滤波周期；

系统模型噪声的方差为：

其中为离散系统噪声方差强度阵，为连续系统噪声方差阵，为状态转移矩阵。

步骤6：进行卡尔曼滤波估计

第k+1时刻的量测值为，则的卡尔曼滤波估计值按下述方程求解：

获取系统状态向量的一步预测：

获取预测误差的方差阵：

获取卡尔曼滤波增益：

获取系统状态估计值：

获取系统的状态估计误差方差：

卡尔曼滤波采用闭环校正，估计结果有姿态误差、速度误差、位置误差、陀螺误差、加速度计误差和，利用对导航解算姿态、速度和位置进行校正，利用、对原惯性器件测量值进行校正，利用导航解算提供的姿态对进行在线滤波估计。

第六步：最后一个位置卡尔曼滤波修正结束时得到惯导系统精确姿态和IMU与磁强计之间安装失准角的估计值。

本发明的有益效果在于：

本发明所提出的方法可以在不需要任何平台基准的情况下，现场完成IMU与磁强计安装失准角的估计，克服了传统实验室标定的不足，提高了系统实际使用精度。

附图说明

图1为本发明提供的基于卡尔曼滤波的IMU/磁强计安装失准角在线标定方法流程图；

图2为本发明实施例中转动过程姿态角误差及其卡尔曼滤波估计误差的方差收敛曲线；

图3为本发明实施例中转动过程安装失准角及其卡尔曼滤波估计误差的方差收敛曲线。

具体实施方式

下面结合附图和实施例对本发明进行详细说明。

本发明提供一种IMU/磁强计安装失准角在线滤波标定方法，如图1所示，所述方法包括如下步骤：

第一步：将IMU和磁强计通过工装固定在六面体装置上，锁紧。连接系统、电源、采集计算机之间的线缆，并检查正确；

第二步：将六面体装置置于桌面或平整的地面上，手动调整六面体装置，上电预热使系统达到热平衡状态；

第三步：向SINS/磁强计系统装订标定现场的地理参数，包括初始的经度、纬度和高度，然后捷联惯导系统进行1min的静态初始对准，以获取导航解算的初始姿态角，并采集磁强计的数据；

第四步：将SINS/磁强计系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据；

第五步：再次将SINS/磁强计系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据。

磁强计\IMU安装失准角在线滤波法估计包含以下几个步骤：

步骤1：建立惯性器件误差模型

惯性器件误差模型包括光纤陀螺误差模型和加速度计误差模型，分别对应如下：

式中为陀螺仪的误差输出矢量；为由陀螺误差引起的误差角速度。分别表示三轴陀螺测量值；分别表示三轴陀螺仪标度因数误差；分别表示陀螺仪各轴间安装误差角；分别表示陀螺仪三轴零偏误差；

式中为加速度计的误差输出矢量；分别表示由加速度计误差引起的误差加速度；分别表示三轴加速度计测量值；分别为三轴标度因数误差；分别为加速度计三轴零偏；分别表示加速度计各轴间安装误差角。

步骤2：地磁场强度归一化处理

通过查询当地地磁模型可以得到磁倾角、磁偏角和地理坐标系下各方向磁场强度分量，而通过磁强计椭球拟合标定得到的模型，其补偿后的磁场测量值大小与当地磁场真实强度存在比例关系，因此，需要先将拟合后的磁场强度和当地磁场模型一同进行归一化处理，再进行安装失准角的估计。即：。

步骤3：建立磁强计\IMU安装失准角模型

基本思路是经过标定补偿后的磁强计的输出，结合地磁模型辅助补偿惯导姿态角误差，同时得到惯组与磁强计的安装误差；

其中b系为惯导载体坐标系，为磁强计坐标系，n系为导航坐标系，系为计算得到的导航坐标系，φ为姿态角，μ为安装失准角

记；；

易知rank(A)=rank(A|b)=2<3;方程有解析，没有唯一解；X初值任意取时只能估计出一个参数，当X初值取值和设定的参数值一致时，才能解出全部参数；通过不同位置提高可观测度；两位置即可全部解出全部参数，得到正确的参数。

步骤4：建立卡尔曼滤波器模型

选取地理坐标系东北天为导航坐标系，建立系统状态方程和量测方程分别如下：

式中表示系统状态的微分，表示状态矩阵、表示系统状态向量、表示系统噪声、表示系统量测矢量、表示观测矩阵、表示量测噪声矢量；

其中，系统状态向量，表示姿态角误差,表示俯仰角误差、表示横滚角误差、表示航向角误差；表示速度误差，表示东向速度误差、北向速度误差、表示天向速度误差。表示位置误差，表示纬度误差、表示经度误差，表示高度误差。表示陀螺标定参数误差、分别表示加速度计标定参数误差。表示IMU与磁强计之间的安装失准角；

分别表示三轴陀螺仪标度因数误差；、、、、、分别表示陀螺仪各轴间安装误差角；分别表示陀螺仪三轴零偏误差；分别为三轴标度因数误差；分别表示三轴加速度计零偏；分别表示加速度计各轴间安装误差角；

状态矩阵

式中表示地球自转角速率；表示系统所在位置的地理纬度；表示系统所在位置的海拔高度；表示当地子午面主曲率半径；表示当地卯酉面主曲率半径；、和分别表示系统东向、北向和天向速度；、和分别表示导航系下系统的比力信息；、和分别表示三轴陀螺测量值；分别表示各轴向陀螺测量值；

系统噪声，分别表示三轴陀螺在导航系下的零均值白噪声，分别表示三轴加速度计在导航系下的零均值白噪声；

由

得

卡尔曼滤波器对状态量建模时，是一个常量，故；

其观测量取：

其量测矩阵取：

系统量测值，式中为系统输出的东北天速度信息，为速度观测信息，静止状态下为0，为系统输出的纬度、经度和高度信息，为位置观测信息，静止状态下位初始位置信息，为当地的磁场分量，根据地磁模型可得磁偏角和磁倾角，为磁强计测得的磁场强度，为导航解算得到的带有姿态误差的捷联矩阵；系统观测矩阵，式中表示六阶单位阵。

步骤5：对系统状态方程进行离散化

采用泰勒级数展开：

其中为状态一步转移矩阵，为30阶单位阵，为状态转移矩阵，为滤波周期；

系统模型噪声的方差为：

其中为离散系统噪声方差强度阵，为连续系统噪声方差阵，为状态转移矩阵。

步骤6：进行卡尔曼滤波估计

第k+1时刻的量测值为，则的卡尔曼滤波估计值按下述方程求解：

获取系统状态向量的一步预测：

获取预测误差的方差阵：

获取卡尔曼滤波增益：

获取系统状态估计值：

获取系统的状态估计误差方差：

卡尔曼滤波采用闭环校正，估计结果有姿态误差、速度误差、位置误差、陀螺误差、加速度计误差和，利用对导航解算姿态、速度和位置进行校正，利用、对原惯性器件测量值进行校正，利用导航解算提供的姿态对进行在线滤波估计。

第六步：最后一个位置卡尔曼滤波修正结束时得到惯导系统精确姿态和IMU与磁强计之间安装失准角的估计值。

实施例

第一步：将IMU和磁强计通过工装固定在六面体装置上，锁紧。连接系统、电源、采集计算机之间的线缆，并检查正确；

第二步：将六面体装置置于平稳桌面上，上电预热使系统达到热平衡状态，并装订系统的初始位置参数，包括初始的经度、纬度和高度；

第三步：静置六面体系统装置，采用静态精对准1min，获取系统的初始姿态角；

第四步：将系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据；

第五步：再次将系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据；

第六步：最后一次卡尔曼滤波修正，估计得到的IMU与磁强计的安装失准角，即为现场标定结果。

结果分析：

卡尔曼滤波修正过程中，姿态角误差及其估计误差的方差收敛曲线如图2所示，安装失准角及其误差的方差收敛曲线如图3所示。从图2和图3可以看出，随着系统的连续转动，所有参数估计误差的方差值逐渐收敛接近零，参数估计值渐近收敛接近至真值。卡尔曼滤波修正最终参数估计值，即现场标定结果，具体值如表1所示：

表1在线标定结果

可得到如下分析结论：在仅采用六面体装置的环境下，本文设计的连续转动路径和卡尔曼滤波器，能有效标定出系统IMU/磁强计的安装失准角参数，可提高组合导航系统实际使用精度。

Claims (1)

1.一种IMU/磁强计安装失准角在线滤波标定方法，其特征在于通过如下步骤实现：

第一步：将IMU和磁强计通过工装固定在六面体装置上，锁紧连接系统、电源、采集计算机之间的线缆，并检查正确；

第二步：将六面体装置置于桌面或平整的地面上，手动调整六面体装置，上电预热使系统达到热平衡状态；

第三步：向SINS/磁强计系统装订标定现场的地理参数，包括初始的经度、纬度和高度，然后捷联惯导系统进行1min的静态初始对准，以获取导航解算的初始姿态角，并采集磁强计的数据；

第四步：将SINS/磁强计系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据；

第五步：再次将SINS/磁强计系统沿Z轴转动大约90度，对系统进行20s的静态初始对准，同时采集磁强计的数据；

磁强计\IMU安装失准角在线滤波法估计包含以下几个步骤：

步骤1：建立惯性器件误差模型；

惯性器件误差模型包括光纤陀螺误差模型和加速度计误差模型，分别对应如下：

式中为陀螺仪的误差输出矢量；为由陀螺误差引起的误差角速度；分别表示三轴陀螺测量值；分别表示三轴陀螺仪标度因数误差；分别表示陀螺仪各轴间安装误差角；分别表示陀螺仪三轴零偏误差；

式中为加速度计的误差输出矢量；分别表示由加速度计误差引起的误差加速度；分别表示三轴加速度计测量值；分别为三轴标度因数误差；分别为加速度计三轴零偏；分别表示加速度计各轴间安装误差角；

步骤2：地磁场强度归一化处理

通过查询当地地磁模型可以得到磁倾角、磁偏角和地理坐标系下各方向磁场强度分量，而通过磁强计椭球拟合标定得到的模型，其补偿后的磁场测量值大小与当地磁场真实强度存在比例关系，因此，需要先将拟合后的磁场强度和当地磁场模型一同进行归一化处理，再进行安装失准角的估计；

即：；

步骤3：建立磁强计\IMU安装失准角模型

基本思路是经过标定补偿后的磁强计的输出，结合地磁模型辅助补偿惯导姿态角误差，同时得到惯组与磁强计的安装误差；

其中b系为惯导载体坐标系，为磁强计坐标系，n系为导航坐标系，系为计算得到的导航坐标系，φ为姿态角，μ为安装失准角

记；；

易知rank(A)=rank(A|b)=2<3;方程有解析，没有唯一解；X初值任意取时只能估计出一个参数，当X初值取值和设定的参数值一致时，才能解出全部参数；通过不同位置提高可观测度；两位置即可全部解出全部参数，得到正确的参数；

步骤4：建立卡尔曼滤波器模型

选取地理坐标系东北天为导航坐标系，建立系统状态方程和量测方程分别如下：

式中表示系统状态的微分，表示状态矩阵、表示系统状态向量、表示系统噪声、表示系统量测矢量、表示观测矩阵、表示量测噪声矢量；

其中，系统状态向量，表示姿态角误差,表示俯仰角误差、表示横滚角误差、表示航向角误差；表示速度误差，表示东向速度误差、北向速度误差、表示天向速度误差；表示位置误差，表示纬度误差、表示经度误差，表示高度误差；表示陀螺标定参数误差、分别表示加速度计标定参数误差；

表示IMU与磁强计之间的安装失准角；

分别表示三轴陀螺仪标度因数误差；、、、、、分别表示陀螺仪各轴间安装误差角；分别表示陀螺仪三轴零偏误差；分别为三轴标度因数误差；分别表示三轴加速度计零偏；分别表示加速度计各轴间安装误差角；

状态矩阵

式中表示地球自转角速率；表示系统所在位置的地理纬度；表示系统所在位置的海拔高度；表示当地子午面主曲率半径；表示当地卯酉面主曲率半径；、和分别表示系统东向、北向和天向速度；、和分别表示导航系下系统的比力信息；、和分别表示三轴陀螺测量值；分别表示各轴向陀螺测量值；

系统噪声，分别表示三轴陀螺在导航系下的零均值白噪声，分别表示三轴加速度计在导航系下的零均值白噪声；

由

得

卡尔曼滤波器对状态量建模时，是一个常量，故；

其观测量取：

其量测矩阵取：

系统量测值，式中为系统输出的东北天速度信息，为速度观测信息，静止状态下为0，为系统输出的纬度、经度和高度信息，为位置观测信息，静止状态下位初始位置信息，为当地的磁场分量，根据地磁模型可得磁偏角和磁倾角，为磁强计测得的磁场强度，为导航解算得到的带有姿态误差的捷联矩阵；系统观测矩阵，式中表示六阶单位阵；

步骤5：对系统状态方程进行离散化

采用泰勒级数展开：

其中为状态一步转移矩阵，为30阶单位阵，为状态转移矩阵，为滤波周期；

系统模型噪声的方差为：

其中为离散系统噪声方差强度阵，为连续系统噪声方差阵，为状态转移矩阵；

步骤6：进行卡尔曼滤波估计

第k+1时刻的量测值为，则的卡尔曼滤波估计值按下述方程求解：

获取系统状态向量的一步预测：

获取预测误差的方差阵：

获取卡尔曼滤波增益：

获取系统状态估计值：

获取系统的状态估计误差方差：

卡尔曼滤波采用闭环校正，估计结果有姿态误差、速度误差、位置误差、陀螺误差、加速度计误差和，利用对导航解算姿态、速度和位置进行校正，利用、对原惯性器件测量值进行校正，利用导航解算提供的姿态对进行在线滤波估计；

第六步：最后一个位置卡尔曼滤波修正结束时得到惯导系统精确姿态和IMU与磁强计之间安装失准角的估计值。