阅读说明：本技术 基于地磁梯度张量矩阵正交对角化的水下全姿态确定方法 (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.)

1. An underwater full attitude determination method based on geomagnetic gradient tensor matrix orthogonal diagonalization is characterized by comprising the following steps of:

step 1: constructing a geomagnetic gradient tensor measuring device in a symmetrical array form of the deca-uniaxial magnetometer and strapdown the geomagnetic gradient tensor measuring device on a carrier; the geomagnetic gradient tensor measuring device comprises a first triaxial magnetometer, a second triaxial magnetometer, a first two-axis magnetometer and a second two-axis magnetometer; setting the position of the first triaxial magnetometer arranged on the carrier as point A, the position of the second triaxial magnetometer arranged on the carrier as point B, the position of the first biaxial magnetometer arranged on the carrier as point C, the position of the second biaxial magnetometer arranged on the carrier as point D, and the line segment AB and the line segment CD are perpendicularly intersected at the point O,

the point O is taken as the origin, and the straight line of the line segment AB is taken as x_bThe axis and the line segment CD are located on the straight line y_bAxis, straight line perpendicular to plane ABCD being z_bAxis, establishing a carrier coordinate system ox_by_bz_b；l_xAnd l_yAre each ox_bCoordinate axes and oy_bThe length of a gradient measurement base line in the coordinate axis direction;

step 2: acquiring geomagnetic field measurement values of a first three-axis magnetometer, a second three-axis magnetometer, a first two-axis magnetometer and a second two-axis magnetometer, and calculating a carrier coordinate system ox_by_bz_bLower geomagnetic gradient tensor matrix G^b；

First three-axis magnetometer at ox_bThe earth magnetic field component value measured in the coordinate axis direction is h₁The first three-axis magnetometer is on oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₂The first three-axis magnetometer is at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₃Second three-axis magnetometer at ox_bThe earth magnetic field component value measured in the coordinate axis direction is h₄The first three-axis magnetometer is on oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₅The first three-axis magnetometer is at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₆(ii) a First two-axis magnetometer in oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₇First two-axis magnetometer at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₈Second two-axis magnetometer at oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₉Second two-axis magnetometer at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₁₀；

The carrier coordinate system ox_by_bz_bLower geomagnetic gradient tensor matrix G^bComprises the following steps:

and step 3: extracting a magnetic gradient tensor matrix G in a geographic coordinate system from a pre-stored geomagnetic gradient tensor database according to the indication position of the inertial integrated navigation systemⁿ5 independent components ofAnd

and 4, step 4: carrier coordinate system ox_by_bz_bLower geomagnetic gradient tensor matrix G^bAnd a geomagnetic gradient tensor matrix G under a geographic coordinate systemⁿThe two geomagnetic gradient tensor matrixes are subjected to diagonalization processing respectively and orthogonal normalization processing on eigenvectors of the two geomagnetic gradient tensor matrixes;

step 4.1: calculating eigenvalues lambda of the geomagnetic gradient tensor_i，i＝1,2,3；

Step 4.2: calculate the matrix G separatelyⁿAnd G^bCharacteristic vector alpha of_niAnd alpha_bi；

Step 4.3: if the eigenvalue λ_iIf there is no heavy root, let beta_ni＝α_ni，β_bi＝α_biAnd executing the step 4.5; otherwise, executing step 4.4;

step 4.4: for the feature vector alpha_niAnd alpha_biPerforming orthogonalization to obtain beta_niAnd beta_bi；

Step 4.5: for beta is_niAnd beta_biPerforming unitization treatment to obtain epsilon_niAnd ε_bi；

And 5: according to the sign of the selected characteristic vector, from epsilon_niAnd ε_biRespectively constructing 8 different orthogonal matrixes Q_nAnd Q_bIs marked as Q_n(k) And Q_b(k)，k＝1,2,…,8；

