CN114440871A - Nine-axis magnetic compass data fusion method based on adaptive complementary filtering - Google Patents

Nine-axis magnetic compass data fusion method based on adaptive complementary filtering Download PDF

Info

Publication number
CN114440871A
CN114440871A CN202111644288.2A CN202111644288A CN114440871A CN 114440871 A CN114440871 A CN 114440871A CN 202111644288 A CN202111644288 A CN 202111644288A CN 114440871 A CN114440871 A CN 114440871A
Authority
CN
China
Prior art keywords
magnetic
axis
data
carrier
target
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.)
Pending
Application number
CN202111644288.2A
Other languages
Chinese (zh)
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.)
Yichang Testing Technique Research Institute
Original Assignee
Yichang Testing Technique 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 Yichang Testing Technique Research Institute filed Critical Yichang Testing Technique Research Institute
Priority to CN202111644288.2A priority Critical patent/CN114440871A/en
Publication of CN114440871A publication Critical patent/CN114440871A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G01C21/1654—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 with electromagnetic compass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • 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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Gyroscopes (AREA)

Abstract

The invention relates to a nine-axis magnetic compass data fusion processing method based on self-adaptive complementary filtering, which comprises the following steps: calculating a system state initial value by using the data of the three-axis magnetic sensor and the data of the three-axis accelerometer in the nine-axis magnetic compass; judging whether magnetic interference exists around the carrier or not through the triaxial magnetic field data, and if the magnetic interference exists, resolving the real-time position of a magnetic interference target source in real time according to triaxial magnetic anomaly data measured by three axes to realize magnetic interference compensation; constructing an error vector by using the magnetic field and the acceleration; solving PI parameters of complementary filtering by fuzzy control; and solving the carrier attitude angle by utilizing the attitude quaternion obtained by the real-time solution of the complementary filtering. According to the invention, high-precision attitude angle output can be realized through self-adaptive complementary filtering data fusion, and when magnetic interference exists around the carrier, the real-time position of a magnetic interference target source can be solved in real time to perform magnetic interference compensation, the finally output attitude angle is not influenced by motion acceleration and external magnetic interference, and errors can not be accumulated along with the increase of time.

Description

