CN105486275A - Magnetic declination calculation method based on nine-axis inertial measurement unit - Google Patents

Abstract

The invention discloses a magnetic declination calculation method based on a nine-axis inertial measurement unit. The magnetic declination calculation method comprises the following steps: calculating a course angle HG relative to the true north at the time by using a triaxial fiber-optic gyroscope, a triaxial MEMS accelerometer and an inertial navigation parsed alignment method; then calculating a magnetic course angle Hm relative to the magnetic north at the time by using the triaxial MEMS accelerometer and a triaxial MEMS magnetometer; and calculating a difference between the course angle HG relative to the true north and the magnetic course angle Hm relative to the magnetic north so as to obtain the magnetic declination delta H at the current position at the time. The method provided by the invention mainly calculates the magnetic declination in time by using an inertial sensor and algorithm, can accurately calculate magnetic declination at any time under the conditions of any latitude and longitude and any carrier attitude, and is easily realizable through programming in engineering.

Description

A kind of magnetic declination computing method based on 9 axle Inertial Measurement Units

Technical field

The present invention relates to a kind of magnetic declination computing method, be specifically related to a kind of magnetic declination computing method based on 9 axle Inertial Measurement Units.

Background technology

Magnetic declination is the angle geographically when magnetic north and geographical north; When carrying out seeking north with magnetometer, although precision is higher, magnetometer can only measure the direction when magnetic north, and the relative real north in the direction of magnetic north exists deviation (that is, magnetic declination), brings error can to the north of seeking of navigational system.

Under normal circumstances, tellurian magnetic declination is all calculate on schedule, announces, and in order to searching use, therefore needs just can know by tabling look-up the magnetic declination under now longitude and latitude; Patent " permanent magnet declination measuring meter and measuring method " (application number 200810121769.3) provides a kind of permanent magnet that utilizes to measure the method for magnetic declination, and the method adopts permanent magnet to measure magnetic declination.

Summary of the invention

In order to solve the technical matters existed in prior art, the invention provides a kind of magnetic declination computing method based on 9 axle Inertial Measurement Units.

For solving the problems of the technologies described above, the invention provides a kind of magnetic declination computing method based on 9 axle Inertial Measurement Units, it is characterized in that, comprise the following steps:

First, utilize three axis optical fibre gyro and 3 axis MEMS accelerometer, the method for being aimed at by inertial navigation analytic expression, calculate now relative to the course angle H in geographical north_g;

Then, 3 axis MEMS accelerometer and 3 axis MEMS magnetometer is utilized to calculate now relative to the magnetic heading angle H of magnetic north_m;

Finally, the course angle H relative to geographical north is calculated_gwith the magnetic heading angle H relative to magnetic north_mdifference be exactly the magnetic declination Δ H of now position.

Course angle H_gcomputing method be:

When carrier stationary, calculate initial strap-down matrix T, wherein in strap-down matrix, nine elements are respectively:

$\begin{array}{ccc}{T}_{31}=-\frac{g_x^b}{g}& T_{32}=-\frac{r}{g}& T_{33}=-\frac{r}{g}\end{array}$

In formula:

for the latitude of locality;

ω_iefor earth rotation angular speed;

G is local gravitational acceleration;

be respectively the output angle speed of x, y, z three axis optical fibre gyro under carrier coordinate system;

be respectively the output acceleration of x, y, z 3 axis MEMS accelerometer under carrier coordinate system;

$H_{Gmain}=arctan\left(\frac{-T_{12}}{T_{22}}\right)$

H_gmainfor course angle main value;

Calculate course angle H_gfor:

$H_G=\left\{\begin{array}{cc}{H}_{Gmain}& T_{22}\≤0,H_{Gmain}\≥0\\ H_{Gmain}+360& T_{22}\≤0,H_{Gmain}\≤0\\ H_{Gmain}+180& T_{22}\≥0\end{array}\right..$

The expression formula of strap-down matrix T is as follows:

$T=\left[\begin{array}{ccc}{\mathrm{cos\γcosH}}_G-{\mathrm{sin\γsin\θsinH}}_G& -{\mathrm{cos\θsinH}}_G& {\mathrm{sin\γcosH}}_G+{\mathrm{cos\γsin\θsinH}}_G\\ {\mathrm{cos\γsinH}}_G+{\mathrm{sin\γsin\θcosH}}_G& {\mathrm{cos\θcosH}}_G& {\mathrm{sin\γsinH}}_G-{\mathrm{cos\γsin\θcosH}}_G\\ -\sin \mathrm{\γ}cos\mathrm{\θ}& \sin \mathrm{\θ}& cos\mathrm{\γ}cos\mathrm{\θ}\end{array}\right]$

In formula: H_gfor course angle, θ is roll angle, and γ is the angle of pitch.

Magnetic heading angle H_mcomputing method be:

When carrier is in stationary state, under geographic coordinate system, the measured value of each axis accelerometer is A_t=[00g]; Now the measured value of carrier mems accelerometer is A_b=[a_xa_ya_z],

By A_b=T^-1a_t, calculate:

$\mathrm{\θ}=arcsin\frac{a_y}{g},\mathrm{\γ}=arccos\frac{a_z}{gcos\mathrm{\θ}};$

Under geographic coordinate system, the measured value of the magnetic field intensity of each axle is M_t=[0M0], now the measured value of the 3 axis MEMS magnetometer of carrier is M_b=[m_xm_ym_z];

By M_b=T^-1m_t, calculate magnetic heading angle H_m:

$H_m=arctan(-\frac{m_{x}cos\mathrm{\γ}+m_{z}sin\mathrm{\γ}}{m_{z}cos\mathrm{\γ}\sin \mathrm{\θ}-m_{x}sin\mathrm{\γ}sin\mathrm{\θ}-m_{y}cos\mathrm{\θ}}).$

The computing method of magnetic declination Δ H are:

ΔH＝H_m-H_G

Δ H>0, represents that magnetic declination is now inclined westwards; Δ H<0, represents that magnetic declination is now inclined eastwards.

The beneficial effect that the present invention reaches:

Method of the present invention calculates magnetic declination in real time mainly through inertial sensor and algorithm, the magnetic declination under accurately can calculating any moment, any longitude and latitude in real time, under any attitude of carrier, and is convenient to pass through programming realization in engineering.

Embodiment

The invention will be further described below.Following examples only for technical scheme of the present invention is clearly described, and can not limit the scope of the invention with this.

Principle of the present invention mainly make use of the principle of inertial navigation system initial alignment; First utilize three axis optical fibre gyro and 3 axis MEMS accelerometer, the method for being aimed at by inertial navigation analytic expression, just can accurately calculate now relative to the course angle H in geographical north_g; Then 3 axis MEMS accelerometer and 3 axis MEMS magnetometer is utilized just can accurately to calculate now relative to the magnetic heading angle H of magnetic north_m; Finally, relative to the course angle H in geographical north_gwith the magnetic heading angle H relative to magnetic north_mdifference be exactly the magnetic declination Δ H of now position.

Specific embodiment of the invention computing method are as follows:

1) course angle H_gcomputing method;

The method of being aimed at by inertial navigation analytic expression, when carrier stationary, can calculate initial strap-down matrix T, wherein in strap-down matrix T, nine elements are respectively:

$\begin{array}{ccc}{T}_{31}=-\frac{g_x^b}{g}& T_{32}=-\frac{g_y^b}{g}& T_{33}=-\frac{g_z^b}{g}\end{array}$

In formula:

for the latitude of locality;

ω_iefor earth rotation angular speed;

G is local gravitational acceleration;

be respectively the output angle speed of x, y, z three axis optical fibre gyro under carrier coordinate system.

be respectively the output acceleration of x, y, z 3 axis MEMS accelerometer under carrier coordinate system.

Therefore, can calculate now relative to the course angle H in geographical north_g, computing method are as follows:

$H_{Gmain}=arctan\left(\frac{-T_{12}}{T_{22}}\right)$

H_gmainfor course angle main value; Calculate course angle H_gfor:

$H_G=\left\{\begin{array}{cc}{H}_{Gmain}& T_{22}\&le;0,H_{Gmain}\&GreaterEqual;0\\ H_{Gmain}+360& T_{22}\&le;0,H_{Gmain}\&le;0\\ H_{Gmain}+180& T_{22}\&GreaterEqual;0\end{array}\right.$

2) magnetic heading angle H_mcomputing method;

The expression formula of strap-down matrix T is as follows:

$T=\left[\begin{array}{ccc}{\mathrm{cos\&gamma;cosH}}_G-{\mathrm{sin\&gamma;sin\&theta;sinH}}_G& -{\mathrm{cos\&theta;sinH}}_G& {\mathrm{sin\&gamma;cosH}}_G+{\mathrm{cos\&gamma;sin\&theta;sinH}}_G\\ {\mathrm{cos\&gamma;sinH}}_G+{\mathrm{sin\&gamma;cos\&theta;cosH}}_G& {\mathrm{cos\&theta;cosH}}_G& {\mathrm{sin\&gamma;sinH}}_G-{\mathrm{cos\&gamma;sin\&theta;cosH}}_G\\ -\sin \mathrm{\&gamma;}cos\mathrm{\&theta;}& \sin \mathrm{\&theta;}& cos\mathrm{\&gamma;}cos\mathrm{\&theta;}\end{array}\right]$

In formula: H_gfor course angle, θ is roll angle, and γ is the angle of pitch.

When carrier is in stationary state, under geographic coordinate system, the measured value of each axis accelerometer is A_t=[00g]; Now the measured value of carrier mems accelerometer is A_b=[a_xa_ya_z], by A_b=T^-1a_t, calculate:

under geographic coordinate system, the measured value of the magnetic field intensity of each axle is M_t=[0M0], now the measured value of the 3 axis MEMS magnetometer of carrier is M_b=[m_xm_ym_z]; By M_b=T^-1m_t, calculate magnetic heading angle H_m:

$H_m=arctan(-\frac{m_{x}cos\mathrm{\&gamma;}+m_{z}sin\mathrm{\&gamma;}}{m_{z}cos\mathrm{\&gamma;}sin\mathrm{\&theta;}-m_{x}sin\mathrm{\&gamma;}sin\mathrm{\&theta;}-m_{y}cos\mathrm{\&theta;}})$

3) computing method of magnetic declination Δ H;

By the course angle H calculated above_gwith magnetic heading angle H_mdifference be magnetic declination Δ H now:

ΔH＝H_m-H_G

Δ H>0, represents that magnetic declination is now inclined westwards; Δ H<0, represents that magnetic declination is now inclined eastwards.

The above is only the preferred embodiment of the present invention; it should be pointed out that for those skilled in the art, under the prerequisite not departing from the technology of the present invention principle; can also make some improvement and distortion, these improve and distortion also should be considered as protection scope of the present invention.

Claims (5)

1., based on magnetic declination computing method for 9 axle Inertial Measurement Units, it is characterized in that, comprise the following steps:

First, utilize three axis optical fibre gyro and 3 axis MEMS accelerometer, the method for being aimed at by inertial navigation analytic expression, calculate now relative to the course angle H in geographical north_g;

Then, 3 axis MEMS accelerometer and 3 axis MEMS magnetometer is utilized to calculate now relative to the magnetic heading angle H of magnetic north_m;

Finally, the course angle H relative to geographical north is calculated_gwith the magnetic heading angle H relative to magnetic north_mdifference be exactly the magnetic declination Δ H of now position.

2. a kind of magnetic declination computing method based on 9 axle Inertial Measurement Units according to claim 1, is characterized in that, course angle H_gcomputing method be:

When carrier stationary, calculate initial strap-down matrix T, wherein in strap-down matrix, nine elements are respectively:

$\begin{array}{ccc}{T}_{31}=-\frac{g_x^b}{g}& T_{32}=-\frac{g_y^b}{g}& T_{33}=-\frac{g_z^b}{g}\end{array}$

In formula:

for the latitude of locality;

ω_iefor earth rotation angular speed;

G is local gravitational acceleration;

be respectively the output angle speed of x, y, z three axis optical fibre gyro under carrier coordinate system;

be respectively the output acceleration of x, y, z 3 axis MEMS accelerometer under carrier coordinate system;

${H_G}_{main}=\arctan \left(\frac{-T_{12}}{T_{22}}\right)$

H_gmainfor course angle main value;

Calculate course angle H_gfor:

$H_G=\left\{\begin{array}{cc}{H_G}_{main}& T_{22}\&le;0,{H_G}_{main}\&GreaterEqual;0\\ {H_G}_{main}+360& T_{22}\&le;0,{H_G}_{main}\&le;0\\ {H_G}_{main}+180& T_{22}\&GreaterEqual;0\end{array}\right..$

3. a kind of magnetic declination computing method based on 9 axle Inertial Measurement Units according to claim 2, it is characterized in that, the expression formula of strap-down matrix T is as follows:

$T=\left[\begin{array}{ccc}{\mathrm{cos\&gamma;cosH}}_G-{\mathrm{sin\&gamma;sin\&theta;sinH}}_G& -{\mathrm{cos\&theta;sinH}}_G& {\mathrm{sin\&gamma;cosH}}_G+{\mathrm{cos\&gamma;sin\&theta;sinH}}_G\\ {\mathrm{cos\&gamma;sinH}}_G+{\mathrm{sin\&gamma;sin\&theta;cosH}}_G& {\mathrm{cos\&theta;cosH}}_G& {\mathrm{sin\&gamma;sinH}}_G-{\mathrm{cos\&gamma;sin\&theta;cosH}}_G\\ -\sin \mathrm{\&gamma;}cos\mathrm{\&theta;}& \sin \mathrm{\&theta;}& cos\mathrm{\&gamma;}cos\mathrm{\&theta;}\end{array}\right]$

In formula: H_gfor course angle, θ is roll angle, and γ is the angle of pitch.

4. a kind of magnetic declination computing method based on 9 axle Inertial Measurement Units according to claim 3, is characterized in that, magnetic heading angle H_mcomputing method be:

When carrier is in stationary state, under geographic coordinate system, the measured value of each axis accelerometer is A_t=[00g]; Now the measured value of carrier mems accelerometer is A_b=[a_xa_ya_z],

By A_b=T^-1a_t, calculate:

$\mathrm{\&theta;}=\arcsin \frac{a_y}{g},\mathrm{\&gamma;}=\arccos \frac{a_z}{gcos\mathrm{\&theta;}};$

Under geographic coordinate system, the measured value of the magnetic field intensity of each axle is M_t=[0M0], now the measured value of the 3 axis MEMS magnetometer of carrier is M_b=[m_xm_ym_z];

By M_b=T^-1m_t, calculate magnetic heading angle H_m:

$H_m=\arctan (-\frac{m_{x}cos\mathrm{\&gamma;}+m_{z}sin\mathrm{\&gamma;}}{m_{z}cos\mathrm{\&gamma;}sin\mathrm{\&theta;}-m_{x}sin\mathrm{\&gamma;}sin\mathrm{\&theta;}-m_{y}cos\mathrm{\&theta;}}).$

5. a kind of magnetic declination computing method based on 9 axle Inertial Measurement Units according to claim 4, it is characterized in that, the computing method of magnetic declination Δ H are:

ΔH＝H_m-H_G

Δ H>0, represents that magnetic declination is now inclined westwards; Δ H<0, represents that magnetic declination is now inclined eastwards.