Step 6: the 8 possible attitude matrices q (k) are calculated,computing an initial attitude matrix from an initial solution of attitude anglesCalculate 8 possible attitude matrices Q (k) andselecting Q (k) with the minimum F-norm value as the final selected attitude matrix

And 7: byCalculating values of a pitch angle theta, a course angle psi and a roll angle phi;

θ＝-arcsinc₃₁

wherein, c_ijAs a matrix of gesturesRow i, column j elements;

Technical Field

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.

Background

The geomagnetic field is used as a stable vector field inherent to the earth, contains more characteristic components, and is widely applied to navigation positioning and attitude control of carriers in the fields of aerospace and navigation. Compared with the traditional inertial navigation, the geomagnetic navigation has the advantage of no accumulative error; and is more interference-resistant than satellite navigation. In addition, because the leakage of electromagnetic signals cannot be generated in geomagnetic measurement, the concealment of the carrier can be guaranteed. Among a plurality of geomagnetic navigation modes, the attitude determination method based on geomagnetic field intensity is easily influenced by various nearby interference magnetic fields due to the limitation of the principle, and the magnetic field generated by the soft magnetic material on the carrier, especially the magnetic field under a large inclination angle, is related to the motion state of the carrier, so that accurate correction is difficult to realize; on the other hand, the determination of all attitude angles is not sufficiently accomplished by the intensity of the earth magnetic field alone. The attitude determination method based on the magnetic field gradient tensor matrix can reduce the influence of an interference magnetic field by extracting the gradient tensor of the magnetic field, and can solve the problem that a single magnetometer cannot autonomously determine the attitude.

At present, most of the full-attitude determination uses navigation methods based on satellites and inertia, and the navigation methods are mostly applied to attitude determination of spacecrafts and earth surface objects; however, inertial navigation is mostly adopted for determining the attitude of the underwater carrier, but only the method can generate errors accumulated along with time, and the accuracy of attitude determination is seriously influenced, so celestial body-assisted navigation, radio-assisted navigation, sonar-assisted navigation and the like are generated, but the method undoubtedly sacrifices the concealment of the underwater carrier. An autonomous and hidden approach is provided for underwater vehicle attitude estimation, and a full attitude determination method (topaz, Wulihua and Sundaze) based on geomagnetic gradient tensor measurement values is provided (the invention patent number: ZL201310692189.0, China, [ P ].2015 12, month and 2), which solves a nonlinear equation set with attitude quaternion as an independent variable by using a Newton downhill method and needs a better initial solution to carry out iterative computation.

Disclosure of Invention

The invention aims to provide an underwater full attitude determination method based on geomagnetic gradient tensor matrix orthogonal diagonalization, which has the advantages of good stability, low requirement on initial solution of attitude angles, complete and autonomous attitude determination, small calculated amount and suitability for determining the full attitude of an underwater carrier under large initial attitude errors.

The purpose of the invention is realized by the following technical scheme: the method comprises the following steps:

step 1: constructing a geomagnetic gradient tensor measuring device in a symmetrical array form of the deca-uniaxial magnetometer and strapdown the geomagnetic gradient tensor measuring device on a carrier; the geomagnetic gradient tensor measuring device comprises a first triaxial magnetometer, a second triaxial magnetometer, a first two-axis magnetometer and a second two-axis magnetometer; setting the position of the first triaxial magnetometer arranged on the carrier as point A, the position of the second triaxial magnetometer arranged on the carrier as point B, the position of the first biaxial magnetometer arranged on the carrier as point C, the position of the second biaxial magnetometer arranged on the carrier as point D, and the line segment AB and the line segment CD are perpendicularly intersected at the point O,

the point O is taken as the origin, and the straight line of the line segment AB is taken as x_bThe axis and the line segment CD are located on the straight line y_bAxis, straight line perpendicular to plane ABCD being z_bAxis, establishing a carrier coordinate system ox_by_bz_b；l_xAnd l_yAre each ox_bCoordinate axes and oy_bThe length of a gradient measurement base line in the coordinate axis direction;

