CN112504266B - 基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法 - Google Patents

Abstract

本发明属于水下地磁辅助导航技术领域，具体涉及一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法。本发明通过地磁梯度张量矩阵的对角化和特征向量的正交归一化处理，得到姿态矩阵，从而确定水下载体的姿态。本发明无积累误差且具有很好的隐蔽性，能较为准确地反演出载体的姿态角信息，且稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量也较小，更适用于大初始姿态误差下水下载体的全姿态确定。

Description

基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法

技术领域

本发明属于水下地磁辅助导航技术领域，具体涉及一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法。

背景技术

地磁场作为地球固有的一个稳定的矢量场，蕴含的特征分量较多，已广泛应用于航空航天及航海领域中载体的导航定位与姿态控制。地磁导航相比于传统的惯性导航，具有无累计误差的优点；而与卫星导航相比，其抗干扰性更强。另外，由于地磁测量不会产生电磁信号的泄露，可以保障载体的隐蔽性。在众多地磁导航方式中，单纯基于地磁场强度的姿态确定方法由于其原理上的限制，易受附近各种干扰磁场影响，且载体上软磁材料所产生的磁场，尤其是大倾斜角下的磁场，与载体的运动状态有关，难以实现精确校正；另一方面，仅靠地磁场强度也不足以完成对全部姿态角的确定。而基于磁场梯度张量矩阵的姿态确定方法，可通过提取磁场的梯度张量消减干扰磁场的影响，同时可以解决单个磁强计无法自主定姿的问题。

目前，全姿态确定使用的大多数是基于卫星和惯性的导航方法，它们大多应用于航天器及地表物体的定姿；而水下载体的定姿大多采用惯性导航，但仅仅使用这种方法会产生随时间累积的误差，严重影响了姿态确定的精度，所以产生了天体辅助导航、无线电辅助导航以及声纳辅助导航等，但上述方法无疑都牺牲了水下载体的隐蔽性。为给水下载体姿态估计提供一种自主的、隐蔽的途径，人们提出了一种基于地磁梯度张量测量值的全姿态确定法(黄玉，武立华，孙铎.一种基于地磁梯度张量测量的水下载体全姿态确定方法.发明专利号：ZL201310692189.0，中国，[P].2015年12月2日)，它运用牛顿下山法求解以姿态四元数为自变量的非线性方程组，这就需要较好的初始解进行迭代计算。

发明内容

本发明的目的在于提供稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量较小，更适用于大初始姿态误差下水下载体的全姿态确定的一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：按十单轴磁强计的对称阵列形式构建地磁梯度张量测量装置并捷联在载体上；所述的地磁梯度张量测量装置包括第一三轴磁强计、第二三轴磁强计、第一二轴磁强计和第二二轴磁强计；设定第一三轴磁强计布置在载体上的位置为点A，第二三轴磁强计布置在载体上的位置为点B，第一二轴磁强计布置在载体上的位置为点C，第二二轴磁强计布置在载体上的位置为点D，且线段AB与线段CD垂直相交于点O，

以点O为原点，线段AB所在直线为x_b轴，线段CD所在直线为y_b轴，垂直于平面ABCD的直线为z_b轴，建立载体坐标系ox_by_bz_b；l_x和l_y分别为ox_b坐标轴和oy_b坐标轴方向上的梯度测量基线长；

步骤2：获取第一三轴磁强计、第二三轴磁强计、第一二轴磁强计和第二二轴磁强计的地磁场测量值，计算载体坐标系ox_by_bz_b下的地磁梯度张量矩阵G^b；

第一三轴磁强计在ox_b坐标轴方向上测得的地磁场分量值为h₁，第一三轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₂，第一三轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₃，第二三轴磁强计在ox_b坐标轴方向上测得的地磁场分量值为h₄，第一三轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₅，第一三轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₆；第一二轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₇，第一二轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₈，第二二轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₉，第二二轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₁₀；

所述的载体坐标系ox_by_bz_b下的地磁梯度张量矩阵G^b为：

步骤3：根据惯性组合导航***的指示位置，从预先存储的地磁梯度张量数据库中提取地理坐标系下地磁梯度张量矩阵Gⁿ的5个独立分量

及

步骤4：载体坐标系ox_by_bz_b下的地磁梯度张量矩阵G^b和地理坐标系下的地磁梯度张量矩阵Gⁿ具有相同的特征值，对这两个地磁梯度张量矩阵分别进行对角化处理，并对它们的特征向量进行正交归一化处理；

步骤4.1：计算地磁梯度张量的特征值λ_i，i＝1,2,3；

步骤4.2：分别计算矩阵Gⁿ和G^b的特征向量α_ni和α_bi；

步骤4.3：如果特征值λ_i没有重根，则令β_ni＝α_ni，β_bi＝α_bi，执行步骤4.5；否则，执行步骤4.4；

步骤4.4：对特征向量α_ni和α_bi进行正交化处理，得到β_ni和β_bi；

步骤4.5：对β_ni和β_bi进行单位化处理，得到ε_ni和ε_bi；

步骤5：根据选取的特征向量正负号的不同，由ε_ni和ε_bi分别构造出8组不同的正交矩阵Q_n和Q_b，记为Q_n(k)和Q_b(k)，k＝1,2,…,8；

步骤6：计算出8种可能的姿态矩阵Q(k)，

由姿态角的初始解计算初始姿态矩阵

计算8种可能的姿态矩阵Q(k)与

之差的F-范数值，选择F-范数值最小的Q(k)作为最终选取的姿态矩阵

步骤7：由

计算得俯仰角θ、航向角ψ与横滚角φ的值；

θ＝-arcsin c₃₁

其中，c_ij为姿态矩阵

的第i行、第j列元素；

本发明的有益效果在于：

本发明通过地磁梯度张量矩阵的对角化和特征向量的正交归一化处理，得到姿态矩阵，从而确定水下载体的姿态。本发明无积累误差且具有很好的隐蔽性，能较为准确地反演出载体的姿态角信息，且稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量也较小，更适用于大初始姿态误差下水下载体的全姿态确定。

附图说明

图1是本发明的总体流程图。

图2是本发明中测量地磁梯度张量的十单轴磁强计阵列配置图。

图3是本发明的实施例1中姿态确定算法的随机仿真实验结果图。

图4是本发明的实施例2中姿态角确定的绝对误差随测量噪声标准差变化的曲线。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明涉及一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法，属于水下地磁辅助导航领域。本发明通过地磁梯度张量矩阵的对角化和特征向量的正交归一化处理，得到姿态矩阵，从而确定水下载体的姿态。这种姿态确定方法的算法稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量也较小，更适用于大初始姿态误差下水下载体的全姿态确定。

捷联于水下载体的矢量磁强计阵列构成地磁梯度张量测量装置，用于测量载体坐标系下的地磁场梯度张量的5个独立分量，得到载体坐标系下的地磁梯度张量矩阵；依据惯性组合导航***输出的参考位置，从地磁梯度张量数据库中提取出该参考位置处地理坐标系下的地磁场梯度张量的5个独立分量，得到地理坐标系下的地磁梯度张量矩阵；由于两个地磁梯度张量矩阵之间满足相似与合同关系，因此二者具有相同的特征值，对两个地磁梯度张量矩阵分别进行对角化处理，并对它们的特征向量进行正交与归一化处理得到正交单位矩阵，进而计算出姿态矩阵，然后由姿态矩阵计算得到载体的姿态角。具体的定姿算法流程如图1所示。

步骤1：采用十单轴磁力计配置方式，建立载体坐标系ox_by_bz_b，载体坐标系ox_by_bz_b由地理坐标系ox_ny_nz_n经以下三步旋转得到：1)ox_ny_nz_n绕oz_n轴旋转ψ得到ox′y′z′；2)ox′y′z′绕oy′轴旋转θ得到ox″y″z″；3)ox″y″z″绕ox″轴旋转φ得到载体坐标系。磁梯度张量测量装置采用图2所示的十单轴磁强计配置方式，ox_b和oy_b坐标轴上的磁强计分别关于原点对称，两个三轴磁强计分别放在ox_b坐标轴上的点A和B处，测得的磁场分量值分别为(h₁、h₂、h₃)和(h₄、h₅、h₆)；另一对二轴磁强计分别置于oy_b坐标轴上的点C和D处，测得的磁场分量值分别为(h₇、h₈)和(h₉、h₁₀)，箭头表示矢量磁强计的敏感轴方向，对准各个单轴磁强计敏感轴使其与箭头方向一致。根据式(1)计算

及

值。

式中，l_x和l_y分别为ox_b坐标轴和oy_b坐标轴方向上的梯度测量基线长。

由式(2)得到载体坐标系下的地磁梯度张量矩阵G^b为

步骤2：根据惯性组合导航***的指示位置，从预先存储的地磁梯度张量数据库中提取地理坐标系下地磁梯度张量Gⁿ的5个独立分量

及

Gⁿ为

步骤3：矩阵Gⁿ和G^b具有相同的特征值，对这两个地磁梯度张量矩阵分别进行对角化处理，并对它们的特征向量进行正交归一化处理。

步1)根据式(4)求出地磁梯度张量的特征值λ_i，i＝1,2,3。

其中，

步2)按式(5)和式(6)分别计算矩阵Gⁿ和G^b的特征向量α_ni和α_bi，i＝1,2,3。

步3)如果特征值λ_i没有重根，则令β_ni＝α_ni，β_bi＝α_bi，转步5)，否则，对特征向量α_ni和α_bi分别进行正交化处理。

步4)由式(7)和式(8)分别对α_ni和α_bi进行正交化处理，得到β_ni和β_bi。

步5)分别按式(9)和式(10)对β_ni和β_bi进行单位化处理，得到ε_ni和ε_bi。

步骤4：根据选取的特征向量正负号的不同，由ε_ni和ε_bi分别按式(11)和(12)构造出8组不同的正交矩阵Q_n和Q_b。

步骤5：根据式(13)计算出8种可能的姿态矩阵Q(k)，k＝1,2,…,8，

由姿态角的初始解ψ₀、

和γ₀计算初始姿态矩阵

计算Q(k)与

之差的Frobenius范数，选择F-范数值最小的Q(k)作为最终选取的姿态矩阵

步骤6：由

根据式(14)计算得到俯仰角θ、航向角的主值与横滚角的主值，按表1和表2分别确定航向角ψ和横滚角φ。

式中，c_ij是姿态矩阵

的第i行、第j列元素。

表1航向角的真值表

表2横滚角的真值表

定义姿态角确定的误差分别为

式中，

和

分别为ψ、θ和φ的确定值，δ_ψ为航向角的误差，δ_θ为俯仰角的误差，δ_φ为横滚角的误差。

与现有技术相比，本发明的有益效果是：本发明提供了一种应用于水下载体的低成本姿态确定方法，该方法无积累误差且具有很好的隐蔽性，能较为准确地反演出载体的姿态角信息；定姿算法的稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量也较小，更适用于大初始姿态误差下水下载体的全姿态确定。

实施例1：

位于点P(50m,-30m,20m)处的偶极子磁性目标的磁矩分量分别为m_x＝10⁸A·m²，m_y＝5×10⁷A·m²，m_z＝2×10⁷A·m²；基线长l_x＝l_y＝1m。载体航向角为ψ＝-60°+Ψ，Ψ是±10°范围内均匀分布的随机值，载体俯仰角为θ＝-40°+Θ，Θ是±10°范围内均匀分布的随机值，载体俯仰角为φ＝20°+Φ，Φ是±10°范围内均匀分布的随机值，共随机生成100组姿态角。设姿态角初始解与真实值之间存在40°的偏差，矢量磁强计的测量噪声是标准差为1nT、零均值的高斯随机噪声。仿真得到姿态角确定的绝对误差曲线如图3所示，从图3可以看出，该姿态确定算法所产生的姿态角误差在0.4°以内。

实施例2：

位于点P(50m,-30m,20m)处的偶极子磁性目标的磁矩分量分别为m_x＝10⁸A·m²，m_y＝5×10⁷A·m²，m_z＝2×10⁷A·m²；基线长l_x＝l_y＝1m。载体航向角为ψ＝-60°，载体俯仰角为θ＝-40°，载体俯仰角为φ＝20°。设姿态角初始解与真实值之间存在40°的偏差。对每一个测量噪声标准差的仿真条件进行50次蒙特卡洛实验并求均值，得到姿态角确定的绝对误差随测量噪声标准差变化的曲线如图4所示。从图4可以看出，姿态角的绝对误差随噪声标准差的增加而近似线性增长，但当测量噪声标准差为10nT时该姿态确定算法所产生的姿态角误差约为1.5°。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法，其特征在于，包括以下步骤：

步骤1：按十单轴磁强计的对称阵列形式构建地磁梯度张量测量装置并捷联在载体上；所述的地磁梯度张量测量装置包括第一三轴磁强计、第二三轴磁强计、第一二轴磁强计和第二二轴磁强计；设定第一三轴磁强计布置在载体上的位置为点A，第二三轴磁强计布置在载体上的位置为点B，第一二轴磁强计布置在载体上的位置为点C，第二二轴磁强计布置在载体上的位置为点D，且线段AB与线段CD垂直相交于点O，

以点O为原点，线段AB所在直线为x_b轴，线段CD所在直线为y_b轴，垂直于平面ABCD的直线为z_b轴，建立载体坐标系ox_by_bz_b；l_x和l_y分别为ox_b坐标轴和oy_b坐标轴方向上的梯度测量基线长；

步骤2：获取第一三轴磁强计、第二三轴磁强计、第一二轴磁强计和第二二轴磁强计的地磁场测量值，计算载体坐标系ox_by_bz_b下的地磁梯度张量矩阵G^b；

第一三轴磁强计在ox_b坐标轴方向上测得的地磁场分量值为h₁，第一三轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₂，第一三轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₃，第二三轴磁强计在ox_b坐标轴方向上测得的地磁场分量值为h₄，第一三轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₅，第一三轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₆；第一二轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₇，第一二轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₈，第二二轴磁强计在oy_b坐标轴方向上测得的地磁场分量值为h₉，第二二轴磁强计在oz_b坐标轴方向上测得的地磁场分量值为h₁₀；

所述的载体坐标系ox_by_bz_b下的地磁梯度张量矩阵G^b为：

步骤3：根据惯性组合导航***的指示位置，从预先存储的地磁梯度张量数据库中提取地理坐标系下地磁梯度张量矩阵Gⁿ的5个独立分量

及

步骤4：载体坐标系ox_by_bz_b下的地磁梯度张量矩阵G^b和地理坐标系下的地磁梯度张量矩阵Gⁿ具有相同的特征值，对这两个地磁梯度张量矩阵分别进行对角化处理，并对它们的特征向量进行正交归一化处理；

步骤4.1：计算地磁梯度张量的特征值λ_i，i＝1,2,3；

步骤4.2：分别计算矩阵Gⁿ和G^b的特征向量α_ni和α_bi；

步骤4.3：如果特征值λ_i没有重根，则令β_ni＝α_ni，β_bi＝α_bi，执行步骤4.5；否则，执行步骤4.4；

步骤4.4：对特征向量α_ni和α_bi进行正交化处理，得到β_ni和β_bi；

步骤4.5：对β_ni和β_bi进行单位化处理，得到ε_ni和ε_bi；

步骤5：根据选取的特征向量正负号的不同，由ε_ni和ε_bi分别构造出8组不同的正交矩阵Q_n和Q_b，记为Q_n(k)和Q_b(k)，k＝1,2,…,8；

步骤6：计算出8种可能的姿态矩阵Q(k)，

由姿态角的初始解计算初始姿态矩阵

计算8种可能的姿态矩阵Q(k)与

之差的F-范数值，选择F-范数值最小的Q(k)作为最终选取的姿态矩阵

步骤7：由

计算得俯仰角θ、航向角ψ与横滚角φ的值；

θ＝-arcsinc₃₁

其中，c_ij为姿态矩阵

的第i行、第j列元素；

Images