CN103630139B - A kind of full attitude determination method of underwater carrier measured based on earth magnetism gradient tensor - Google Patents

Abstract

The invention belongs to geomagnetic auxiliary navigation field under water, be specifically related to a kind of full attitude determination method of underwater carrier measured based on earth magnetism gradient tensor.The present invention includes: under carrier system, set up 5 components that three-dimensional cartesian coordinate system obtains magnetic gradient tensor; 5 isolated components of earth magnetism gradient tensor under Department of Geography are extracted from the earth magnetism gradient tensor database prestored; Substitute into about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations; Hypercomplex number with substitute into the transformation matrix of carrier system and Department of Geography, obtain the estimated value of pitching angle theta.The underwater carrier location technology based on geomagnetic anomaly inverting that the present invention proposes has that cost is low, highly sensitive, antijamming capability is strong and without characteristics such as accumulated errors, solve the problem that single magnetometer cannot rely on terrestrial magnetic field autonomous attitude determination, for underwater carrier Attitude estimation provides a kind of independently, hidden approach.

Description

A kind of full attitude determination method of underwater carrier measured based on earth magnetism gradient tensor

Technical field

The invention belongs to geomagnetic auxiliary navigation field under water, be specifically related to a kind of full attitude determination method of underwater carrier measured based on earth magnetism gradient tensor.

Background technology

Underwater vehicle can keep higher positioning precision according to inertia/geomagnetic matching integrated navigation system, but long-time underwater navigation can make submarine attitude angle have several years error, can launch to the Underwater Battery depending on navigational system attitude and bring comparatively big error, the fighting efficiency of serious restriction submerge device.Therefore, underwater carrier determines appearance technology is one of core content of underwater vehicle navigational system.GPS determines appearance technology and loses the height independence of underwater navigation system and concealed feature, normally cannot accept gps signal by the restriction of underwater complex landform simultaneously.The degree of accuracy of optical profile type attitude determination system and resolution decline with the increase of target and optical sensor distance, and optical noise, parasitic light, shade to block etc. all may cause determines appearance failure, so be difficult to be applied under water based on the attitude determination system of optical measurement.The star sensor attitude measure method being considered to precision at present the highest is mainly used in the spacecraft such as airship, satellite, and what the method provided is the attitude of carrier relative to inertial space, independently can not determine appearance, needs to be combined with inertial navigation system and expensive.Based on the attitude measurement method of geomagnetic field intensity due to the restriction in its principle, easily be subject to the impact of various disturbing magnetic field around, and the magnetic field that on carrier, soft iron magnetic material produces is relevant with carrier movement state, be difficult to accurate correction, particularly to the magnetic field under large angle of inclination.On the other hand, single geomagnetic field intensity can not provide enough attitude measurement informations.The present invention proposes a kind of attitude determination method based on magnetic field gradient Tensor measuring, the impact of disturbing magnetic field is eliminated by the extraction of magnetic field gradient information, solve the problem that single magnetometer cannot rely on terrestrial magnetic field autonomous attitude determination, simultaneously for underwater carrier Attitude estimation provides a kind of independently, hidden approach.

Summary of the invention

The object of the present invention is to provide that a kind of cost is low, highly sensitive, antijamming capability is strong and without the full attitude determination method of the underwater carrier based on magnetic field gradient Tensor measuring of accumulated error.

The object of the present invention is achieved like this:

(1) under carrier system, three-dimensional cartesian coordinate system x is set up _b, y _b, z _b, the orthogonal magnetometer of two three axles is placed on x respectively _ba, B symmetrical centered by initial point of axle two point, measures magnetic-field component (h respectively ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another is placed in y respectively to two axle orthogonal magnetometers _bc, D symmetrical centered by initial point of axle two point, measures magnetic-field component (h respectively ₉, h ₁₀) and (h ₇, h ₈), the sensitive axes direction of all magnetometers is consistent, base area magnetic gradient tensor G ^b5 components and with magnetic-field component h ₁~ h ₁₀relational expression

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.$ Obtain 5 components of magnetic gradient tensor, L _xand L _ybe respectively x _band y _bgradiometry base length on direction;

(2) according to the indicating positions of inertia/earth magnetism integrated navigation system, from the earth magnetism gradient tensor database prestored, earth magnetism gradient tensor G under Department of Geography is extracted ⁿ5 isolated components and

(3) earth magnetism gradient tensor component under carrier system step 1,2 obtained with earth magnetism gradient tensor component under Department of Geography and substitute into about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{(q_0^2+q_1^2-q_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{q}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{q}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{(q_0^2-q_1^2+q_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{q}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{q}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+(q_0^2-q_1^2+q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_2{q}_3-q_0{q}_1)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[(q_0^2-q_1^2+q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$

About q ₀, q ₁, q ₂and q ₃nonlinear System of Equations can by G ⁿand G ^brelational expression obtain, wherein T representing matrix transposition, the transformation matrix that geography is tied to carrier system, and the transformation matrix of carrier system and Department of Geography, and in conjunction with q ₀, q ₁, q ₂and q ₃restriction relation newton-decline method is utilized to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations;

(4) hypercomplex number meeting iterated conditional will obtained in step (3) with substitute into the transformation matrix of carrier system and Department of Geography,

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right],$

Obtain matrix element (i, j=1,2,3),

Obtain the estimated value of pitching angle theta;

Course angle estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle estimated value divide 3 kinds of situations: work as c ₃₃during > 0, work as c ₃₃< 0, time, work as c ₃₃< 0, time,

Beneficial effect of the present invention is: the underwater carrier location technology based on geomagnetic anomaly inverting that the present invention proposes has that cost is low, highly sensitive, antijamming capability is strong and without characteristics such as accumulated errors, solve the problem that single magnetometer cannot rely on terrestrial magnetic field autonomous attitude determination, for underwater carrier Attitude estimation provides a kind of independently, hidden approach.

Accompanying drawing explanation

Fig. 1 is method flow diagram.

Fig. 2 is that earth magnetism gradient tensor measures arrangement plan.

Fig. 3 is attitude measurement principle schematic.

Fig. 4 is the algorithm convergence situation of different initial solution condition.

Fig. 5 is the attitude angle estimated in different initial solution situation.

Fig. 6 is the algorithm convergence situation under different noise level.

Fig. 7 is the pose estimation value under different noise level.

Embodiment

The invention provides a kind of carrier method for determining posture be applied under water, the method can be finally inversed by the attitude angle information of carrier comparatively accurately; Solve the problem that single magnetometer cannot rely on terrestrial magnetic field autonomous attitude determination, for underwater carrier Attitude estimation provides a kind of independently, hidden approach.

A kind of complete under water attitude determination method measured based on earth magnetism gradient tensor of the present invention, strapdown forms earth magnetism gradient tensor measurement mechanism in the vector magnetometer group of underwater carrier, 5 isolated components of geomagnetic gradient tensor under mensuration carrier system.According to the reference position that inertia/earth magnetism integrated navigation system exports from earth magnetism gradient tensor database, extract 5 isolated components of geomagnetic gradient tensor under local Department of Geography, the Nonlinear System of Equations about carrier 3 attitude angle is formed with earth magnetism gradient tensor measured value under carrier system, hypercomplex number corresponding to attitude angle is exported as initial solution using inertial navigation system, the optimized algorithms such as Newton-decline method are used to estimate attitude of carrier, Fig. 1 gives the process flow diagram of the method, and its concrete steps are as follows:

Step 1, for more accurately geodetic magnetic gradient tensor adopts ten single axis magnetometers configuration modes, under carrier system, set up three-dimensional cartesian coordinate system x _by _bz _b, Fig. 2 gives the configuration mode of magnetometer group: the orthogonal magnetometer of two three axles is placed on x respectively _ba, B 2 point of axle, measures magnetic-field component (h respectively ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another is placed in y respectively to two axle orthogonal magnetometers _bc, D 2 point of axle, measures magnetic-field component (h respectively ₉, h ₁₀) and (h ₇, h ₈), arrow represents the sensitive axes direction of each magnetometer.5 components of base area magnetic gradient tensor Gb and with magnetic-field component h ₁~ h ₁₀relational expression

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.---\left(1\right)$

5 components of magnetic gradient tensor can be calculated, L in equation (1) _xand L _ybe respectively x _band y _bgradiometry base length on direction;

Step 2, indicating positions according to inertia/earth magnetism integrated navigation system, extract earth magnetism gradient tensor G under Department of Geography from the earth magnetism gradient tensor database prestored ⁿ5 isolated components and

Earth magnetism gradient tensor component under step 3, carrier system that step 1,2 is obtained with earth magnetism gradient tensor component under Department of Geography substitute into about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{(q_0^2+q_1^2-q_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}---\left(2\right)$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{q}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{q}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{(q_0^2-q_1^2+q_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}---\left(3\right)$

$\begin{array}{c}{g}_{\mathrm{yx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2+q_0{q}_3)+g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2+q_2^2-q_3^2)+4\left(q_1{q}_2{-q}_0{q}_3\right)(q_1{q}_2+q_0{q}_3)]\\ +2g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)+2(q_1{q}_3+q_0{q}_2)(q_1{q}_2+q_0{q}_3)]+2g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)\\ +2g_{\mathrm{zy}}^n[2(q_1{q}_2-q_0{q}_3)(q_2{q}_3-q_0{q}_1)+(q_1{q}_3+q_0{q}_2)(q_0^2-q_0^2+q_0^2-q_0^2)]-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_2{q}_3-q_0{q}_1)\end{array}---\left(4\right)$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{q}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{q}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+(q_0^2-q_1^2+q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_2{q}_3-q_0{q}_1)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[(q_0^2-q_1^2+q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}---\left(5\right)$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}---\left(6\right)$

About q ₀, q ₁, q ₂and q ₃nonlinear System of Equations can by G ⁿand G ^brelational expression obtain, wherein T representing matrix transposition, the transformation matrix that geography is tied to carrier system, and the transformation matrix of carrier system and Department of Geography, and in conjunction with q ₀, q ₁, q ₂and q ₃restriction relation newton-decline method is utilized to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations;

Step 4: by the hypercomplex number meeting iterated conditional obtained in step 3 with substitute into the transformation matrix of carrier system and Department of Geography

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(7\right)$

Obtain matrix element (i, j=1,2,3).According to following formula:

Obtain the estimated value of pitching angle theta.

Course angle estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle estimated value divide 3 kinds of situations: work as c ₃₃during > 0, work as c ₃₃< 0, time, work as c ₃₃< 0, time,

Below in conjunction with accompanying drawing, embodiments of the present invention are described in detail:

Step 1, the single axis magnetometers group measuring earth magnetism gradient tensor to be configured by Fig. 2 mode for more accurately geodetic magnetic gradient tensor.Arrow represents the sensitive axes direction of each magnetometer.There are a pair three axle orthogonal magnetometers at A, B place, measure magnetic-field component (h respectively ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another at C, D place, measures magnetic-field component (h to two axle magnetometers respectively ₉, h ₁₀) and (h ₇, h ₈).X _band y _bon direction, gradiometry base length is respectively L _xand L _y, then earth magnetism gradient tensor G ^bfive isolated components calculated by following formula:

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.---\left(1\right)$

Step 2, indicating positions according to inertia/earth magnetism integrated navigation system, extract earth magnetism gradient tensor G under Department of Geography from the earth magnetism gradient tensor database prestored ⁿ5 isolated components with

Step 3, set the course angle of carrier as Ψ (traditionally with north by east for just), the angle of pitch is θ, and roll angle is γ, gets three coordinate axis x of geographic coordinate system _n, y _n, z _nsensing be followed successively by sky, northeast, as shown in Figure 3.Then the relationship description of body axis system b and geographic coordinate system n is, the 1st step: Ox _ny _nz _naround-z _naxle rotates Ψ and obtains Ox ₁y ₁z ₁, the 2nd step: Ox ₁y ₁z ₁around x ₁axle rotates θ and obtains Ox ₂y ₂z ₂, the 3rd step: Ox ₂y ₂z ₂around y ₂axle rotates γ and obtains Ox _by _bz _b, then the transformation matrix between Two coordinate system for

$C_n^b=\left[\begin{array}{ccc}\begin{array}{c}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\\ \sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& \begin{array}{c}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \begin{array}{c}\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\\ \mathrm{sos\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& \begin{array}{c}-\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\\ \cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}\end{array}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]---\left(2\right)$

If G ⁿand G ^bbe respectively the expression of earth magnetism gradient tensor under n system and b system, then have

$G^b=C_n^b{G}^n{\left(C_n^b\right)}^T---\left(3\right)$

In formula, T representing matrix transposition.G ^bmeasured by earth magnetism gradient measuring device, G ⁿcan measure in advance and be stored in navigational computer.Five independent equations about attitude of carrier angle (Ψ, θ, γ) are obtained by formula (3).For avoiding the trigonometric function operation in optimum estimation algorithm, introduce hypercomplex number Q=q ₀+ q ₁i ₀+ q ₂j ₀+ q ₃k ₀determine that b is tied to the transformation matrix of n system, namely

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(4\right)$

Obtained about q by formula (3) and formula (4) ₀, q ₁, q ₂and q ₃5 independent equations

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{(q_0^2+q_1^2-q_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}---\left(5\right)$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{q}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{q}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{(q_0^2-q_1^2+q_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}---\left(6\right)$

$\begin{array}{c}{g}_{\mathrm{yx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2+q_0{q}_3)+g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2+q_2^2-q_3^2)+4\left(q_1{q}_2{-q}_0{q}_3\right)(q_1{q}_2+q_0{q}_3)]\\ +2g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)+2(q_1{q}_3+q_0{q}_2)(q_1{q}_2+q_0{q}_3)]+2g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)\\ +2g_{\mathrm{zy}}^n[2(q_1{q}_2-q_0{q}_3)(q_2{q}_3-q_0{q}_1)+(q_1{q}_3+q_0{q}_2)(q_0^2-q_0^2+q_0^2-q_0^2)]-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_2{q}_3-q_0{q}_1)\end{array}---\left(7\right)$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{q}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{q}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+(q_0^2-q_1^2+q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_2{q}_3-q_0{q}_1)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[(q_0^2-q_1^2+q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}---\left(8\right)$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}---\left(9\right)$

Earth magnetism gradient tensor component under the carrier system that step 1,2 is obtained with earth magnetism gradient tensor component under Department of Geography and substitute into about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

In conjunction with q ₀, q ₁, q ₂and q ₃restriction relation newton-decline method is utilized to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations.

The iterative formula of Newton-decline method is:

x ⁿ⁺¹＝x ⁿ-ω(F′(x ⁿ)) ^-1F(x ⁿ)(10)

The span of ω is 0 < ω≤1, for ensureing convergence, generally needs the value of ω to meet

||F(x ⁿ⁺¹)||＜||F(x ⁿ)||(11)

The value of ω adopts successively halving method, for reducing calculated amount, also the formal approximation of partial derivative difference coefficient in Jacobian matrix is replaced; Inertial navigation system export attitude angle and the actual attitude angle of submerge device comparatively close, inertial navigation system can be exported hypercomplex number corresponding to attitude angle as Newton-decline method iteration initial value.

Under Matlab simulated conditions, emulation experiment is carried out to the method:

The magnetic field gradient utilizing single magnetic dipole to produce magnetic anomaly field gradient in analog, magnetic dipole magnetic moment component is respectively m _x=10 × 10 ⁸am ², m _y=2 × 10 ⁸am ²and m _z=1 × 10 ⁸am ², earth magnetism gradient measuring device is respectively x=100m relative to the location components of magnetic dipole, y=50m and z=20m, Magnetic Gradient Measurement base length Δ x=Δ y=1m, algorithm iteration end condition be error ε=|| x _k+1-x _k||≤10 ^-6.

First probe into the impact of initial solution on Algorithm for Solving precision, suppose that the noise of each magnetometer is separate Gaussian process, its average is 0, and mean square deviation is σ=2nT.What Fig. 4 represented is in different initial solution situation, adopt the convergence situation of Newton-decline method, just because of relaxing to the iterative algorithm condition of convergence exception causing convergence curve, but finally all can restrain after a few step iteration, in Multi simulation running experiment, attitude angle deviation generally all can not more than 20 times lower than iterative steps when 20 °, and visible, algorithm the convergence speed is very fast.Algorithm initial solution can estimate attitude angle within relatively truly separating deviation ± 20 ° well as can be seen from Figure 5, the estimation error of the angle of pitch is slightly large, between ± 0.7 °, course angle estimation error is between ± 0.15 °, and roll angle estimation error is between ± 0.12 °.Visible algorithm is low to initial solution accuracy requirement, can estimate true attitude angle preferably within departing from actual value ± 20 °, and inertial navigation system can be utilized completely to export attitude information as algorithm initial solution, and device is practical.

When initial solution is constant, consider the calculation accuracy of algorithm under different magnetometer noise level, other simulated conditions are constant.As can be seen from Figure 6 noise is on the impact of algorithm the convergence speed, when noise level is lower than 10nT, algorithm the convergence speed difference is not that clearly after noise level is higher than 10nT, speed of convergence is obviously slack-off, is only 18 steps lower than greatest iteration step number under 10nT noise level.The noise level of three axis fluxgate magnetometers can be less than 10nT, therefore can measure earth magnetism gradient tensor with multiple fluxgate magnetometer, form cheap earth magnetism gradient tensor measurement mechanism.Pose estimation value under the corresponding different magnetometer noise level of Fig. 7, when magnetometer noise mean square deviation is less than 10nT, angle of pitch estimation error is substantially all within ± 0.5 °, course angle estimation error is within ± 0.13 °, roll angle estimation error is also no more than ± and 0.1 °, algorithm still more adequately can calculate attitude angle.

Step 4, the hypercomplex number of iterated conditional will be met with substitute into the transformation matrix of carrier system and Department of Geography

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(12\right)$

Obtain matrix element (i, j=1,2,3).Comparison expression (2) and formula (3) obtain:

for the estimated value of the angle of pitch, according to matrix element and formula (13) obtains three attitude angle.Course angle estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle estimated value divide 3 kinds of situations: work as c ₃₃during > 0, work as c ₃₃< 0, time, work as c ₃₃< 0, time,

Beneficial effect of the present invention is described as follows:

The magnetic field gradient utilizing single magnetic dipole to produce magnetic anomaly field gradient in analog, Multi simulation running experimental result shows: this algorithm is low to initial solution accuracy requirement, true attitude angle can be estimated preferably within magnetometer noise level 10nT and initial value deviation true value ± 20 °, fluxgate magnetometer can be utilized completely as earth magnetism gradient tensor measuring unit, inertial navigation system exports attitude information as algorithm initial solution, and device is cheap, practical.

Claims (1)

1., based on the full attitude determination method of underwater carrier that earth magnetism gradient tensor is measured, it is characterized in that:

(1) under carrier system, three-dimensional cartesian coordinate system x is set up _b, y _b, z _b, the orthogonal magnetometer of two three axles is placed on x respectively _ba, B symmetrical centered by initial point of axle two point, measures magnetic-field component (h respectively ₄, h ₅, h ₆) and (h ₁, h ₂, h ₃); Another is placed in y respectively to two axle orthogonal magnetometers _bc, D symmetrical centered by initial point of axle two point, measures magnetic-field component (h respectively ₉, h ₁₀) and (h ₇, h ₈), the sensitive axes direction of all magnetometers is consistent, base area magnetic gradient tensor G ^b5 components and with magnetic-field component h ₁~ h ₁₀relational expression,

$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.$

Obtain 5 components of magnetic gradient tensor, L _xand L _ybe respectively x _band y _bgradiometry base length on direction;

(2) according to the indicating positions of inertia/earth magnetism integrated navigation system, from the earth magnetism gradient tensor database prestored, earth magnetism gradient tensor G under Department of Geography is extracted ⁿ5 isolated components and

(3) earth magnetism gradient tensor component under carrier system step 1,2 obtained with earth magnetism gradient tensor component under Department of Geography and substitute into about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations:

$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{(q_0^2+q_1^2-q_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}$

$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{q}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{q}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{(q_0^2-q_1^2+q_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{q}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{q}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+(q_0^2-q_1^2+q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_2{q}_3-q_0{q}_1)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[(q_0^2-q_1^2+q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$

$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$

About q ₀, q ₁, q ₂and q ₃nonlinear System of Equations can by G ⁿand G ^brelational expression obtain, wherein T representing matrix transposition, the transformation matrix that geography is tied to carrier system, and the transformation matrix of carrier system and Department of Geography, and in conjunction with q ₀, q ₁, q ₂and q ₃restriction relation newton-decline method is utilized to solve about q ₀, q ₁, q ₂and q ₃nonlinear System of Equations;

(4) hypercomplex number meeting iterated conditional will obtained in step (3) with substitute into the transformation matrix of carrier system and Department of Geography,

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right],$

Obtain matrix element (i, j=1,2,3),

Obtain the estimated value of pitching angle theta;

Course angle estimated value in the following several ways: when ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}>0$ Time, $\widehat{\mathrm{\&psi;}}={90}^0;$ When ${\hat{c}}_{22}\&RightArrow;0,{\hat{c}}_{12}<0$ Time, $\widehat{\mathrm{\&psi;}}=-{90}^0;$ When ${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$ Time, when ${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$ Time,

Roll angle estimated value divide 3 kinds of situations: work as c ₃₃during > 0, work as c ₃₃< 0, time, when time,