step 2: acquiring geomagnetic field measurement values of a first three-axis magnetometer, a second three-axis magnetometer, a first two-axis magnetometer and a second two-axis magnetometer, and calculating a carrier coordinate system ox_by_bz_bLower geomagnetic gradient tensor matrix G^b；

First three-axis magnetometer at ox_bThe earth magnetic field component value measured in the coordinate axis direction is h₁The first three-axis magnetometer is on oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₂The first three-axis magnetometer is at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₃Second three-axis magnetometer at ox_bMeasured in the direction of the coordinate axisHas a geomagnetic field component value of h₄The first three-axis magnetometer is on oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₅The first three-axis magnetometer is at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₆(ii) a First two-axis magnetometer in oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₇First two-axis magnetometer at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₈Second two-axis magnetometer at oy_bThe earth magnetic field component value measured in the coordinate axis direction is h₉Second two-axis magnetometer at oz_bThe earth magnetic field component value measured in the coordinate axis direction is h₁₀；

The carrier coordinate system ox_by_bz_bLower geomagnetic gradient tensor matrix G^bComprises the following steps:

and step 3: extracting a magnetic gradient tensor matrix G in a geographic coordinate system from a pre-stored geomagnetic gradient tensor database according to the indication position of the inertial integrated navigation systemⁿ5 independent components ofAnd

and 4, step 4: carrier coordinate system ox_by_bz_bLower geomagnetic gradient tensor matrix G^bAnd the tensor moment of geomagnetic gradient in geographic coordinate systemArray GⁿThe two geomagnetic gradient tensor matrixes are subjected to diagonalization processing respectively and orthogonal normalization processing on eigenvectors of the two geomagnetic gradient tensor matrixes;

step 4.1: calculating eigenvalues lambda of the geomagnetic gradient tensor_i，i＝1,2,3；

Step 4.2: calculate the matrix G separatelyⁿAnd G^bCharacteristic vector alpha of_niAnd alpha_bi；

Step 4.3: if the eigenvalue λ_iIf there is no heavy root, let beta_ni＝α_ni，β_bi＝α_biAnd executing the step 4.5; otherwise, executing step 4.4;

step 4.4: for the feature vector alpha_niAnd alpha_biPerforming orthogonalization to obtain beta_niAnd beta_bi；

Step 4.5: for beta is_niAnd beta_biPerforming unitization treatment to obtain epsilon_niAnd ε_bi；

And 5: according to the sign of the selected characteristic vector, from epsilon_niAnd ε_biRespectively constructing 8 different orthogonal matrixes Q_nAnd Q_bIs marked as Q_n(k) And Q_b(k)，k＝1,2,…,8；

Step 6: the 8 possible attitude matrices q (k) are calculated,computing an initial attitude matrix from an initial solution of attitude anglesCalculate 8 possible attitude matrices Q (k) andthe F-norm value of the difference is selected as FQ (k) with the minimum norm value as the finally selected attitude matrix

And 7: byCalculating values of a pitch angle theta, a course angle psi and a roll angle phi;

θ＝-arcsin c₃₁

wherein, c_ijAs a matrix of gesturesRow i, column j elements;

the invention has the beneficial effects that:

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.

Drawings

FIG. 1 is a general flow diagram of the present invention.

Fig. 2 is a diagram showing an arrangement of a decadic magnetometer array for measuring the magnetic gradient tensor according to the present invention.

Fig. 3 is a graph of the results of a random simulation experiment of the attitude determination algorithm in embodiment 1 of the present invention.

Fig. 4 is a graph of absolute error of attitude angle determination as a function of standard deviation of measurement noise in embodiment 2 of the present invention.

Detailed Description

The invention is further described below with reference to the accompanying drawings.