Nine-axis magnetic compass data fusion method based on adaptive complementary filtering
Technical Field
The invention belongs to the technical field of navigation, and particularly relates to a nine-axis magnetic compass data fusion method based on self-adaptive complementary filtering.
Background
The nine-axis magnetic compass is composed of a three-axis gyroscope, a three-axis accelerometer, a three-axis magnetic sensor, a single chip microcomputer and corresponding peripheral circuits, and is mainly used for measuring the attitude of a carrier.
Although the attitude angle obtained by integrating the triaxial angular velocity measured by the triaxial gyroscope has high short-time accuracy, errors can be accumulated along with the increase of time due to the defects of low accuracy, large drift and the like of the MEMS gyroscope, while the attitude angle of the carrier measured by the triaxial accelerometer and the triaxial magnetic sensor can not be accumulated along with the increase of time, when the carrier moves, signals of the accelerometer cannot correctly reflect the attitude information of the carrier due to the influence of the motion acceleration. Therefore, a multi-sensor data fusion method needs to be designed, data of a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetic sensor are fused, advantages and disadvantages of the three-axis gyroscope, the three-axis accelerometer and the three-axis magnetic sensor are complemented, a nine-axis magnetic compass data fusion method based on adaptive complementary filtering is established, and an accurate attitude angle is calculated.
Disclosure of Invention
In view of the above, the invention provides a nine-axis magnetic compass data fusion method based on adaptive complementary filtering, which realizes data fusion of a three-axis accelerometer, a three-axis magnetic sensor and a three-axis gyroscope through adaptive complementary filtering, and solves to obtain a high-precision attitude angle.
The invention is realized by the following technical scheme:
a nine-axis magnetic compass data fusion processing method based on self-adaptive complementary filtering comprises the following steps:
the method comprises the following steps: calculating a system state initial value by using the data of the three-axis magnetic sensor and the data of the three-axis accelerometer in the nine-axis magnetic compass;
step two: judging whether magnetic interference exists around the carrier or not through the triaxial magnetic field data, and if the magnetic interference exists, resolving the real-time position of a magnetic interference target source in real time according to triaxial magnetic anomaly data measured by three axes to realize magnetic interference compensation;
step three: constructing an error vector by using the magnetic field and the acceleration;
step four: solving PI parameters of complementary filtering by fuzzy control;
step five: and solving the carrier attitude angle by utilizing the attitude quaternion obtained by the real-time solution of the complementary filtering.
Furthermore, the nine-axis magnetic compass in the first step is mainly composed of a three-axis MEMS gyroscope, a three-axis MEMS accelerometer and a three-axis magnetic sensor.
Further, the method for solving the initial value of the system state in the step one is specifically as follows:
s1.1, calculating to obtain a pitch angle and a roll angle according to a coordinate transformation relation of the gravity acceleration at the initial moment under a carrier coordinate system and a geographic coordinate system;
s1.2, calculating by utilizing a pitch angle, a roll angle and measured triaxial magnetic field data to obtain a horizontal component of a geomagnetic field, and calculating by utilizing an inverse trigonometric function to obtain an azimuth angle;
and S1.3, calculating to obtain the attitude angle quaternion in the initial state through the conversion relation between the attitude angle and the quaternion.
Further, the magnetic interference compensation method in the second step includes the following specific steps:
s2.1: deducing a calculation method of the target position when the target magnetic moment and the magnetic field are known according to the target magnetic dipole model;
s2.2: assuming that the magnetic target and the carrier do relative linear motion in a short time, the variation of the relative distance is equal in the same time, converting the target inversion problem into a nonlinear optimization problem, solving the nonlinear optimization problem through a particle swarm optimization algorithm to obtain a target magnetic moment, and calculating the relative position of the target;
s3.3: and calculating an interference magnetic field generated by the target at the position of the carrier according to the calculated target parameters to realize magnetic interference compensation.
Further, the error vector constructed in the third step is specifically:
s3.1: constructing an error vector from the magnetic field: obtaining a cross product of a magnetic field value under a carrier coordinate system and a value measured by a triaxial magnetic sensor after the triaxial magnetic field is normalized by the geomagnetic field and changing coordinates;
s3.2: and (3) constructing an error vector according to the acceleration: and rotating the gravity vector of the geographic coordinate system to the cross product of the gravity acceleration on the carrier coordinate system and the normalized acceleration value measured by the three-axis accelerometer.
Further, the implementation steps of the adaptive adjustment of the complementary filtering PI parameter in the fourth step are as follows:
s4.1: defining a parameter capable of reflecting the motion state of the system;
s4.2: taking the parameters as the input of fuzzy control, and constructing a fuzzy control rule and an input-output membership function;
s4.3: and adjusting the PI parameter of the system complementary filtering in real time according to the output of the fuzzy control.
Further, the solving of the attitude angle of the carrier in the fifth step is obtained by calculating according to a conversion relation between quaternions and the attitude angle after the attitude quaternion obtained by the system complementary filtering calculation is normalized.
Has the advantages that:
according to the invention, high-precision attitude angle output can be realized through self-adaptive complementary filtering data fusion, and when magnetic interference exists around the carrier, the real-time position of a magnetic interference target source can be solved in real time to perform magnetic interference compensation, the finally output attitude angle is not influenced by motion acceleration and external magnetic interference, and errors can not be accumulated along with the increase of time.
Drawings
FIG. 1 is a flow chart of a system attitude angle calculation;
FIG. 2 is a complementary filter solution logic diagram;
FIG. 3 is a complementary filtering schematic;
FIG. 4 is a graph of a membership function for a fuzzy control input;
FIG. 5 is a graph of a membership function for fuzzy control output;
FIG. 6 is a flow chart of system complementary filtering solution data;
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings.
The nine-axis magnetic compass is mainly composed of a three-axis gyroscope, a three-axis magnetic sensor, a three-axis accelerometer and a corresponding data acquisition and processing system, wherein the three-axis gyroscope is used for measuring three-axis direction angular velocity data, the three-axis magnetic sensor is used for measuring three-axis direction magnetic field data, the three-axis accelerometer is used for measuring three-axis direction acceleration data, and a system attitude angle resolving flow is shown in figure 1.
(1) Calculating a system state initial value by using the data of the three-axis magnetic sensor and the data of the three-axis accelerometer in the nine-axis magnetic compass;
1) calculating a pitch angle and a roll angle:
under the quasi-static condition, the gravity acceleration has no projection component in the horizontal direction in the geographic coordinate system, the accelerometer only has measurement value output in the Z-axis direction, and the measurement value is the earth gravity acceleration g. Then there are:
Figure RE-GDA0003547717660000041
in the formula (1), the first and second groups,
Figure RE-GDA0003547717660000042
for three components of three axes of the gravity acceleration measured by the three-axis accelerometer in a carrier coordinate system, the formula (1) is developed to calculate the pitch beta and the roll angle gamma:
Figure RE-GDA0003547717660000043
Figure RE-GDA0003547717660000044
2) calculating an azimuth angle:
the carrier coordinate system is not coincident with the geographic coordinate system. At this time, components of the earth magnetic field in each axis are superposed, and the heading angle α can be solved only by solving the projections of the components of the earth magnetic field measured by the three-axis magnetic sensor in the three axes of the carrier coordinate system.
Figure RE-GDA0003547717660000051
In the formula (2), [ m ]x my mz]Three-axis three-component, m, of the geomagnetic field measured by the three-axis magnetic sensor in the carrier coordinate systemx' and my' is their projection in the horizontal plane.
3) And calculating to obtain the attitude angle quaternion in the initial state through the conversion relation between the attitude angle and the quaternion.
(2) And (3) solving the real-time position of the magnetic interference target source in real time according to triaxial magnetic anomaly data measured by three axes to realize magnetic interference compensation:
1) deriving the target position at the known target magnetic moment and field:
in the practical application process, the nine-axis magnetic compass is inevitably interfered by various external ferromagnetic substances, the long-term precision of the azimuth angle of the nine-axis magnetic compass is ensured by accurately measuring the geomagnetic field by the magnetic sensor,
based on the classical electromagnetic theory, when the position of the detection point and the target is larger than 2.5 times of the maximum size of the target, the magnetic interference substance at one point in the space can be equivalent to a magnetic dipole model, and the magnetic dipole moment is
Figure RE-GDA0003547717660000052
Then at a distance from the magnetically interfering substance
Figure RE-GDA0003547717660000053
The magnetic potential and the magnetic field are respectively as follows:
Figure RE-GDA0003547717660000054
Figure RE-GDA0003547717660000055
as can be seen from the formula, the three vectors are located in the same plane when viewed as three vectors, that is:
Figure RE-GDA0003547717660000061
wherein k is1And k is2The solving method of (2) is as follows:
substituting equation (7) into equation (6) yields:
Figure RE-GDA0003547717660000062
due to the fact that
Figure RE-GDA0003547717660000063
And
Figure RE-GDA0003547717660000064
non-collinear, geometric principles are available:
Figure RE-GDA0003547717660000065
solving the system of linear equations in equation (9) yields k1And k is2。
Accordingly, the target magnetic moment is known
Figure RE-GDA0003547717660000066
And at a distance target
Figure RE-GDA0003547717660000067
The magnetic field value measured by a triaxial magnetic sensor
Figure RE-GDA0003547717660000068
The magnetic target position can be inverted and recorded as
Figure RE-GDA0003547717660000069
2) Calculating the relative position of the target:
when the nine-axis magnetic compass is arranged on the carrier, the carrier and the magnetic target can be regarded as relatively uniform linear motion in a short time, namely, the variation of the relative distance in the same time
Figure RE-GDA00035477176600000610
The same is true.
In the nine-axis magnetic compass, three-axis magnetic fields measured by the magnetic sensors at equal time intervals are sequentially
Figure RE-GDA00035477176600000611
The positions of the interference targets relative to the carrier are respectively obtained according to the measured three-axis magnetic field inversion
Figure RE-GDA00035477176600000612
Solving the target position and magnetic moment can be finally converted into a nonlinear optimization problem:
Figure RE-GDA00035477176600000613
aiming at the nonlinear optimization problem, a target magnetic moment is obtained by adopting a self-adaptive particle swarm optimization algorithm
Figure RE-GDA00035477176600000614
Then can pass through
Figure RE-GDA00035477176600000615
And inverting the target position.
3) Magnetic interference compensation:
after the target position and the magnetic couple magnetic moment are obtained through calculation, an interference magnetic field generated by the magnetic interference target at the installation position of the nine-axis magnetic compass can be obtained through calculation according to the formula (6), and the compensation of the interference of the external ferromagnetic substances is realized.
There is a singularity problem due to the use of the euler angle method to calculate the attitude angle. From the practical engineering angle, the invention adopts a quaternion method to solve the attitude angle of the carrier, the quaternion method is a method for equivalently describing the rotation of a coordinate system by utilizing the multiplication property of a hypercomplex number, and the definition of a rotation quaternion Q is as follows:
Q＝q0+q1i+q2j+q3k (11)
the attitude rotation rate equation based on the attitude quaternion can be derived from the relationship between the attitude quaternion and the rotation vector, and the instantaneous rotation of the coordinate system can be represented by the differential equation of the rotation quaternion as in equation (12):
Figure RE-GDA0003547717660000071
in the formula
Figure RE-GDA0003547717660000072
Figure RE-GDA0003547717660000073
The three-axis angular rate output value under the carrier coordinate system is the theoretical output value of the three-axis gyroscope. Writing equation (12) in a matrix form according to quaternion multiplication expansion can obtain:
Figure RE-GDA0003547717660000074
as can be seen from the differential equation in the formula (13), when the attitude quaternion at the initial moment of the system is known to be output, the attitude quaternion of the system can be solved in real time according to the output of the gyroscope.
(3) Construction of error vectors using magnetic fields and acceleration
The dynamic response of the gyroscope for solving the attitude angle is fast, but accumulated errors, zero drift and the like can be generated in the process of solving the attitude. The magnetic sensor and the accelerometer have no accumulated error, but have poor instantaneous characteristics, and cannot meet the requirement of real-time attitude calculation when the attitude of the carrier is changed violently. Therefore, the advantages of the two can be combined, the high-pass filtering is carried out on the attitude angle measured by the gyroscope by utilizing the complementary filtering principle, the low-pass filtering is carried out on the attitude angle measured by the magnetic sensor and the accelerometer, and finally, a signal which is good in both high-frequency band and low-frequency band is obtained. The principle of the complementary filter is shown in fig. 2 and 3, and the flow of the complementary filter solution data is shown in fig. 6.
Wherein, theta represents the output of the accelerometer and the magnetic sensor, omega represents the output of the gyroscope, and an integral link K is introduced for compensating the drift error of the gyroscopeiAnd s. And KpFor the switching frequencies of the low-pass filter and the high-pass filter,
Figure RE-GDA0003547717660000081
representing the system complementary filter output.
According to the definition of the coordinate transformation matrix, the gravity acceleration of the gravity vector of the geographic coordinate system rotating to the carrier coordinate system is as follows:
Figure RE-GDA0003547717660000082
the acceleration measured by the triaxial accelerometer under the carrier coordinate system is normalized and then recorded as abAcceleration g in a carrier coordinate system obtained by coordinate transformation of the acceleration and the gravitational accelerationbThe error vector in between can be represented by the cross product of these two vectors as:
eacc(k)＝gb×ab (15)
this cross product vector is still located in the carrier coordinate system. The gyro integral error is also on the body coordinate system, and the cross product is in direct proportion to the gyro integral error, which is just used for correcting the gyro error.
The error vector can likewise be calculated from the magnetic field measurements in the magnetic field carrier coordinate system:
emag(k)＝mb×Hb (16)
wherein m isbRepresenting geomagnetismMagnetic field value H under carrier coordinate system obtained by coordinate change after field normalizationbAnd the value of the normalized triaxial magnetic field measured by the triaxial magnetic sensor in the carrier coordinate system is represented.
Solving the differential equation in equation (13) using a first order Runge-Kutta method:
Figure RE-GDA0003547717660000083
wherein
Figure RE-GDA0003547717660000084
Figure RE-GDA0003547717660000085
It can be seen that the error vector calculated by the accelerometer is subjected to PI operation, and then is subjected to complementary filtering fusion with the output of the gyroscope to obtain the final attitude angle output.
Effect of complementary filter with Kp、KiThe selection of the accelerometer and the gyroscope has a great relationship, if the selection is not proper, the error of attitude estimation is large, and even the problem of divergence of a calculation result occurs, the selection needs to be carried out according to the output frequency characteristics of the accelerometer and the gyroscope. Based on the frequency characteristics of the accelerometer and gyroscopepAnd KiCan be designed according to classical control theory.
The input and output relations of the system in the s domain are as follows:
Figure RE-GDA0003547717660000091
wherein the content of the first and second substances,
Figure RE-GDA0003547717660000092
respectively representing low-pass filter and high-pass filter coefficients, and F1(s)+F2(s) ═ 1 satisfies the complementary filter equation, R1(s) represents the output of the accelerometer and magnetic sensor in the s-domain, R2(s) representing gyroscopes in the s domainAnd outputting the signals to the computer for output,
Figure RE-GDA0003547717660000093
representing the output of the complementary filtering system in the s-domain. It can be seen that the system is a second order system, where Kp＝2ξωn，
Figure RE-GDA0003547717660000094
Xi is the system damping coefficient, omeganIs the natural vibration angular frequency.
When the system is in a static or low-acceleration motion state, the attitude angle measured according to the acceleration has higher precision, KpaccCan be taken to be larger, whereas K is the case when the system is in a state of high acceleration motionpaccA smaller value is required.
Defining:
Figure RE-GDA0003547717660000095
the difference between the module value representing the acceleration vector and the module value of the gravity acceleration is that err is close to zero when the carrier is static, and the err is larger when the carrier is in a maneuvering state such as acceleration, so that the dynamic condition of the carrier can be reflected.
(4) Solving PI parameters of complementary filtering by fuzzy control:
using err as input of fuzzy control to detect the maneuvering state of carrier, and output K of self-adaptive modulepaccThe switching frequency of the low-pass filter and the high-pass filter. It can be seen that err is close to zero when the carrier is at rest, when K is presentpaccTaking the larger value, when the carrier is in low dynamic state, the err value is smaller, and K is in this casepaccTaking a moderate value, when the carrier is in a high dynamic state, the value of err is larger, and K is in the momentpaccTaking the smaller value. Let the fuzzy set of the fuzzy variable err be { zero, small, large }, the domain of discourse after quantization be { zero, small, big }, the fuzzy set of err be { large, medium, small }, and the domain of discourse after quantization be { big, medium, small }. Adjustment coefficient KpaccThe fuzzy control rule of (1) is as follows:
Figure RE-GDA0003547717660000101
err and KpThe membership function is shown in fig. 4 and fig. 5, respectively, and the center method is selected as the defuzzification method.
Output coefficient K of fuzzy control modulepaccK can be calculated by combining the damping coefficient xi of the systemiaccThe three-axis magnetic field after magnetic interference compensation is not influenced by external magnetic interference, KpmagAnd KimagThe parameter K can be kept constantpAnd KiThe posture quaternion Q of the system in real time can be obtained by calculation in the formula (17)kAnd a system data resolving flow chart is shown in fig. 5, and three-axis magnetic field data, three-axis acceleration data and three-axis angular velocity data are subjected to adaptive complementary filtering fusion to finally obtain a system attitude quaternion output Q.
(5) Solving carrier attitude angle
Real-time carrier attitude angle data can be obtained through a conversion formula from quaternion to attitude angle, and the conversion formula from quaternion to attitude angle is shown as a formula (20).
Figure RE-GDA0003547717660000102
Wherein [ q ]0 q1 q2 q3]k＝Qk。
Therefore, the attitude angle data of the installed carrier can be obtained by utilizing the three-axis acceleration data, the three-axis angular velocity data and the three-axis magnetic field data of the nine-axis magnetic compass through self-adaptive complementary filtering data fusion. According to the invention, high-precision attitude angle output can be realized through self-adaptive complementary filtering data fusion, and when magnetic interference exists around the carrier, the real-time position of a magnetic interference target source can be solved in real time to perform magnetic interference compensation, the finally output attitude angle is not influenced by motion acceleration and external magnetic interference, and errors can not be accumulated along with the increase of time.

Claims (7)

1. A nine-axis magnetic compass data fusion processing method based on self-adaptive complementary filtering is characterized in that the nine-axis magnetic compass data comprises three-axis magnetic sensor data, three-axis accelerometer data and three-axis magnetic field data; the data fusion processing method comprises the following steps:
the method comprises the following steps: calculating a system state initial value by using the three-axis magnetic sensor data and the three-axis accelerometer data;
step two: judging whether magnetic interference exists around the carrier or not through the triaxial magnetic field data, and if the magnetic interference exists, resolving the real-time position of a magnetic interference target source in real time according to the measured triaxial magnetic anomaly data to realize magnetic interference compensation;
step three: constructing an error vector by using the triaxial magnetic field data and the triaxial accelerometer data;
step four: solving PI parameters of complementary filtering by fuzzy control;
step five: and solving the carrier attitude angle by utilizing the attitude quaternion obtained by the real-time solution of the complementary filtering.
2. The nine-axis magnetic compass data fusion processing method according to claim 1, wherein in the first step, the nine-axis magnetic compass is mainly composed of a three-axis MEMS gyroscope, a three-axis MEMS accelerometer, and a three-axis magnetic sensor.
3. The nine-axis magnetic compass data fusion processing method according to claim 1, wherein the solving method of the system state initial value in the first step is specifically:
s1.1, calculating a pitch angle and a roll angle in an attitude angle according to a coordinate transformation relation of the gravity acceleration at the initial moment under a carrier coordinate system and a geographic coordinate system;
s1.2, calculating to obtain the horizontal component of the geomagnetic field by using the pitch angle, the roll angle and the measured triaxial magnetic field data, and calculating to obtain an azimuth angle in an attitude angle by using an inverse trigonometric function;
and S1.3, calculating to obtain the attitude angle quaternion in the initial state through the conversion relation between the attitude angle and the quaternion.
4. The nine-axis magnetic compass data fusion processing method according to claim 1, wherein the magnetic interference compensation method in the second step is specifically realized by the steps of:
s2.1: deducing a calculation method of the target position when the target magnetic moment and the magnetic field are known according to the target magnetic dipole model;
s2.2: assuming that the magnetic target and the carrier do relative linear motion in a short time, the variation of the relative distance is equal in the same time, converting the target inversion problem into a nonlinear optimization problem, solving the nonlinear optimization problem through a particle swarm optimization algorithm to obtain a target magnetic moment, and calculating the relative position of the target;
s3.3: and calculating an interference magnetic field generated by the target at the position of the carrier according to the calculated target parameters to realize magnetic interference compensation.
5. The nine-axis magnetic compass data fusion processing method according to claim 1, wherein the error vector constructed in the third step is specifically:
s3.1: constructing an error vector according to the three-axis magnetic field data: obtaining a cross product of a magnetic field value under a carrier coordinate system and a value measured by a triaxial magnetic sensor after the triaxial magnetic field is normalized by the geomagnetic field and changing coordinates;
s3.2: and (3) constructing an error vector according to the acceleration: and rotating the gravity vector of the geographic coordinate system to the cross product of the gravity acceleration on the carrier coordinate system and the normalized acceleration value measured by the three-axis accelerometer.
6. The nine-axis magnetic compass data fusion processing method according to claim 1, wherein the implementation steps of the adaptive adjustment of the complementary filtering PI parameter in the fourth step are as follows:
s4.1: defining a parameter capable of reflecting the motion state of the system;
s4.2: taking the parameters as the input of fuzzy control, and constructing a fuzzy control rule and an input-output membership function;
s4.3: and adjusting the PI parameter of the system complementary filtering in real time according to the output of the fuzzy control.
7. The nine-axis magnetic compass data fusion processing method according to claim 1, wherein the solving of the attitude angle of the carrier in the fifth step is obtained by calculation according to a conversion relation between quaternions and attitude angles after the attitude quaternion obtained by the system complementary filtering calculation is normalized.
CN202111644288.2A 2021-12-29 2021-12-29 Nine-axis magnetic compass data fusion method based on adaptive complementary filtering Pending CN114440871A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111644288.2A CN114440871A (en) 2021-12-29 2021-12-29 Nine-axis magnetic compass data fusion method based on adaptive complementary filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111644288.2A CN114440871A (en) 2021-12-29 2021-12-29 Nine-axis magnetic compass data fusion method based on adaptive complementary filtering

