CN108151765B - Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error - Google Patents

Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error Download PDF

Info

Publication number
CN108151765B
CN108151765B CN201711439349.5A CN201711439349A CN108151765B CN 108151765 B CN108151765 B CN 108151765B CN 201711439349 A CN201711439349 A CN 201711439349A CN 108151765 B CN108151765 B CN 108151765B
Authority
CN
China
Prior art keywords
magnetometer
output
information
error
navigation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201711439349.5A
Other languages
Chinese (zh)
Other versions
CN108151765A (en
Inventor
蔚保国
王青江
智奇楠
刘鹏飞
马国驹
贾瑞才
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
CETC 54 Research Institute
Original Assignee
CETC 54 Research Institute
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by CETC 54 Research Institute filed Critical CETC 54 Research Institute
Priority to CN201711439349.5A priority Critical patent/CN108151765B/en
Publication of CN108151765A publication Critical patent/CN108151765A/en
Application granted granted Critical
Publication of CN108151765B publication Critical patent/CN108151765B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a positioning and attitude measuring method for estimating and compensating errors of a magnetometer on line in real time, which aims to solve the technical problems that: aiming at the problem that MINS course information is rapidly diverged when GNSS signals are interrupted in a GNSS/MINS combined navigation system, the magnetometer and external interference magnetic intensity are calibrated in real time by utilizing attitude information provided by the combined navigation system when the GNSS observation condition is good, and the magnetometer information is utilized to assist INS in navigation when the GNSS signals are interrupted, so that the navigation precision of the system is improved.

Description

Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error
Technical Field
The invention relates to a positioning and attitude measuring problem of a GNSS/MINS (Global Navigation Satellite System/Micro inertial Navigation System) integrated Navigation System, in particular to a method for calibrating the error of a magnetometer by using attitude information solved by the GNSS/MINS integrated Navigation System as a reference so as to improve the positioning and attitude measuring precision of the GNSS/MINS integrated Navigation System when GNSS signals are interrupted.
Background
The GNSS has the advantages of global, rapid and all-weather positioning, is the most widely applied positioning mode in the current positioning field, and the application field covers various fields such as navigation, spaceflight, vehicle navigation and the like, however, in modern cities, high-rise forests, overpasses, tunnels, green trees and the like seriously shield GNSS signals, and a large amount of signal multipath interference is caused. In addition, the existence of various communication facilities in cities and the extremely bad electromagnetic environment cause various intentional and unintentional interferences to bring great challenges to the signal receiving capability of the GNSS receiver. The INS utilizes angular rate and specific force information provided by a gyroscope and an accelerometer to calculate information such as position, speed and attitude of a carrier, the information is not interfered by the outside, and the navigation positioning calculation precision is diverged along with time due to the influence of error accumulation. The GNSS and the INS have good complementarity, and the combination of the two can provide more accurate and reliable navigation positioning results compared to a single system. In the GNSS/INS combined navigation, the GNSS provides the updating information required by inertial navigation, so that the divergence of inertial navigation information is restrained, and when the GNSS is interrupted due to the shielding or interference of signals, the inertial navigation can still continue to work, so that the reliability and the robustness of the system are improved.
The combined application mode of the satellite navigation system (GNSS) and the Inertial Navigation System (INS) can greatly improve the usability of the existing navigation system and effectively enhance the dynamic performance and the anti-jamming capability of military equipment. Currently, GNSS/INS integrated navigation systems have gained some applications, particularly in the military field. Since the inertial sensors that make up the INS are generally expensive, the scope of application of GNSS/INS technology is limited. For military equipment, a high-performance and low-cost navigation technology has very urgent requirements in a plurality of application fields such as vehicles, airplanes, ships, missiles, informationized ammunition, micro-satellites and the like so as to realize high reliability, high anti-interference capability and accurate guidance capability of a system.
With the development of semiconductor integrated circuit Micro-machining technology and ultra-precision machining technology, MEMS (Micro-Electrical-Mechanical System) sensors have been developed vigorously. The MEMS IMU has the advantages of small volume, light weight, low power consumption, low cost and the like. Therefore, the inertial navigation technology gradually enters the civil fields such as vehicle navigation, unmanned aerial vehicle navigation positioning and attitude determination and the like. The advent of MINS technology has made possible the widespread use of low cost GNSS/INS combinatorial technology.
The MINS has advantages of small volume, light weight, low power consumption, etc., but the navigation result includes a large error due to the limitation of the manufacturing process and the influence of the noise of the sensor itself. In the GNSS/MINS integrated navigation system, when the GNSS system cannot work due to occlusion or interference, the MINS error may diverge at a fast speed, for example, a zero offset error of a gyroscope may cause an attitude angle to diverge in a first power of time, and further cause a speed error to diverge in a second power of time, and finally cause position information to diverge in a third power of time. Therefore, divergence of inertial navigation information must be suppressed by a certain method in the GNSS signal interruption process, a magnetometer is usually integrated in the current MINS system, geomagnetic field information is usually stably distributed at each position of the earth, and navigation information of a carrier can be obtained by using geomagnetic field information obtained by measurement of the magnetometer and performing appropriate error correction, so that the positioning and attitude measurement accuracy of the system when GNSS signals are interrupted can be improved by adding magnetometer information into the GNSS/MINS combined navigation system.
The premise of obtaining the course by using the magnetometer is that the geomagnetic field where the magnetometer is located is not interfered by the outside, but in actual vehicle navigation application, the magnetometer is affected by an interference magnetic field generated by a metal structure of a vehicle, generally speaking, the direction of the interference magnetic field generated by the vehicle structure is fixed and unchanged relative to the direction of a vehicle body, and in actual vehicle navigation application, because the MEMS INS is fixedly connected with the vehicle body, the projection of the interference magnetic field in the MEMS INS coordinate system is also fixed and unchanged, so that the interference magnetic field can be eliminated. Besides errors caused by disturbing magnetic fields, the magnetometer itself has errors due to manufacturing process limitations, and deterministic errors include zero offset, scale factor, cross-axis coupling. During the GNSS/INS integrated navigation, the attitude information of the carrier under a geographic coordinate system can be acquired in real time, and the output of the magnetometer can be acquired at the same time, so that the absolute attitude information of the carrier can be utilized to calibrate the error of the magnetometer, and when the GNSS signal is interrupted for a long time, the course information of the integrated navigation system can be assisted by the magnetometer information after calibration, so that the quick dispersion of inertial navigation information is inhibited, and the accuracy of positioning and attitude measurement is improved.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: aiming at the problem that MINS course information is rapidly diverged when GNSS signals are interrupted in a GNSS/MINS combined navigation system, the magnetometer and external interference magnetic intensity are calibrated in real time by utilizing attitude information provided by the combined navigation system when the GNSS observation condition is good, and the magnetometer information is utilized to assist INS in navigation when the GNSS signals are interrupted, so that the navigation precision of the system is improved.
The main content of the invention is as follows:
a positioning and attitude measuring method for online real-time estimation and compensation of errors of a magnetometer comprises the following steps:
step 1: firstly, initializing a GNSS/MINS integrated navigation system, wherein the initialization comprises the initialization of a position and an attitude;
step 2: recording data required for estimating errors of the magnetometer in the real-time navigation process, wherein the data comprises real-time output of the magnetometer and theoretical output values of the magnetometer;
and step 3: calculating errors of the magnetometer in real time by a least square method according to the real-time output of the magnetometer and the theoretical output value of the magnetometer recorded in the step 2, wherein the errors of the magnetometer comprise zero offset of the magnetometer, a scale factor and a quadrature axis coupling error;
and 4, step 4: when the GNSS signal is interrupted, firstly, the error of the magnetometer calculated in the step 3 is used for compensating the output of the magnetometer, and then the compensated magnetometer information is used for assisting the course of the integrated navigation system.
The method of step 2 is as follows:
the mathematical model of the magnetometer was set to:
Figure BDA0001526357570000041
or:
Figure BDA0001526357570000042
wherein M represents the error matrix of the magnetometer, where the diagonal elements correspond to the scale factor error of the magnetometer and the off-diagonal elements represent the cross-axis coupling error of the magnetometer, bmiRepresents the zero offset error of the i-axis of the magnetometer, i is x, y, z;
setting the attitude information of the carrier at a certain moment to be [ phi ]1θ1ψ1]At that time, the output of the magnetometerThe theoretical values of (a) are expressed as:
Figure BDA0001526357570000043
wherein m is1' represents the projection of the local magnetic field in the b-system, i.e. the theoretical output value of the magnetometer, mbx1,mby1,mbz1Which in turn represent the components of the local magnetic field at the magnetometers x, y, z,
Figure BDA0001526357570000044
the direction cosine matrix from the carrier coordinate system to the navigation coordinate system, c theta, c phi and c psi sequentially represent cosine values of a roll angle, a pitch angle and a course angle, s theta, s phi and s psi sequentially represent sine values of the roll angle, the pitch angle and the course angle, mnThe projection of the geomagnetic field in a navigation coordinate system;
obtaining theoretical values output by a plurality of groups of magnetometers:
Figure BDA0001526357570000045
simultaneous record carrier corresponding [ phi ]iθiψi]The actual output of the magnetometer at an angle constitutes an observation matrix u as shown belowi:
Figure BDA0001526357570000046
Wherein m isxi,myi,mziSequentially representing the outputs of x, y and z axes of the magnetometer, and recording the actual outputs of i groups of independent magnetometers as:
Figure BDA0001526357570000051
wherein, the error of the magnetometer calculated in real time by the least square method in the step 3 is specifically as follows:
and (3) according to the real-time output of the magnetometer and the theoretical output value of the magnetometer recorded in the step (2), constructing a coefficient matrix of least squares as follows:
Figure BDA0001526357570000052
the least squares observation matrix is as follows:
U＝[u1u2... ui]；
and calculating an error matrix by a least square method:
M＝U·AT·(AAT)-1；
and calculating the error of the magnetometer in real time according to the error matrix.
Wherein, the step 4 is specifically as follows:
when the GNSS signal is interrupted, firstly, the magnetometer error obtained by calculation in the step 3 is used for compensating the output of the magnetometer, and then the compensated magnetometer is used for outputting and calculating course information, wherein the specific calculation method of the course information comprises the following steps:
Figure BDA0001526357570000053
wherein the content of the first and second substances,
Figure BDA0001526357570000054
theta and phi are respectively a roll angle and a pitch angle of the carrier in real time; psimagFor outputting the calculated course information of the carrier by means of a magnetometer, gammamThe local declination is obtained by referring to data by combining the latitude information; mx,My,MzThe outputs of the compensated magnetometer in X, Y and Z axes are respectively.
Carrying out weighted average on the course information calculated by using the magnetometer and the inertial navigation to obtain the course information with higher precision, wherein the calculation method comprises the following steps:
Figure BDA0001526357570000061
wherein psiintTo use a magnetometer and an inertial navigation meterThe calculated course values are combined to obtain a calculated course value psiinsHeading information calculated for inertial navigation, andmagheading information, σ, calculated for the magnetometerins、σmagThe variance of the heading calculated for inertial navigation and magnetometer.
The method realizes real-time online calibration of the magnetometer, calibrates the original magnetometer by using high-precision attitude information of GNSS signals as reference values when the GNSS signals are good, and can effectively improve the real-time navigation efficiency.
Drawings
FIG. 1 is a schematic diagram of a magnetometer calibration and assisted initial alignment procedure.
Detailed Description
The invention will be further described with reference to specific embodiments and the accompanying drawings in which:
the method comprises the following steps: initial alignment is performed first before vehicle navigation. In the vehicle-mounted navigation application, the combined navigation receiver is fixedly connected with a vehicle body, the GNSS/MINS combined navigation receiver is initially aligned before navigation, the initial alignment of the position can be obtained through longitude, latitude and geodetic height information of a GNSS, the horizontal attitude information can be obtained through calculation of an accelerometer, and the course information can be obtained through a magnetometer or double antennas. If the heading information is the initial alignment of MINS attitude assisted by the magnetometer, the magnetometer needs to be corrected before alignment to eliminate the influence of the interfering magnetic field.
The specific flow of the initial alignment is as follows:
1. and the vehicle is in a static state before navigation starts, the GNSS/MINS combined navigation receiver is powered on, and the output of the INS day-oriented gyroscope in the combined navigation receiver is continuously recorded after the combined navigation receiver is powered on. The vehicle is rotated for a circle in an open place, even if the heading of the vehicle traverses 0-360 degrees. The method for judging the start of the rotation of the vehicle is to detect whether the output of the gyro is larger than a set threshold (for example, 10 DEG/s), and if the output of the gyro is larger than the threshold, the vehicle is considered to have started the maneuver. The vehicle continuously records the output of the daily gyro while maneuvering, and the output of the daily gyro is less than a set threshold (such as 2 degrees/h) continuously for 10 seconds, and the vehicle is considered to be motorized and is in a static state.
The absolute values of the maximum value and the minimum value of each axis of the magnetometer in the horizontal direction in the GNSS/MINS combined navigation receiver are approximately equal if no interference of an external magnetic field exists. However, due to the presence of external disturbing magnetic fields, the absolute values of the maximum and minimum output values of the magnetometer horizontal axes are generally not equal.
In the step, the maximum value and the minimum value Mx of the output of each axis of the magnetometers X, Y and Z in the course of traversing the course by 0-360 DEG are obtainedmax、Mxmin，Mymax、Mymin，Mzmax、MzminAnd calculating correction values of all axes of the magnetometer according to the maximum value and the minimum value of the output of the magnetometer recorded in the maneuvering process of the vehicle, and calculating to obtain compensation values of X, Y and Z axes of the magnetometer as follows:
Figure BDA0001526357570000071
Figure BDA0001526357570000072
Figure BDA0001526357570000073
2. after the vehicle course traverses 0-360 degrees, the vehicle is made to be stationary for a certain period of time (such as 90s), and the outputs of X, Y and Z axes of the accelerometer are continuously recorded in the stationary process. Calculating to obtain the average value f of the outputs of the X, Y and Z axes of the accelerometerx,fy,fzAnd calculating the roll angle and the pitch angle of the carrier by using the following two formulas:
φ＝atan2(fy,fz)
Figure BDA0001526357570000074
wherein f isx、fy、fzWhich in turn represent the average of the outputs of the accelerometers in the X, Y, Z axes during periods of vehicle inactivity.
3. The output of the magnetometer is compensated by the compensation values biasx, biasy and biasz of each axis of the magnetometer, which are obtained by calculation, so as to obtain the compensated output of the magnetometer, and the compensation method comprises the following steps:
Figure BDA0001526357570000075
Figure BDA0001526357570000081
Figure BDA0001526357570000082
in the above formula
Figure BDA0001526357570000083
Sequentially the original output of the x, y and z axes of the magnetometer,
Figure BDA0001526357570000084
magnetic in turn
And (4) correction values output by the x, y and z axes of the intensity meter.
4. And calculating the average value of the output of the compensated magnetometer, continuously recording the output of the accelerometer in the static process, and solving the average value of the output of the magnetometer. The mean values of X, Y and Z axes of the magnetometer after correction are as follows in sequence:
Figure BDA0001526357570000085
and calculating the course information of the carrier according to the following formula:
Figure BDA0001526357570000086
wherein the content of the first and second substances,
Figure BDA0001526357570000087
γmto initially align the local declination, its value can be consulted in conjunction with latitude informationThus obtaining the product.
Step two: after the initial alignment is finished, the navigation state is entered, and different numbers of state vectors can be selected according to different application scenes of Kalman filtering algorithm. In the embodiment, 23 parameters of attitude errors (roll, pitch and course), position errors (longitude, latitude and altitude), speed errors (east speed, north speed and sky speed), gyro zero offset, accelerometer zero offset, gyro scale factor, accelerometer scale factor, receiver clock error and clock drift are selected as state vectors of Kalman filtering. The state vector is as follows:
Figure BDA0001526357570000088
in the above formula psiN,ψE,ψDRepresenting the roll, pitch and course errors of the attitude in sequence; delta vN,δvE,δvUSequentially representing the speed errors of north direction, east direction and sky direction; delta rN,δrE,δrDSequentially representing latitude, longitude and elevation errors;
Figure BDA0001526357570000089
sequentially represents the zero offset of X, Y and Z axes of the gyroscope,
Figure BDA00015263575700000810
sequentially represents the zero offset of X, Y and Z axes of the accelerometers,
Figure BDA0001526357570000091
sequentially representing the scale factor errors of the X, Y and Z axes of the gyroscope;
Figure BDA0001526357570000092
sequentially representing the proportional factor errors of the X, Y and Z axes of the accelerometer; δ tu,δtruIn turn, the clock offset and the clock drift of the receiver.
The kalman filter combining process includes two processes of prediction and update. The prediction process includes prediction of a state vector and prediction of a covariance matrix, and the update process includes update of a state vector and covarianceAnd updating the array, namely performing feedback compensation on the original output of the gyroscope and the accelerometer after zero offset and scale factor errors of all axes of the gyroscope and the accelerometer are obtained through Kalman filtering estimation in the updating process. The attitude information phi theta psi of the carrier can be continuously acquired in the navigation process]And recording the output u of the magnetometer under different postures, and continuously establishing a least square equation to estimate the sensor error of the magnetometer in real time. When the least square observed quantity is constructed, in order to increase observability of an equation, the observed quantity of the magnetometer under the scene with larger attitude difference is preferentially selected. Recording a first set of observations u, e.g. in a least squares observation equation1The attitude of the time carrier is phi1θ1ψ1]Recording a second set of observations u if the heading of the vehicle changes by more than 5 DEG2Multiple sets of data are recorded for adjustment calculations in the same way. If the data participating in adjustment exceeds 20 groups, the data recorded firstly is removed, the latest recorded data is added in for estimation, and the purpose of updating is to obtain the latest data capable of more accurately reflecting the local magnetic field intensity.
And estimating errors of the magnetometer in real time, wherein the estimated errors of the magnetometer comprise zero offset of the magnetometer, a scale factor and cross-axis coupling errors. The mathematical model in which the magnetometer outputs can be expressed as:
Figure BDA0001526357570000093
or:
Figure BDA0001526357570000094
in the above formula, the M matrix represents the error matrix of the magnetometer, where the diagonal elements correspond to the scale factor error of the magnetometer and the off-diagonal elements represent the cross-axis coupling error of the magnetometer, bmi(i ═ x, y, z) represents the zero bias error of the i axis of the magnetometer. In the process of running test, when GNSS signals exist, the GNSS/MINS combined navigation system can accurately determine the attitude information of the carrier, namely the roll phi information, the pitch theta information and the heading psi information. Suppose that the attitude information of the carrier at a certain time is [ phi ]1θ1ψ1]The intensity of the magnetic flux projected onto the x, y, z axes of the magnetometer at that instant may be expressed as:
Figure BDA0001526357570000101
recording different angles phi of the carrier during its traveliθiψi]The projection of the time-of-day magnetic field in the magnetometer coordinate system, i sets of independent magnetometer observations, are as follows:
Figure BDA0001526357570000102
simultaneous record carrier corresponding [ phi ]iθiψi]The output at an angle constitutes an observation matrix u as shown belowi
Figure BDA0001526357570000103
Step three: the error of the magnetometer is estimated in real time. The coefficient matrix for least squares is constructed as follows:
Figure BDA0001526357570000104
the observation matrix is as follows: u ═ U1u2... ui]The error matrix can be calculated by the least square method:
M＝U·AT·(AAT)-1
step four: after the satellite signal is interrupted, Kalman filtering still carries out prediction, the possibility that the magnetometer is interfered by the outside world is lower because the magnetic field is relatively stable in a short time, the course information calculated by the magnetometer is relatively stable, and at the moment, the course information calculated by the magnetometer and inertial navigation can be used for carrying out weighted average to obtain the course information with higher precision. The calculation method is as follows:
Figure BDA0001526357570000105
in the above formulainsHeading information calculated for inertial navigation, andmagheading information, σ, calculated for the magnetometerins、σmagThe variance of the heading calculated for inertial navigation and magnetometer.
The steps finish the course that the magnetometer is calibrated by utilizing the observation information when the GNSS observation condition is good in the GNSS/MINS system, and the course is assisted by utilizing the calibrated magnetometer information when the GNSS signal is weak.

