CN107860382A - 一种在地磁异常情况下应用ahrs测量姿态的方法 - Google Patents

Abstract

本发明涉及一种在地磁异常情况下应用AHRS测量姿态的方法，其通过对磁力计输出值进行修正，用以提高当物体处于磁异常的地域环境下的姿态角输出精度。当物体处于地磁异常的情况时，通过对方向向量的微分方程的推导和利用，求解磁力计输出值在一段连续时刻之间的关系；再利用无迹卡尔曼滤波，通过上一时段磁力计的输出值连续校准下一时刻磁力计的输出值；最终，利用校准后的磁力计输出值同陀螺仪、加速度计的输出值进行数据融合，以求解出姿态角。本发明方法可依据磁力计连续时刻的输出关系，修正磁力计在磁异常环境下的输出值，其计算量较低，在海底勘探仪器的姿态测量方面，能有效的提高当物体处于磁异常的地域环境下的姿态角输出精度。

Description

一种在地磁异常情况下应用AHRS测量姿态的方法

技术领域

本发明属于球物理勘探技术领域，涉及一种海底勘探仪器的姿态测量算法，尤其涉及一种在地磁异常情况下应用AHRS测量姿态的方法，其通过校正磁力计获得准确姿态角。

背景技术

AHRS被称为航姿参考系统，其包括多个轴向传感器，能够为飞行器提供航向、横滚和侧翻信息，也可以应用在海底地球物理勘探中，测量海底仪器的姿态信息。航姿参考系统包括基于MEMS的三轴陀螺仪，加速度计和磁强计，通过对这三种传感器采集的数据进行数据融合能够有效的进行姿态测量。常用的数据融合算法有互补滤波、卡尔曼滤波等。目前常用的航姿参考系统(AHRS)内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器。

但是，由于海底环境复杂，磁力计易受到海底矿藏等产生的磁场的影响，而造成测量的不准确，导致惯性测量系统最终测量的姿态不准确。目前常用的校正磁力计输出值异常的方法，是对载体的外壳和导线等存在的电磁异常进行校正，这些因素产生的磁力异常可以通过绕载体的三个轴轴向转动而获得造成影响的具体数值而进行校正。但是，这种方法并不适合海底矿藏等产生的固定点的地磁异常校正。

现有方法针对地磁异常的校正，需要先已知仪器行进的准确路径，并需要已知路径下各个点在无磁异常环境下的准确输出。这种方法需要事先进行详尽的规划和测量，而海底勘探存在各种不确定性，行进路线不可能严格按照设定进行。因此，该方法也不适用于海底勘探系统中进行姿态的测量。

发明内容

本发明的目的就在于针对上述现有技术的不足，提出一种在地磁异常情况下应用AHRS测量姿态的方法，其通过对磁力计输出值进行修正，用以提高当物体处于磁异常的地域环境下的姿态角输出精度。

本发明通过积分公式，求解连续时刻姿态角之间的关系，通过上一时段磁力计的输出值连续校准下一时刻磁力计的输出值。最终，利用校准后的传感器输出值进行数据融合求解出姿态角。

本文的目的是通过以下技术方案实现的：

一种在地磁异常情况下应用AHRS测量姿态的方法，包括以下步骤：

A、基于无迹卡尔曼滤波的磁力计异常矫正：

A1、磁异常的判断：通过对三轴磁力计的输出值求出总磁通量和磁倾角，当其偏离当地磁通量和磁倾角时则此时刻磁力计处于磁异常状态；

A2、为卡尔曼滤波选定状态量其含义为方位角偏离值和磁力计异常值；

A3、利用状态量的运动关系推导出状态方程，并求出状态矩阵A和状态估计值X_t；

A4、通过对方向向量开微分方程的改进，在其原来旋转矩阵的基础上，推导出连续时刻磁力计输出的关系，进而推导出测量方程，并求解出测量矩阵Z和测量估计值z_t；

A5、基于上述状态方程和测量方程，通过无迹卡尔曼滤波对磁力计输出的异常值进行修正；

A6、通过迭代算法用上一时段磁力计的输出值连续校准下一时刻磁力计的输出值使每一个异常值可以多次参加卡尔曼滤波过程，尽可能的消除极端异常值对整个过程的影响；

B、求解方位角：

B1、根据四元数的微分方程推算出姿态运动学方程作为卡尔曼滤波状态方程，代入磁力计修正值和加速度计值求出方位角测量值；

B2、通过扩展卡尔曼滤波融合AHRS数据求出姿态角。

步骤B2，所述AHRS包括磁力计、加速度计和陀螺仪。

步骤B2，所述姿态角包括航向角、俯仰角和横滚角。

与现有技术相比，本发明的有益效果在于：

1、利用本发明方法可以依据磁力计连续时刻的输出关系，有效修正磁力计在磁异常环境下的输出值。