Publications (1)

Publication Number Publication Date
CN114440871A true CN114440871A (en) 2022-05-06

Family

ID=81364859

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111644288.2A Pending CN114440871A (en) 2021-12-29 2021-12-29 Nine-axis magnetic compass data fusion method based on adaptive complementary filtering

Country Status (1)

Country Link
CN (1) CN114440871A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116449204A (en) * 2023-06-16 2023-07-18 德电北斗电动汽车有限公司 Fault detection method for opposed-piston magnetic force linear generator and related device

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104197927A (en) * 2014-08-20 2014-12-10 江苏科技大学 Real-time navigation system and real-time navigation method for underwater structure detection robot
CN106546235A (en) * 2016-11-02 2017-03-29 哈尔滨工程大学 A kind of locating magnetic objects method compensated based on carrier
CN108731676A (en) * 2018-05-04 2018-11-02 北京摩高科技有限公司 A kind of posture fusion enhancing measurement method and system based on inertial navigation technology
CN109108936A (en) * 2018-10-24 2019-01-01 电子科技大学 A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion
CN109459020A (en) * 2018-12-24 2019-03-12 哈尔滨工程大学 A kind of inertia and magnetometer combine self-adapting anti-jamming method
CN110361683A (en) * 2019-07-17 2019-10-22 哈尔滨工程大学 Magnetometer bearing calibration based on two-objective programming particle group optimizing
CN111551168A (en) * 2020-04-30 2020-08-18 江苏帝一集团有限公司 Underwater robot position and attitude data acquisition system and data fusion method thereof
CN113063416A (en) * 2021-02-05 2021-07-02 重庆大学 Robot attitude fusion method based on adaptive parameter complementary filtering
CN113310470A (en) * 2021-05-27 2021-08-27 湖南国天电子科技有限公司 Wave measuring method based on inertia measuring system

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104197927A (en) * 2014-08-20 2014-12-10 江苏科技大学 Real-time navigation system and real-time navigation method for underwater structure detection robot
CN106546235A (en) * 2016-11-02 2017-03-29 哈尔滨工程大学 A kind of locating magnetic objects method compensated based on carrier
CN108731676A (en) * 2018-05-04 2018-11-02 北京摩高科技有限公司 A kind of posture fusion enhancing measurement method and system based on inertial navigation technology
CN109108936A (en) * 2018-10-24 2019-01-01 电子科技大学 A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion
CN109459020A (en) * 2018-12-24 2019-03-12 哈尔滨工程大学 A kind of inertia and magnetometer combine self-adapting anti-jamming method
CN110361683A (en) * 2019-07-17 2019-10-22 哈尔滨工程大学 Magnetometer bearing calibration based on two-objective programming particle group optimizing
CN111551168A (en) * 2020-04-30 2020-08-18 江苏帝一集团有限公司 Underwater robot position and attitude data acquisition system and data fusion method thereof
CN113063416A (en) * 2021-02-05 2021-07-02 重庆大学 Robot attitude fusion method based on adaptive parameter complementary filtering
CN113310470A (en) * 2021-05-27 2021-08-27 湖南国天电子科技有限公司 Wave measuring method based on inertia measuring system

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
陈雷 等: "基于模糊自适应互补滤波的姿态解算算法" *
马剑飞 等: "基于正交基函数的矢量磁梯度异常探测" *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116449204A (en) * 2023-06-16 2023-07-18 德电北斗电动汽车有限公司 Fault detection method for opposed-piston magnetic force linear generator and related device
CN116449204B (en) * 2023-06-16 2023-08-11 德电北斗电动汽车有限公司 Fault detection method for opposed-piston magnetic force linear generator and related device