Claims (3)

1. A positioning and attitude-measuring method for online real-time estimation and compensation of errors of a magnetometer is characterized in that,
the method comprises the following steps:
step 1: firstly, initializing a GNSS/MINS integrated navigation system, wherein the initialization comprises the initialization of a position and an attitude;
step 2: recording data required for estimating errors of the magnetometer in the real-time navigation process, wherein the data comprises real-time output of the magnetometer and theoretical output values of the magnetometer; the method specifically comprises the following steps:
the mathematical model of the magnetometer was set to:
Figure FDA0002407386620000011
or:
Figure FDA0002407386620000012
wherein M represents the error matrix of the magnetometer, where the diagonal elements correspond to the scale factor error of the magnetometer and the off-diagonal elements represent the cross-axis coupling error of the magnetometer, bmx,bmy,bmzRespectively representing zero offset errors of x, y and z axes of the magnetometer;
setting the attitude information of the carrier at a certain moment to be [ phi ]1θ1ψ1]Then the theoretical value of the magnetometer output at that time is expressed as:
Figure FDA0002407386620000013
wherein m'1Representing the projection of the local magnetic field in the b-system, i.e. the theoretical output value of the magnetometer, mbx1,mby1,mbz1Which in turn represent the components of the local magnetic field at the magnetometers x, y, z,
Figure FDA0002407386620000014
the direction cosine matrix from the carrier coordinate system to the navigation coordinate system, c theta, c phi and c psi sequentially represent cosine values of a roll angle, a pitch angle and a course angle, s theta, s phi and s psi sequentially represent sine values of the roll angle, the pitch angle and the course angle, mnThe projection of the geomagnetic field in a navigation coordinate system;
obtaining theoretical values output by a plurality of groups of magnetometers:
Figure FDA0002407386620000021
simultaneous record carrier corresponding [ phi ]iθiψi]The actual output of the magnetometer at an angle constitutes an observation matrix u as shown belowi:
Figure FDA0002407386620000022
Wherein m isxi,myi,mziSequentially representing the outputs of x, y and z axes of the magnetometer, and recording the actual outputs of i groups of independent magnetometers as:
Figure FDA0002407386620000023
and step 3: calculating errors of the magnetometer in real time by a least square method according to the real-time output of the magnetometer and the theoretical output value of the magnetometer recorded in the step 2, wherein the errors of the magnetometer comprise zero offset of the magnetometer, a scale factor and a quadrature axis coupling error;
and 4, step 4: when the GNSS signal is interrupted, firstly, the error of the magnetometer calculated in the step 3 is used for compensating the output of the magnetometer, and then the compensated magnetometer information is used for assisting the course of the integrated navigation system.
2. The method for positioning and attitude measurement for online real-time estimation and compensation of errors of a magnetometer according to claim 1, wherein the real-time calculation of the errors of the magnetometer by a least square method in step 3 specifically comprises:
and (3) according to the real-time output of the magnetometer and the theoretical output value of the magnetometer recorded in the step (2), constructing a coefficient matrix of least squares as follows:
Figure FDA0002407386620000024
the least squares observation matrix is as follows:
U＝[u1u2... ui]；
and calculating an error matrix by a least square method:
M＝U·AT·(AAT)-1；
and calculating the error of the magnetometer in real time according to the error matrix.
3. The method for positioning and attitude measurement by online real-time estimation and compensation of errors of a magnetometer according to claim 1, wherein the step 4 specifically comprises:
when the GNSS signal is interrupted, firstly, the magnetometer error obtained by calculation in the step 3 is used for compensating the output of the magnetometer, and then the compensated magnetometer is used for outputting and calculating course information, wherein the specific calculation method of the course information comprises the following steps:
wherein the content of the first and second substances,
Figure FDA0002407386620000032
theta and phi are respectively a roll angle and a pitch angle of the carrier in real time; psimagFor outputting the calculated course information of the carrier by means of a magnetometer, gammamThe local declination is obtained by referring to data by combining the latitude information; mx,My,MzThe outputs of X, Y and Z axes output by the compensated magnetometer are respectively;
carrying out weighted average on the course information calculated by using the magnetometer and the inertial navigation to obtain the course information with higher precision, wherein the calculation method comprises the following steps:
Figure FDA0002407386620000033
wherein psiintIs a heading value calculated by combining heading values calculated by using a magnetometer and inertial navigationinsHeading information calculated for inertial navigation, andmagheading information, σ, calculated for the magnetometerins、σmagThe variance of the heading calculated for inertial navigation and magnetometer.
CN201711439349.5A 2017-12-27 2017-12-27 Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error Active CN108151765B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711439349.5A CN108151765B (en) 2017-12-27 2017-12-27 Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711439349.5A CN108151765B (en) 2017-12-27 2017-12-27 Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error