2、在海底勘探仪器的姿态测量方面，有效的提高当物体处于磁异常的地域环境下的姿态角输出精度。

3、本发明可应用在仪器行进路径和路径各点磁异常未知的情况下，及时校准磁力计输出。

4、相比于现有的方法，本发明方法计算量较低，一定程度上可以实现磁力计输出值得实时修正。

附图说明

图1为坐标系转换图；

图2为测量姿态方法总框图；

图3为ukf过程框图；

图4为迭代过程框图；

图5为数据融合框图。

具体实施方式

一种在地磁异常情况下应用AHRS测量姿态的方法，包括以下步骤：

A、基于无迹卡尔曼滤波算法的磁力计异常矫正：

A1、磁异常的判断：在一般情况下地球磁场磁通量和磁倾角是一个固定数值；通过对三轴磁力计的输出值求出总磁通量和磁倾角；当其偏离当地磁通量和磁倾角时则此时刻磁力计处于磁异常状态；

A2、为卡尔曼滤波选定状态量其含义为方位角偏离值和磁力计异常值；

A3、利用状态量的运动关系推导出状态方程，并求出状态矩阵A和状态估计值X_t；

A4、通过对方向向量开微分方程的改进，在其原来旋转矩阵的基础上，推导出连续时刻磁力计输出的关系，进而推导出测量方程，并求解出测量矩阵Z和测量估计值z_t；

A5、基于上述状态方程和测量方程，通过无迹卡尔曼滤波算法对磁力计输出的异常值进行修正；

A6、通过迭代算法用上一时段磁力计的输出值连续校准下一时刻磁力计的输出值使每一个异常值可以多次参加卡尔曼滤波过程，尽可能的消除极端异常值对整个过程的影响；

B、求解方位角方法：

B1、根据四元数的微分方程推算出姿态运动学方程作为卡尔曼滤波状态方程，代入磁力计修正值和加速度计值求出方位角测量值；

B2、通过扩展卡尔曼滤波融合AHRS数据求出姿态角。

具体如下：

1、坐标系转换：

如图1所示，载体坐标系与导航坐标系之前存在一定的夹角，因此，可以通过欧拉角来表示载体坐标系向导航坐标系转换的姿态矩阵。设载体坐标系b是由导航坐标系n按照Z-X-Y旋转顺序，分别旋转θ,φ,

经过3次旋转得到的方向余弦矩阵可表示为：

用四元素表示为：

四元数更新为：

其中Ω_b表示载体坐标系相对于导航坐标系的角速度在载体坐标系上的分量，可以通过3轴陀螺仪测得。其为：

采用一阶Runge-Kutta法求解四元数微分方程，其离散时间模型为：

则加速度的输出a_x，a_y，a_z为：

磁力计输出为：m_x，m_y，m_z

带入可得姿态角计算公式，如下所示：

而此时如果拖体或拖缆做在其附近有较大的磁场干扰(这里假设这个干扰是附近海底固定的矿藏或者其他无法移动的干扰)。那磁力计的计算公式将改变如下：

式中d_x，d_y，d_z为该磁干扰在导航坐标系x,y,z方向的分磁量。

运动加速度与磁干扰变化情况如下:

d_t＝c_dd_t-1+v_t (10)

其中c_d为一个介于(0,1)的常数，v_t为磁力计前后时刻输出的干扰误差。

为方便运算现引入新的变量如下，令：

其物理意义为:在载体坐标系下各干扰量在x,y,z轴的分量。

2、对于磁异常的判断条件：

在一般情况下地球磁场磁通量和磁倾角是一个固定数值(对于某一地区)其与传感器磁力计输出关系如下：

其中

通过对三轴磁力计的输出值求出总磁通量和磁倾角。当其偏离当地磁通量和磁倾角时则此时刻磁力计处于磁异常状态。

3、磁力计校准滤波过程：

状态方程：

w_t为对应系统的噪声，A矩阵为状态矩阵。

由式(10)可知：

即：

式中c_a，c_α为一个介于(0,1)的常数；分别是角度误差矩阵和磁力计干扰量的误差矩阵，其理论上为高斯白噪声矩阵。

测量方程：

式中Z为测量矩阵；z_t为测量值，其为t时刻磁力计的输出值；r_t为测量噪声。由John.E推导的公式：

其中C^RB是方向余弦变换矩阵；

其构成元素为：关于载体坐标系的角速度。

在本文中即体现为旋转矩阵即

用时间积分来描述即为：

而

即

所以两边同乘[Mcosη 0 Msinη]^T可以改写为

当磁力计有异常情况时表述如下：

其中M_(t)表示t时刻磁力计的输出值，表示t-1时刻磁力计修正后的输出值。

4、迭代处理：

由卡尔曼滤波过程中，我们发现卡尔曼的校准对于特定的时刻t仅仅只是做了一次迭代，在实际过程中如果t时刻的磁干扰相比于于之前时刻的干扰较强或者较弱时，一次卡尔曼运算并不能很好的处理这样的情况。因此可以采用重复迭代的方式，将一次卡尔曼的运算量加大，由单时刻更新变为多时刻更新。这样就可以让同一时刻多次参加校准，消除突然强干扰，或者异常值对整个修正过程的影响。其处理过程如下流程图2所示。

如图3所示为ukf的过程，由t-1时刻的状态量和磁力计估计值，以及t-1时刻由陀螺仪输出得到的旋转矩阵，求得状态值和测量值并结合它们的协方差，与t时刻磁力计的输出值融合求出t时刻磁力计估计值和状态量，再结合t时刻由陀螺仪输出的旋转矩阵进行对下一时刻磁力计估计值的计算，依次循环计算。

如图4所示其在正常ukf过程后，加入一个循环因子，当达到循环条件时，它会让其计算返回，并把上轮的测量更新值作为新的输入带人新的一轮运算，这样一个滤波结果至少需要参与n-1次运算，这样在尽量少的影响卡尔曼系数的情况下，让卡尔曼系数的变化不随特殊时刻磁强干扰的影响而产生抖动而影响最后结果。

5、求解最终姿态角：

由以上方案我们可以求出磁力计的修正值，再与陀螺仪和加速度进行数据融合即可求出在磁异常情况下，姿态角的修正值。其计算如图5：

其计算过程如下：

(1)求解坐标系转换：由于实际过程中载体坐标系与理论导航坐标系不同，我们首先需要对其进行一定的转换，这里采用欧拉角和四元数表示。

载体坐标系和导航坐标系之间的转换关系可表示为其中

四元数定义为其满足归一化要求q^Tq＝q₀ ²+q₁ ²+q₂ ²+q₃ ²＝1则由四元数表示转换矩阵为

所以可得载体的姿态角为其中翻滚角和航向角φ需要对其进行相位判断。

(2)四元数姿态运动模型求解：由四元数的定义可以推算出姿态运动学方程为其中Ω_b可通过载体上的3轴陀螺仪测得，其表示载体坐标系相对于导航坐标系的角速度在载体坐标系山的分量。其矩阵形式如下：

其中ω_a表示绕a轴的角速度。

采用一阶龙格塔库法求解四元数微分方程，其离散时间模型为

其中T是时间间隔，I为单位矩阵

(3)求解状态方程，测量方程：由于陀螺仪有漂移误差b_w，因此采用上文所述的特殊情况的处理方式，将其放入状态矩阵。

综上述，扩展卡尔曼滤波的状态方程，测量方程如下：

其中状态方程由姿态变换中的四元素方程得到，观测方程由磁力计和加速度计联合测得的航向角、俯仰角和翻滚角组成。由此可以按照扩展卡尔曼滤波的递推周期进行运算处理。

(4)求解方向角：通过上述卡尔曼滤波过程，递推更新四元数，最终求解出方位角。其公式如下：

Claims (3)

1.一种在地磁异常情况下应用AHRS测量姿态的方法，其特征在于，包括以下步骤：

A、基于无迹卡尔曼滤波算法的磁力计异常矫正：

A1、磁异常的判断：通过对三轴磁力计的输出值求出总磁通量和磁倾角，当其偏离当地磁通量和磁倾角时则此时刻磁力计处于磁异常状态；

A2、为卡尔曼滤波选定状态量其含义为方位角偏离值和磁力计异常值；

A3、利用状态量的运动关系推导出状态方程，并求出状态矩阵A和状态估计值X_t；

A4、通过对方向向量开微分方程的改进，在其原来旋转矩阵的基础上，推导出连续时刻磁力计输出的关系，进而推导出测量方程，并求解出测量矩阵Z和测量估计值z_t；

A5、基于上述状态方程和测量方程，通过无迹卡尔曼滤波算法对磁力计输出的异常值进行修正；

A6、通过迭代算法用上一时段磁力计的输出值连续校准下一时刻磁力计的输出值使每一个异常值可以多次参加卡尔曼滤波过程，尽可能的消除极端异常值对整个过程的影响；

B、求解方位角方法：

B1、根据四元数的微分方程推算出姿态运动学方程作为卡尔曼滤波状态方程，代入磁力计修正值和加速度计值求出方位角测量值；

B2、通过扩展卡尔曼滤波融合AHRS数据求出姿态角。

2.根据权利要求1所述的一种在地磁异常情况下应用AHRS测量姿态的方法，其特征在于：步骤B2，所述AHRS包括磁力计、加速度计和陀螺仪。

3.根据权利要求1所述的一种在地磁异常情况下应用AHRS测量姿态的方法，其特征在于：步骤B2，所述姿态角包括航向角、俯仰角和横滚角。