Similar Documents

Publication Publication Date Title
Poddar et al. A comprehensive overview of inertial sensor calibration techniques
Gebre-Egziabher et al. A gyro-free quaternion-based attitude determination system suitable for implementation using low cost sensors
CN104931028B (en) A kind of three axle magneto-electronic compass error compensation methods based on deep learning
CN104698485B (en) Integrated navigation system and air navigation aid based on BD, GPS and MEMS
CN111678538A (en) Dynamic level meter error compensation method based on speed matching
CN109443349A (en) A kind of posture Course Measure System and its fusion method, storage medium
CN108731676B (en) Attitude fusion enhanced measurement method and system based on inertial navigation technology
Sabet et al. Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS
CN105910606A (en) Direction adjustment method based on angular velocity difference
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN116817896B (en) Gesture resolving method based on extended Kalman filtering
CN110567492A (en) Low-cost MEMS inertial sensor system-level calibration method
CN107860382B (en) Method for measuring attitude by applying AHRS under geomagnetic anomaly condition
CN108592943A (en) A kind of inertial system coarse alignment computational methods based on OPREQ methods
CN114440871A (en) Nine-axis magnetic compass data fusion method based on adaptive complementary filtering
CN114459466A (en) MEMS multi-sensor data fusion processing method based on fuzzy control
Wei et al. Novel rotation scheme for dual-axis rotational inertial navigation system based on body diagonal rotation of inertial measurement unit
CN110737194B (en) Multi-MEMS sensor combination attitude measurement method
Chen et al. A novel calibration method for tri-axial magnetometers based on an expanded error model and a two-step total least square algorithm
CN108592918A (en) The full attitude algorithm method of MEMS IMU under swaying base
Chen et al. High-precision geomagnetic directional technology based on sensor error correction and adaptive hybrid filter
CN114440870B (en) Nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering
CN112882118B (en) Method and system for estimating gravity vector of movable base under earth-fixed coordinate system and storage medium
CN112683265B (en) MIMU/GPS integrated navigation method based on rapid ISS collective filtering
Ye et al. Heading angle estimation using rotating magnetometer for mobile robots under environmental magnetic disturbances

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
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20220506