Publications (2)

Publication Number Publication Date
CN108151765A CN108151765A (en) 2018-06-12
CN108151765B true CN108151765B (en) 2020-05-08

Family

ID=62463327

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711439349.5A Active CN108151765B (en) 2017-12-27 2017-12-27 Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error

Country Status (1)

Country Link
CN (1) CN108151765B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109959392B (en) * 2019-04-04 2021-05-04 上海扩博智能技术有限公司 Motion sensor calibration method, system, device and medium for intelligent refrigerator
CN112147924A (en) * 2020-08-27 2020-12-29 航天东方红卫星有限公司 High-precision program-controlled task management system
CN112461262A (en) * 2020-11-06 2021-03-09 长沙天仪空间科技研究院有限公司 Device and method for correcting errors of three-axis magnetometer
CN113670334B (en) * 2021-08-06 2024-02-20 广东汇天航空航天科技有限公司 Initial alignment method and device for aerocar

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8275544B1 (en) * 2005-11-21 2012-09-25 Miltec Missiles & Space Magnetically stabilized forward observation platform
CN100487378C (en) * 2007-01-10 2009-05-13 北京航空航天大学 Data blending method of navigation system combined by SINS/GPS micromagnetic compass
US8717009B2 (en) * 2010-10-06 2014-05-06 Apple Inc. Magnetometer calibration
US9285224B2 (en) * 2011-04-06 2016-03-15 Uti Limited Partnership System and method for gyroscope error estimation
KR101833217B1 (en) * 2011-12-07 2018-03-05 삼성전자주식회사 Mobile terminal device for positioning system based on magnetic map and positioning method using the device
US20150019159A1 (en) * 2013-07-15 2015-01-15 Honeywell International Inc. System and method for magnetometer calibration and compensation
CN104165642B (en) * 2014-08-29 2017-02-15 东南大学 Method for directly correcting and compensating course angle of navigation system
CN104730541B (en) * 2015-04-21 2017-09-26 中国电子科技集团公司第五十四研究所 A kind of real-time GNSS/INS integrated navigation signal simulators of navigation based on PXI buses
CN105466458A (en) * 2016-01-06 2016-04-06 中国人民解放军海军航空工程学院 Direct error correction method for regular-hexahedron magnetic gradient tensor system
CN105910623B (en) * 2016-04-14 2018-08-14 中国电子科技集团公司第五十四研究所 The method for carrying out the correction of course using magnetometer assisted GNSS/MINS tight integration systems
CN106125026B (en) * 2016-06-12 2019-02-26 哈尔滨工程大学 A kind of three axis magnetometer total error parameter identification and bearing calibration independent of earth's magnetic field amount
CN107024674B (en) * 2017-05-26 2019-04-26 北京航空航天大学 A kind of magnetometer method for quick field calibration based on least square method of recursion