The invention relates to an underwater full attitude determination method based on geomagnetic gradient tensor matrix orthogonal diagonalization, and belongs to the field of underwater geomagnetic auxiliary navigation. 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 attitude determination method has the advantages of good algorithm stability, low requirement on initial solution of the attitude angle, complete and autonomous attitude determination, small calculated amount and suitability for determining the full attitude of the underwater carrier under large initial attitude error.

The vector magnetometer array strapped to the underwater carrier forms a geomagnetic gradient tensor measuring device which is used for measuring 5 independent components of the geomagnetic gradient tensor under the carrier coordinate system to obtain a geomagnetic gradient tensor matrix under the carrier coordinate system; extracting 5 independent components of the geomagnetic field gradient tensor under a geographic coordinate system at the reference position from a geomagnetic gradient tensor database according to the reference position output by the inertial integrated navigation system to obtain a geomagnetic gradient tensor matrix under the geographic coordinate system; because the two geomagnetic gradient tensor matrixes meet the similarity and contract relationship, the two geomagnetic gradient tensor matrixes have the same eigenvalue, the two geomagnetic gradient tensor matrixes are respectively subjected to diagonalization, the eigenvectors of the two geomagnetic gradient tensor matrixes are subjected to orthogonal and normalization processing to obtain orthogonal identity matrixes, then the attitude matrixes are calculated, and the attitude angles of the carrier are calculated by the attitude matrixes. The specific attitude determination algorithm flow is shown in fig. 1.

Step 1: a carrier coordinate system ox is established by adopting a configuration mode of a ten-axis magnetometer_by_bz_bVector coordinate system ox_by_bz_bFrom a geographical coordinate system ox_ny_nz_nThe compound is obtained by the following three steps of rotation: 1) ox_ny_nz_nAround oz_nRotating the shaft psi to obtain ox ' y ' z '; 2) rotation of ox 'y' z 'by θ about the oy' axis yields ox "y" z "; 3) the ox "y" z "is rotated phi around the ox" axis to obtain the carrier coordinate system. The magnetic gradient tensor measuring device adopts a ten-axis magnetometer configuration mode shown in FIG. 2, ox_bAnd oy_bThe magnetometers on the coordinate axes are respectively symmetrical about the origin, and the two three-axis magnetometers are respectively arranged at ox_bAt points A and B on the coordinate axis, the measured magnetic field component values are respectively (h)₁、h₂、h₃) And (h)₄、h₅、h₆) (ii) a Another pair of biaxial magnetometers is respectively arranged at oy_bAt points C and D on the coordinate axis, the measured magnetic field component values are (h) respectively₇、h₈) And (h)₉、h₁₀) The arrows indicate the direction of the sensitive axes of the vector magnetometers, and the sensitive axes of the individual uniaxial magnetometers are aligned to coincide with the direction of the arrows. Calculating according to equation (1)Andthe value is obtained.

In the formula I_xAnd l_yAre each ox_bCoordinate axes and oy_bThe gradient in the direction of the coordinate axis measures the base length.

Obtaining a geomagnetic gradient tensor matrix G under a carrier coordinate system by the formula (2)^bIs composed of

Step 2: extracting a magnetic gradient tensor G in a geographic coordinate system from a pre-stored geomagnetic gradient tensor database according to the indication position of the inertial integrated navigation systemⁿ5 independent components ofAndGⁿis composed of

And step 3: matrix GⁿAnd G^bThe two geomagnetic gradient tensor matrices are diagonalized and orthogonal normalized with the same eigenvalue.

Step 1) obtaining an eigenvalue λ of the geomagnetic gradient tensor from the equation (4)_i，i＝1,2,3。

Wherein the content of the first and second substances,

step 2) calculating matrix G according to formula (5) and formula (6) respectivelyⁿAnd G^bCharacteristic vector alpha of_niAnd alpha_bi，i＝1,2,3。

Step 3) if the characteristic value lambda_iIf there is no heavy root, let beta_ni＝α_ni，β_bi＝α_biGo to step 5), otherwise, for the feature vectorα_niAnd alpha_biThe orthogonalization processing is performed separately.

Step 4) separately aligning alpha with formula (7) and formula (8)_niAnd alpha_biPerforming orthogonalization to obtain beta_niAnd beta_bi。

Step 5) p beta according to formula (9) and formula (10) respectively_niAnd beta_biPerforming unitization treatment to obtain epsilon_niAnd ε_bi。

And 4, step 4: according to the sign of the selected characteristic vector, from epsilon_niAnd ε_bi8 different sets of orthogonal matrices Q are constructed according to equations (11) and (12), respectively_nAnd Q_b。

And 5: the 8 possible attitude matrices q (k), k 1,2, …,8,

by initial solution of attitude angle psi₀、And gamma₀Computing an initial attitude matrixCalculating Q (k) andthe Frobenius norm of the difference is selected, and Q (k) with the minimum F-norm value is used as the finally selected attitude matrix

Step 6: byAnd (3) calculating according to the formula (14) to obtain a pitch angle theta, a main value of the course angle and a main value of the roll angle, and respectively determining the course angle psi and the roll angle phi according to the table 1 and the table 2.

In the formula, c_ijIs a matrix of gesturesRow i and column j.

TABLE 1 truth table of course angles

TABLE 2 truth table of roll angle

The error defining the determination of the attitude angle is respectively

In the formula (I), the compound is shown in the specification,anddetermined values of psi, theta and phi, delta_ψError of course angle, delta_θError of pitch angle, δ_φIs the error of the roll angle.

Compared with the prior art, the invention has the beneficial effects that: the invention provides a low-cost attitude determination method applied to an underwater carrier, which has no accumulated error and good concealment and can accurately invert attitude angle information of the carrier; the attitude determination algorithm has good stability, low requirement on initial solution of an attitude angle, complete and autonomous attitude determination and smaller calculated amount, and is more suitable for determining the full attitude of an underwater carrier under large initial attitude error.

Example 1:

the magnetic moment components of the dipole magnetic target at points P (50m, -30m,20m) are m_x＝10⁸A·m²，m_y＝5×10⁷A·m²，m_z＝2×10⁷A·m²(ii) a Base length l_x＝l_y1 m. The carrier heading angle psi is-60 degrees + psi, psi is a random value uniformly distributed in a range of +/-10 degrees, the carrier pitch angle theta is-40 degrees + theta, theta is a random value uniformly distributed in a range of +/-10 degrees, the carrier pitch angle phi is 20 degrees + phi, and phi is a random value uniformly distributed in a range of +/-10 degrees, and 100 groups of attitude angles are randomly generated. And setting the deviation of 40 degrees between the initial solution of the attitude angle and the true value, wherein the measurement noise of the vector magnetometer is Gaussian random noise with the standard deviation of 1nT and zero mean value. The absolute error curve of attitude angle determination obtained by simulation is shown in FIG. 3. As can be seen from FIG. 3, the attitude angle error generated by the attitude determination algorithmThe difference is within 0.4 deg.

Example 2:

the magnetic moment components of the dipole magnetic target at points P (50m, -30m,20m) are m_x＝10⁸A·m²，m_y＝5×10⁷A·m²，m_z＝2×10⁷A·m²(ii) a Base length l_x＝l_y1 m. The carrier course angle phi is-60 degrees, the carrier pitch angle theta is-40 degrees, and the carrier pitch angle phi is 20 degrees. Let there be a 40 deg. deviation between the initial solution of attitude angle and the true value. The simulation conditions for each measured noise standard deviation were subjected to 50 monte carlo experiments and averaged to obtain a curve showing the absolute error of attitude angle determination as a function of the measured noise standard deviation, as shown in fig. 4. As can be seen from fig. 4, the absolute error of the attitude angle increases approximately linearly with increasing noise standard deviation, but the attitude determination algorithm produces an attitude angle error of about 1.5 ° when the measured noise standard deviation is 10 nT.

The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.