基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法
阅读说明：本技术 基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法 (Underwater full attitude determination method based on geomagnetic gradient tensor matrix orthogonal diagonalization ) 是由 黄玉 武立华 秦洋 刘苏涛 张涛 王洋 于 2020-11-17 设计创作，主要内容包括：本发明属于水下地磁辅助导航技术领域,具体涉及一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法。本发明通过地磁梯度张量矩阵的对角化和特征向量的正交归一化处理,得到姿态矩阵,从而确定水下载体的姿态。本发明无积累误差且具有很好的隐蔽性,能较为准确地反演出载体的姿态角信息,且稳定性好,对姿态角初始解的要求不高,姿态确定完全自主,计算量也较小,更适用于大初始姿态误差下水下载体的全姿态确定。(The invention belongs to the technical field of underwater geomagnetic aided navigation, and particularly relates to an underwater full attitude determination method based on geomagnetic gradient tensor matrix orthogonal diagonalization. The attitude matrix is obtained through diagonalization of the geomagnetic gradient tensor matrix and orthogonal normalization processing of the eigenvector, so that the attitude of the underwater carrier is determined. The method has no accumulated error, has good concealment, can accurately invert the attitude angle information of the carrier, has good stability, has low requirement on the initial solution of the attitude angle, has completely autonomous attitude determination and smaller calculated amount, and is more suitable for determining the full attitude of the underwater carrier under the condition of large initial attitude error.)
技术领域
本发明属于水下地磁辅助导航技术领域，具体涉及一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法。
背景技术
地磁场作为地球固有的一个稳定的矢量场，蕴含的特征分量较多，已广泛应用于航空航天及航海领域中载体的导航定位与姿态控制。地磁导航相比于传统的惯性导航，具有无累计误差的优点；而与卫星导航相比，其抗干扰性更强。另外，由于地磁测量不会产生电磁信号的泄露，可以保障载体的隐蔽性。在众多地磁导航方式中，单纯基于地磁场强度的姿态确定方法由于其原理上的限制，易受附近各种干扰磁场影响，且载体上软磁材料所产生的磁场，尤其是大倾斜角下的磁场，与载体的运动状态有关，难以实现精确校正；另一方面，仅靠地磁场强度也不足以完成对全部姿态角的确定。而基于磁场梯度张量矩阵的姿态确定方法，可通过提取磁场的梯度张量消减干扰磁场的影响，同时可以解决单个磁强计无法自主定姿的问题。
目前，全姿态确定使用的大多数是基于卫星和惯性的导航方法，它们大多应用于航天器及地表物体的定姿；而水下载体的定姿大多采用惯性导航，但仅仅使用这种方法会产生随时间累积的误差，严重影响了姿态确定的精度，所以产生了天体辅助导航、无线电辅助导航以及声纳辅助导航等，但上述方法无疑都牺牲了水下载体的隐蔽性。为给水下载体姿态估计提供一种自主的、隐蔽的途径，人们提出了一种基于地磁梯度张量测量值的全姿态确定法(黄玉，武立华，孙铎.一种基于地磁梯度张量测量的水下载体全姿态确定方法.发明专利号：ZL201310692189.0，中国，[P].2015年12月2日)，它运用牛顿下山法求解以姿态四元数为自变量的非线性方程组，这就需要较好的初始解进行迭代计算。
发明内容
本发明的目的在于提供稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量较小，更适用于大初始姿态误差下水下载体的全姿态确定的一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：按十单轴磁强计的对称阵列形式构建地磁梯度张量测量装置并捷联在载体上；所述的地磁梯度张量测量装置包括第一三轴磁强计、第二三轴磁强计、第一二轴磁强计和第二二轴磁强计；设定第一三轴磁强计布置在载体上的位置为点A，第二三轴磁强计布置在载体上的位置为点B，第一二轴磁强计布置在载体上的位置为点C，第二二轴磁强计布置在载体上的位置为点D，且线段AB与线段CD垂直相交于点O，
以点O为原点，线段AB所在直线为xb轴，线段CD所在直线为yb轴，垂直于平面ABCD的直线为zb轴，建立载体坐标系oxbybzb；lx和ly分别为oxb坐标轴和oyb坐标轴方向上的梯度测量基线长；
步骤2：获取第一三轴磁强计、第二三轴磁强计、第一二轴磁强计和第二二轴磁强计的地磁场测量值，计算载体坐标系oxbybzb下的地磁梯度张量矩阵Gb；
第一三轴磁强计在oxb坐标轴方向上测得的地磁场分量值为h1，第一三轴磁强计在oyb坐标轴方向上测得的地磁场分量值为h2，第一三轴磁强计在ozb坐标轴方向上测得的地磁场分量值为h3，第二三轴磁强计在oxb坐标轴方向上测得的地磁场分量值为h4，第一三轴磁强计在oyb坐标轴方向上测得的地磁场分量值为h5，第一三轴磁强计在ozb坐标轴方向上测得的地磁场分量值为h6；第一二轴磁强计在oyb坐标轴方向上测得的地磁场分量值为h7，第一二轴磁强计在ozb坐标轴方向上测得的地磁场分量值为h8，第二二轴磁强计在oyb坐标轴方向上测得的地磁场分量值为h9，第二二轴磁强计在ozb坐标轴方向上测得的地磁场分量值为h10；
所述的载体坐标系oxbybzb下的地磁梯度张量矩阵Gb为：
步骤3：根据惯性组合导航系统的指示位置，从预先存储的地磁梯度张量数据库中提取地理坐标系下地磁梯度张量矩阵Gn的5个独立分量及
步骤4：载体坐标系oxbybzb下的地磁梯度张量矩阵Gb和地理坐标系下的地磁梯度张量矩阵Gn具有相同的特征值，对这两个地磁梯度张量矩阵分别进行对角化处理，并对它们的特征向量进行正交归一化处理；
步骤4.1：计算地磁梯度张量的特征值λi，i＝1,2,3；
步骤4.2：分别计算矩阵Gn和Gb的特征向量α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组不同的正交矩阵Qn和Qb，记为Qn(k)和Qb(k)，k＝1,2,…,8；
步骤6：计算出8种可能的姿态矩阵Q(k)，由姿态角的初始解计算初始姿态矩阵计算8种可能的姿态矩阵Q(k)与之差的F-范数值，选择F-范数值最小的Q(k)作为最终选取的姿态矩阵
步骤7：由计算得俯仰角θ、航向角ψ与横滚角φ的值；
θ＝-arcsin c31
其中，cij为姿态矩阵的第i行、第j列元素；
本发明的有益效果在于：
本发明通过地磁梯度张量矩阵的对角化和特征向量的正交归一化处理，得到姿态矩阵，从而确定水下载体的姿态。本发明无积累误差且具有很好的隐蔽性，能较为准确地反演出载体的姿态角信息，且稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量也较小，更适用于大初始姿态误差下水下载体的全姿态确定。
附图说明
图1是本发明的总体流程图。
图2是本发明中测量地磁梯度张量的十单轴磁强计阵列配置图。
图3是本发明的实施例1中姿态确定算法的随机仿真实验结果图。
图4是本发明的实施例2中姿态角确定的绝对误差随测量噪声标准差变化的曲线。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明涉及一种基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法，属于水下地磁辅助导航领域。本发明通过地磁梯度张量矩阵的对角化和特征向量的正交归一化处理，得到姿态矩阵，从而确定水下载体的姿态。这种姿态确定方法的算法稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量也较小，更适用于大初始姿态误差下水下载体的全姿态确定。
捷联于水下载体的矢量磁强计阵列构成地磁梯度张量测量装置，用于测量载体坐标系下的地磁场梯度张量的5个独立分量，得到载体坐标系下的地磁梯度张量矩阵；依据惯性组合导航系统输出的参考位置，从地磁梯度张量数据库中提取出该参考位置处地理坐标系下的地磁场梯度张量的5个独立分量，得到地理坐标系下的地磁梯度张量矩阵；由于两个地磁梯度张量矩阵之间满足相似与合同关系，因此二者具有相同的特征值，对两个地磁梯度张量矩阵分别进行对角化处理，并对它们的特征向量进行正交与归一化处理得到正交单位矩阵，进而计算出姿态矩阵，然后由姿态矩阵计算得到载体的姿态角。具体的定姿算法流程如图1所示。
步骤1：采用十单轴磁力计配置方式，建立载体坐标系oxbybzb，载体坐标系oxbybzb由地理坐标系oxnynzn经以下三步旋转得到：1)oxnynzn绕ozn轴旋转ψ得到ox′y′z′；2)ox′y′z′绕oy′轴旋转θ得到ox″y″z″；3)ox″y″z″绕ox″轴旋转φ得到载体坐标系。磁梯度张量测量装置采用图2所示的十单轴磁强计配置方式，oxb和oyb坐标轴上的磁强计分别关于原点对称，两个三轴磁强计分别放在oxb坐标轴上的点A和B处，测得的磁场分量值分别为(h1、h2、h3)和(h4、h5、h6)；另一对二轴磁强计分别置于oyb坐标轴上的点C和D处，测得的磁场分量值分别为(h7、h8)和(h9、h10)，箭头表示矢量磁强计的敏感轴方向，对准各个单轴磁强计敏感轴使其与箭头方向一致。根据式(1)计算及值。
式中，lx和ly分别为oxb坐标轴和oyb坐标轴方向上的梯度测量基线长。
由式(2)得到载体坐标系下的地磁梯度张量矩阵Gb为
步骤2：根据惯性组合导航系统的指示位置，从预先存储的地磁梯度张量数据库中提取地理坐标系下地磁梯度张量Gn的5个独立分量及Gn为
步骤3：矩阵Gn和Gb具有相同的特征值，对这两个地磁梯度张量矩阵分别进行对角化处理，并对它们的特征向量进行正交归一化处理。
步1)根据式(4)求出地磁梯度张量的特征值λi，i＝1,2,3。
其中，
步2)按式(5)和式(6)分别计算矩阵Gn和Gb的特征向量α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组不同的正交矩阵Qn和Qb。
步骤5：根据式(13)计算出8种可能的姿态矩阵Q(k)，k＝1,2,…,8，
由姿态角的初始解ψ0、和γ0计算初始姿态矩阵计算Q(k)与之差的Frobenius范数，选择F-范数值最小的Q(k)作为最终选取的姿态矩阵
步骤6：由根据式(14)计算得到俯仰角θ、航向角的主值与横滚角的主值，按表1和表2分别确定航向角ψ和横滚角φ。
式中，cij是姿态矩阵的第i行、第j列元素。
表1航向角的真值表
表2横滚角的真值表
定义姿态角确定的误差分别为
式中，和分别为ψ、θ和φ的确定值，δψ为航向角的误差，δθ为俯仰角的误差，δφ为横滚角的误差。
与现有技术相比，本发明的有益效果是：本发明提供了一种应用于水下载体的低成本姿态确定方法，该方法无积累误差且具有很好的隐蔽性，能较为准确地反演出载体的姿态角信息；定姿算法的稳定性好，对姿态角初始解的要求不高，姿态确定完全自主，计算量也较小，更适用于大初始姿态误差下水下载体的全姿态确定。
实施例1：
位于点P(50m,-30m,20m)处的偶极子磁性目标的磁矩分量分别为mx＝108A·m2，my＝5×107A·m2，mz＝2×107A·m2；基线长lx＝ly＝1m。载体航向角为ψ＝-60°+Ψ，Ψ是±10°范围内均匀分布的随机值，载体俯仰角为θ＝-40°+Θ，Θ是±10°范围内均匀分布的随机值，载体俯仰角为φ＝20°+Φ，Φ是±10°范围内均匀分布的随机值，共随机生成100组姿态角。设姿态角初始解与真实值之间存在40°的偏差，矢量磁强计的测量噪声是标准差为1nT、零均值的高斯随机噪声。仿真得到姿态角确定的绝对误差曲线如图3所示，从图3可以看出，该姿态确定算法所产生的姿态角误差在0.4°以内。
实施例2：
位于点P(50m,-30m,20m)处的偶极子磁性目标的磁矩分量分别为mx＝108A·m2，my＝5×107A·m2，mz＝2×107A·m2；基线长lx＝ly＝1m。载体航向角为ψ＝-60°，载体俯仰角为θ＝-40°，载体俯仰角为φ＝20°。设姿态角初始解与真实值之间存在40°的偏差。对每一个测量噪声标准差的仿真条件进行50次蒙特卡洛实验并求均值，得到姿态角确定的绝对误差随测量噪声标准差变化的曲线如图4所示。从图4可以看出，姿态角的绝对误差随噪声标准差的增加而近似线性增长，但当测量噪声标准差为10nT时该姿态确定算法所产生的姿态角误差约为1.5°。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。