Also Published As

Publication number Publication date
CN108151765A (en) 2018-06-12

Similar Documents

Publication Publication Date Title
CN107655476B (en) Pedestrian high-precision foot navigation method based on multi-information fusion compensation
CN109459044B (en) GNSS dual-antenna assisted vehicle-mounted MEMS inertial navigation combined navigation method
CN110031882B (en) External measurement information compensation method based on SINS/DVL integrated navigation system
CN109556631B (en) INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares
CN108151765B (en) Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error
CN108594283B (en) Free installation method of GNSS/MEMS inertial integrated navigation system
CN110779521A (en) Multi-source fusion high-precision positioning method and device
CN102169184B (en) Method and device for measuring installation misalignment angle of double-antenna GPS (Global Position System) in integrated navigation system
CN110926468B (en) Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment
CN103389092B (en) A kind of kite balloon airship attitude measuring and measuring method
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN103245360A (en) Autocollimation method of carrier aircraft rotating type strapdown inertial navigation system under shaking base
CN105928515B (en) A kind of UAV Navigation System
US10514261B2 (en) Gyromagnetic geopositioning system
CN111121766A (en) Astronomical and inertial integrated navigation method based on starlight vector
KR101211703B1 (en) Calibration method of the magnetometer error using a line of sight vector and the integrated navigation system using the same
CN110672130A (en) EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle
CN105910623B (en) The method for carrying out the correction of course using magnetometer assisted GNSS/MINS tight integration systems
CN103278165A (en) Remanence-calibration-based autonomous navigation method of magnetic survey and starlight backup based on
CN108225312B (en) Lever arm estimation and compensation method in GNSS/INS loose combination
CN110887472B (en) Polarization-geomagnetic information deep fusion fully-autonomous attitude calculation method
CN105928519B (en) Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer
Zhang et al. A multi-position calibration algorithm for inertial measurement units
CN104501809A (en) Attitude coupling-based strapdown inertial navigation/star sensor integrated navigation method
Sollie et al. Pose estimation of UAVs based on INS aided by two independent low-cost GNSS receivers

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant