CN114440870A - Nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering - Google Patents
Nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering Download PDFInfo
- Publication number
- CN114440870A CN114440870A CN202111637776.0A CN202111637776A CN114440870A CN 114440870 A CN114440870 A CN 114440870A CN 202111637776 A CN202111637776 A CN 202111637776A CN 114440870 A CN114440870 A CN 114440870A
- Authority
- CN
- China
- Prior art keywords
- magnetic
- axis
- axis magnetic
- magnetic compass
- compass
- 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.)
- Granted
Links
- 230000005291 magnetic effect Effects 0.000 title claims abstract description 317
- 238000000034 method Methods 0.000 title claims abstract description 35
- 230000000295 complement effect Effects 0.000 title claims abstract description 28
- 238000001914 filtration Methods 0.000 title claims abstract description 17
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 8
- 239000000126 substance Substances 0.000 claims abstract description 55
- 238000012886 linear function Methods 0.000 claims abstract description 16
- 238000004364 calculation method Methods 0.000 claims abstract description 15
- 239000002245 particle Substances 0.000 claims abstract description 11
- 238000005457 optimization Methods 0.000 claims abstract description 10
- 230000008859 change Effects 0.000 claims abstract description 6
- 230000001133 acceleration Effects 0.000 claims description 35
- 239000013598 vector Substances 0.000 claims description 17
- 230000002452 interceptive effect Effects 0.000 claims description 16
- 230000005484 gravity Effects 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 10
- 238000009434 installation Methods 0.000 claims description 7
- 230000005358 geomagnetic field Effects 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 description 4
- 230000004927 fusion Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000013016 damping Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000013139 quantization Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005294 ferromagnetic effect Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/20—Instruments for performing navigational calculations
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
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)
- Measuring Magnetic Variables (AREA)
- Gyroscopes (AREA)
Abstract
The invention discloses a nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering, which is used for solving the real-time position of a magnetic interference target by establishing the position of a magnetic interference substance, the magnetic dipole moment of the magnetic interference substance and the linear function of the magnetic field of the magnetic interference substance and compensating the magnetic interference. And then, a self-adaptive particle swarm optimization algorithm is adopted to find out the magnetic dipole moment which enables the relative distance change quantity of the magnetic interference substance and the nine-axis magnetic compass carrier to measure the minimum value, the parameters of the complementary filter are solved through fuzzy control, the frequency characteristics of the triaxial accelerometer and the triaxial gyroscope are fully considered, and the problem of divergence of the calculation result is avoided. And then, solving an attitude angle by adopting a quaternion method to obtain the real-time attitude of the nine-axis magnetic compass.
Description
Technical Field
The invention relates to the technical field of navigation, in particular to a method for acquiring the posture of a nine-axis magnetic compass 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.
Disclosure of Invention
In view of the above, the invention provides a nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering, which can realize data fusion of a three-axis accelerometer, a three-axis magnetic sensor and a three-axis gyroscope and obtain a high-precision attitude angle through calculation.
The invention adopts the following specific technical scheme:
a nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering comprises the following steps:
establishing a position of a magnetic interference substance, a magnetic dipole moment of the magnetic interference substance and a linear function of an interference magnetic field of the magnetic interference substance;
step two, adopting a self-adaptive particle swarm optimization algorithm, representing the position parameters of the particles by using the positions of the magnetic interference substances, and calculating according to the linear function in the step one to obtain the position parameters of each particle; finding out magnetic dipole moment which enables the relative distance change quantity of the magnetic interference substance and the nine-axis magnetic compass carrier to be the minimum value, and calculating and obtaining an interference magnetic field at the installation position of the nine-axis magnetic compass according to the magnetic dipole moment;
thirdly, compensating the magnetic field measured by the nine-axis magnetic compass by using the interference magnetic field in the second step;
after compensating the magnetic field measured by the nine-axis magnetic compass, acquiring the difference between the modulus of the acceleration vector measured by the three-axis accelerometer of the nine-axis magnetic compass and the modulus of the gravity acceleration as the input of fuzzy control to obtain the proportional parameter of the complementary filter and the integral parameter of the complementary filter;
and fifthly, calculating the real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the step four, and calculating the carrier attitude angle of the nine-axis magnetic compass according to the real-time attitude quaternion.
Further, in step one, the linear function is:
wherein,
indicating the location of the magnetically interfering substance,
represents the magnetic dipole moment of a magnetically interfering substance,
magnetic field, k, representing a magnetically interfering substance1、k2Is a linear parameter of a linear function.
Further, in the second step, the minimum value of the relative distance variation between the magnetic interfering substance and the nine-axis magnetic compass carrier is determined by:
wherein,
k denotes the time sequence of a nine-axis magnetic compass,
indicating the position of the magnetic interference target relative to the nine-axis magnetic compass vehicle at different times,
represents the three-axis magnetic field measured by the magnetic sensor of the nine-axis magnetic compass at different moments,
denotes the magnetic dipole moment, min [ of a magnetic interfering substance]Representing a minimum function.
Further, in step two, the interference magnetic field at the installation position of the nine-axis magnetic compass obtained by calculation according to the magnetic dipole moment is:
wherein,
which is indicative of a disturbing magnetic field,
indicating the position of the magnetic interference target relative to a nine-axis magnetic compass vehicle,
represents the magnetic dipole moment of a magnetically interfering substance,
to represent
The unit vector of (2).
Further, in the third step, the magnetic field measured by the nine-axis magnetic compass is compensated as follows: and the magnetic field measured by the nine-axis magnetic compass is used for subtracting the interference magnetic field, so that the compensation of the magnetic field measured by the nine-axis magnetic compass is realized.
Further, in the fifth step, the real-time attitude quaternion of the nine-axis magnetic compass calculated according to the proportional parameter and the integral parameter in the fourth step is as follows: firstly, calculating system compensation quantity of the nine-axis magnetic compass according to the proportional parameter and the integral parameter, then calculating to obtain a differential representation form of an attitude quaternion according to the system compensation quantity, and then calculating to obtain the real-time attitude quaternion according to the differential representation form of the attitude quaternion;
the real-time attitude quaternion determination method comprises the following steps:
wherein Q iskRepresenting real-time attitude quaternion, Qk-1Representing an attitude quaternion at the k-1 th moment, t representing a time step, and k representing a time sequence of the nine-axis magnetic compass;
Further, in the fifth step, the carrier attitude angle of the nine-axis magnetic compass calculated according to the real-time attitude quaternion is:
wherein [ q ]0 q1 q2 q3]k＝QkRepresenting real-time attitude quaternion, q0,q1,q2,q3And alpha is a quaternion parameter, alpha represents the heading angle of the nine-axis magnetic compass, beta represents the pitch angle of the nine-axis magnetic compass, and gamma represents the roll angle of the nine-axis magnetic compass.
Further, before the interference magnetic field is compensated, calculating an initial pitch angle value and an initial roll angle value of the nine-axis magnetic compass through triaxial accelerometer data, and calculating an initial heading angle value of the nine-axis magnetic compass through triaxial magnetic sensor data;
and calculating to obtain an attitude angle quaternion under the initial state of the nine-axis magnetic compass according to the initial pitch angle value, the initial roll angle value and the initial course angle value.
Has the advantages that:
(1) a method for acquiring the attitude of a nine-axis magnetic compass based on self-adaptive complementary filtering is characterized in that the real-time position of a magnetic interference target is actually solved by establishing the position of a magnetic interference substance, the magnetic dipole moment of the magnetic interference substance and the linear function of the magnetic field of the magnetic interference substance, and the magnetic interference is compensated, so that the accuracy of the geomagnetic field measured by a three-axis magnetic sensor of the nine-axis magnetic compass is ensured. And a self-adaptive particle swarm optimization algorithm is adopted to find the magnetic dipole moment which enables the relative distance variation of the magnetic interference substance and the nine-axis magnetic compass carrier to measure the minimum value, so that the relative distance variation of the magnetic interference substance and the nine-axis magnetic compass carrier can be ensured to be minimum, and the stability of the system is ensured. The parameters of the complementary filter are solved through fuzzy control, the frequency characteristics of the three-axis accelerometer and the three-axis gyroscope are fully considered, the problem of divergence of a calculation result is avoided, and meanwhile the problems of errors and null shift in the attitude settlement process are avoided. The problem that singularities exist in the traditional method for calculating the attitude angle can be solved by adopting a quaternion method to solve the attitude angle.
(2) The position of the magnetic interference substance, the magnetic dipole moment of the magnetic interference substance and the linear function of the magnetic field of the magnetic interference substance are established, the coplanar and non-collinear property of the magnetic field and the magnetic dipole moment is fully considered, and the real-time position of the magnetic interference substance can be rapidly and efficiently solved.
(3) When the interference magnetic field at the installation position of the nine-axis magnetic compass is calculated according to the position of the magnetic interference substance, the magnetic interference substance is equivalent to a magnetic dipole model, so that the calculation speed of the interference magnetic field is improved, and the calculation amount of the attitude acquisition method of the nine-axis magnetic compass is further reduced.
Drawings
FIG. 1 is a flow chart of the attitude angle calculation of the attitude acquisition method of the nine-axis magnetic compass of the present invention.
FIG. 2 is a flow chart of the complementary filter parameter calculation in the attitude acquisition method of the nine-axis magnetic compass of the present invention.
FIG. 3 is a diagram illustrating the membership function of the difference between the modulus of the acceleration vector measured by the triaxial accelerometer of the nine-axis magnetic compass and the modulus of the gravity acceleration.
Detailed Description
The invention provides a nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering, which is used for solving the real-time position of a magnetic interference target by establishing the position of a magnetic interference substance, the magnetic dipole moment of the magnetic interference substance and the linear function of the magnetic field of the magnetic interference substance and compensating the magnetic interference. And then, a self-adaptive particle swarm optimization algorithm is adopted to find out the magnetic dipole moment which enables the relative distance change quantity of the magnetic interference substance and the nine-axis magnetic compass carrier to measure the minimum value, the parameters of the complementary filter are solved through fuzzy control, the frequency characteristics of the triaxial accelerometer and the triaxial gyroscope are fully considered, and the problem of divergence of the calculation result is avoided. And then, solving the attitude angle by adopting a quaternion method to obtain the real-time attitude of the nine-axis magnetic compass.
The invention is described in detail below by way of example with reference to the accompanying drawings.
Fig. 1 shows a flow chart of the attitude angle calculation of the attitude acquisition method of the nine-axis magnetic compass according to the present invention. A nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering comprises the following steps:
step one, establishing a position of a magnetic interference substance, a magnetic dipole moment of the magnetic interference substance and a linear function of an interference magnetic field of the magnetic interference substance.
The linear function is:
wherein,
indicating the location of the magnetically interfering substance,
represents the magnetic dipole moment of a magnetically interfering substance,
magnetic field, k, representing a magnetically interfering substance1、k2Is a linear parameter of a linear function.
Before the interference magnetic field is compensated, the initial pitch angle value and the initial roll angle value of the nine-axis magnetic compass are calculated through the data of the triaxial accelerometer, and the initial heading angle value of the nine-axis magnetic compass is calculated through the data of the triaxial magnetic sensor.
And calculating according to the coordinate transformation relation of the gravity acceleration at the initial moment under the carrier coordinate system and the geographic coordinate system to obtain a pitch angle and a roll angle, calculating by using the pitch angle, the roll angle and the measured triaxial magnetic field data to obtain the horizontal component of the geomagnetic field, and calculating by using an inverse trigonometric function to obtain an azimuth angle. And finally, calculating to obtain the attitude angle quaternion in the initial state through the conversion relation between the attitude angle and the quaternion.
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, and the three-axis accelerometer is used for measuring three-axis direction acceleration data.
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:
in the formula (1), the first and second groups,
for three components of three axes of the gravity acceleration measured by the triaxial accelerometer under a carrier coordinate system, the pitch angle beta and the roll angle gamma can be calculated by unfolding the formula (1):
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.
[mx my mz]For three-axis magnetic sensor measurementsThe three-component of the three axes, m, of the geomagnetic field in the carrier coordinate systemx' and myIn the practical application process, the nine-axis magnetic compass is inevitably interfered by various external ferromagnetic substances, and the long-term accuracy of the azimuth angle of the nine-axis magnetic compass is guaranteed by depending on the accurate measurement of the geomagnetic field by the magnetic sensor.
And step two, adopting a self-adaptive particle swarm optimization algorithm to find out the magnetic dipole moment which enables the relative distance change quantity of the magnetic interference substance and the nine-axis magnetic compass carrier to measure the minimum value, and calculating the position of the magnetic interference substance according to the linear function in the step one.
When the nine-axis magnetic compass is arranged on the carrier, the carrier and the magnetic target can be regarded as making relative uniform linear motion in a short time, namely the variation of the relative distance in the same time
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
The positions of the interference targets relative to the carrier are respectively obtained according to the measured three-axis magnetic field inversion
Solving the target position and magnetic moment can ultimately be translated into a nonlinear optimization problem.
The minimum value of the relative distance variation of the magnetic interference substance and the nine-axis magnetic compass carrier is expressed by the following formula:
wherein,
k denotes the time sequence of a nine-axis magnetic compass,
indicating the position of the magnetic interference target relative to the nine-axis magnetic compass vehicle at different times,
represents the three-axis magnetic field measured by the magnetic sensor of the nine-axis magnetic compass at different moments,
denotes the magnetic dipole moment, min [ of a magnetic interference substance]Representing a minimum function.
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
Then at a distance from the magnetically interfering substance
The magnetic potential and the magnetic field are respectively as follows:
as can be seen from the formula, the three vectors are located in the same plane when viewed as three vectors, that is:
wherein k is1And k is2The solution method of (2) is as follows:
putting equation (8) into equation (7):
due to the fact that
And
non-collinear, geometric principles can be derived:
solving the system of linear equations in equation (10) yields k1And k is2。
The method comprises the steps of firstly deducing a calculation method of a target magnetic moment and a target position when the magnetic moment and the magnetic field of a known target are known according to a target magnetic dipole model, assuming that a magnetic target and a carrier do relative linear motion in a short time, so that the variation of relative distance in the same time is equal, converting a target inversion problem into a nonlinear optimization problem, solving the nonlinear optimization problem through a particle swarm optimization algorithm to obtain the target magnetic moment, calculating the relative position of the target, and finally calculating an interference magnetic field generated by the target at the position of the carrier according to the target parameters obtained through calculation to realize magnetic interference compensation.
And step three, calculating an interference magnetic field at the installation position of the nine-axis magnetic compass according to the position of the magnetic interference substance in the step two, and compensating the interference magnetic field.
Calculating the interference magnetic field at the installation position of the nine-axis magnetic compass according to the position of the magnetic interference substance in the step two as follows:
wherein,
which is indicative of a disturbing magnetic field,
indicating the position of the magnetic interference target relative to a nine-axis magnetic compass vehicle,
representing the magnetic dipole moment of the magnetically interfering species,
to represent
The unit vector of (2).
The disturbing magnetic field is compensated as follows: the magnetic field measured by the nine-axis magnetic compass is used for subtracting the interference magnetic field, so that the compensation of the interference magnetic field is realized.
And step four, taking the difference between the modulus of the acceleration vector measured by the triaxial accelerometer of the nine-axis magnetic compass and the modulus of the gravity acceleration as the input of fuzzy control to obtain the proportional parameter of the complementary filter and the integral parameter of the complementary filter.
Firstly, defining a parameter capable of reflecting the motion state of the system, taking the parameter as the input of fuzzy control, then constructing a fuzzy control rule and an input-output membership function, and adjusting the PI parameter of system complementary filtering in real time according to the output of the fuzzy control.
When the attitude quaternion at the initial moment of the known system is output, the attitude quaternion of the system can be solved in real time according to the output of the gyroscope.
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.
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:
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 (12)
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 and 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 (13)
wherein m isbRepresenting the magnetic field value H under the carrier coordinate system obtained by the coordinate change of the normalized geomagnetic fieldbAnd the value of the normalized triaxial magnetic field measured by the triaxial magnetic sensor in the carrier coordinate system is represented.
The differential equation in equation (13) is solved using the first order Runge-Kutta method:
wherein
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:
wherein,
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) represents the output of the gyroscope in the s domain,
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，
Xi is the system damping coefficient, omeganIs the natural vibration angular frequency.
Measured from acceleration when the system is in a state of rest or low-acceleration motionHigh precision of attitude angle, 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:
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.
Using err as input of fuzzy control to detect the maneuvering state of carrier, and output K of self-adaptive modulepaccThe switching frequencies 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 value of err 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:
membership functions of err are respectively shown in fig. 3, and a central 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 fixed value can be kept constant.
As can be seen from the above, the parameters of the complementary filter include the scaling parameter KpAnd integral parameter KiIn the present invention, the ratio parameter KpProportional parameter K including a three-axis accelerometerpaccAnd the proportional parameter K of the three-axis magnetic sensorpmag(ii) a Integral parameter KiIntegral parameter K comprising a three-axis accelerometeriaccAnd integral parameter K of three-axis magnetic sensorimag。
And step five, calculating the real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the step four, and calculating the carrier attitude angle of the nine-axis magnetic compass according to the real-time attitude quaternion.
And calculating the real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the step four as follows: calculating system compensation quantity of the nine-axis magnetic compass according to the proportional parameter and the integral parameter, then calculating to obtain a differential representation form of the attitude quaternion according to the system compensation quantity, and then calculating to obtain a real-time attitude quaternion according to the differential representation form of the attitude quaternion;
the real-time attitude quaternion is formulated as:
wherein Q iskRepresenting real-time attitude quaternion, Qk-1Representing an attitude quaternion at the k-1 th moment, t representing a time step, and k representing a time sequence of the nine-axis magnetic compass;
And calculating the carrier attitude angle of the nine-axis magnetic compass according to the real-time attitude quaternion as follows:
wherein [ q ]0 q1 q2 q3]k＝QkRepresenting real-time attitude quaternion, q0,q1,q2,q3And alpha is a quaternion parameter, alpha represents the heading angle of the nine-axis magnetic compass, beta represents the pitch angle of the nine-axis magnetic compass, and gamma represents the roll angle of the nine-axis magnetic compass.
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.
The above embodiments are merely illustrative of the design principles of the present invention, and the shapes of the components in the description may be different and the names are not limited. Therefore, a person skilled in the art of the present invention can modify or substitute the technical solutions described in the foregoing embodiments; such modifications and substitutions do not depart from the spirit and scope of the present invention.
Claims (8)
1. A nine-axis magnetic compass attitude acquisition method based on adaptive complementary filtering is characterized by comprising the following steps:
establishing a position of a magnetic interference substance, a magnetic dipole moment of the magnetic interference substance and a linear function of an interference magnetic field of the magnetic interference substance;
step two, adopting a self-adaptive particle swarm optimization algorithm, representing the position parameters of the particles by using the positions of the magnetic interference substances, and calculating according to the linear function in the step one to obtain the position parameters of each particle; finding out magnetic dipole moment which enables the relative distance change quantity of the magnetic interference substance and the nine-axis magnetic compass carrier to be the minimum value, and calculating and obtaining an interference magnetic field at the installation position of the nine-axis magnetic compass according to the magnetic dipole moment;
thirdly, compensating the magnetic field measured by the nine-axis magnetic compass by using the interference magnetic field in the second step;
after compensating the magnetic field measured by the nine-axis magnetic compass, acquiring the difference between the modulus of the acceleration vector measured by the three-axis accelerometer of the nine-axis magnetic compass and the modulus of the gravity acceleration as the input of fuzzy control to obtain the proportional parameter of the complementary filter and the integral parameter of the complementary filter;
and fifthly, calculating the real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the step four, and calculating the carrier attitude angle of the nine-axis magnetic compass according to the real-time attitude quaternion.
2. The method for obtaining the attitude of a nine-axis magnetic compass according to claim 1, wherein in step one, the linear function is:
wherein,
indicating the location of the magnetically interfering substance,
representing the magnetic dipole moment of the magnetically interfering species,
magnetic field, k, representing a magnetically interfering substance1、k2Is a linear parameter of a linear function.
3. The method for obtaining the attitude of a nine-axis magnetic compass according to claim 1, wherein in the second step, the minimum value of the variation of the relative distance between the magnetic interference substance and the nine-axis magnetic compass carrier is determined by:
wherein,
k denotes the time sequence of a nine-axis magnetic compass,
indicating the position of the magnetic interference target relative to the nine-axis magnetic compass vehicle at different times,
represents the three-axis magnetic field measured by the magnetic sensor of the nine-axis magnetic compass at different moments,
denotes the magnetic dipole moment, min [ of a magnetic interfering substance]Representing a minimum function.
4. The attitude obtaining method of a nine-axis magnetic compass according to claim 1, wherein in step two, the magnetic interference field at the installation position of the nine-axis magnetic compass obtained by the calculation of the magnetic dipole moment is:
wherein,
which is indicative of a disturbing magnetic field,
indicating the position of the magnetic interference target relative to a nine-axis magnetic compass vehicle,
represents the magnetic dipole moment of a magnetically interfering substance,
to represent
The unit vector of (2).
5. The attitude obtaining method of the nine-axis magnetic compass according to claim 1, wherein in step three, the magnetic field measured by the nine-axis magnetic compass is compensated as follows: and the magnetic field measured by the nine-axis magnetic compass is used for subtracting the interference magnetic field, so that the compensation of the magnetic field measured by the nine-axis magnetic compass is realized.
6. The attitude obtaining method of a nine-axis magnetic compass according to claim 1, wherein in the fifth step, the real-time attitude quaternion of the nine-axis magnetic compass calculated according to the proportional parameter and the integral parameter in the fourth step is: firstly, calculating system compensation quantity of the nine-axis magnetic compass according to the proportional parameter and the integral parameter, then calculating to obtain a differential representation form of an attitude quaternion according to the system compensation quantity, and then calculating to obtain the real-time attitude quaternion according to the differential representation form of the attitude quaternion;
the real-time attitude quaternion determination method comprises the following steps:
wherein Q iskRepresenting real-time attitude quaternion, Qk-1Representing an attitude quaternion at the k-1 th moment, t representing a time step, and k representing a time sequence of the nine-axis magnetic compass;
wherein KpaccRepresenting the scale parameter, K, of a three-axis accelerometeriaccRepresenting integral parameters of a three-axis accelerometer, eacc(k) A cross product of the value of the acceleration measured by the triaxial accelerometer after the acceleration is normalized and the value of the acceleration obtained by coordinate transformation of the gravitational acceleration, emag(k) Representing the magnetic field value measured by the triaxial magnetic sensor after the triaxial magnetic field normalization and the magnetic field value of the geomagnetic field after coordinate transformation under the carrier coordinate systemCross product of (K)pmagThe proportional parameter of the triaxial magnetic sensor is represented, and the proportional parameter is a fixed value after magnetic interference compensation; kimagAnd the integral parameters of the three-axis magnetic sensor are represented, and the integral parameters are fixed values after magnetic interference compensation.
7. The attitude obtaining method of a nine-axis magnetic compass according to claim 1, wherein in step five, the carrier attitude angle of the nine-axis magnetic compass calculated according to the real-time attitude quaternion is:
wherein [ q ]0 q1 q2 q3]k＝QkRepresenting real-time attitude quaternion, q0,q1,q2,q3And alpha is a quaternion parameter, alpha represents the heading angle of the nine-axis magnetic compass, beta represents the pitch angle of the nine-axis magnetic compass, and gamma represents the roll angle of the nine-axis magnetic compass.
8. The attitude obtaining method of a nine-axis magnetic compass according to claim 1, wherein the initial pitch angle value and the initial roll angle value of the nine-axis magnetic compass are calculated by three-axis accelerometer data and the initial heading angle value of the nine-axis magnetic compass is calculated by three-axis magnetic sensor data before the compensation of the disturbing magnetic field;
and calculating to obtain an attitude angle quaternion under the initial state of the nine-axis magnetic compass according to the initial pitch angle value, the initial roll angle value and the initial course angle value.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111637776.0A CN114440870B (en) | 2021-12-29 | 2021-12-29 | Nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111637776.0A CN114440870B (en) | 2021-12-29 | 2021-12-29 | Nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114440870A true CN114440870A (en) | 2022-05-06 |
| CN114440870B CN114440870B (en) | 2023-06-27 |
Family
ID=81366142
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111637776.0A Active CN114440870B (en) | 2021-12-29 | 2021-12-29 | Nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114440870B (en) |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7932718B1 (en) * | 2009-03-12 | 2011-04-26 | The United States Of America As Represented By The Secretary Of The Navy | System and method using magnetic anomaly field magnitudes for detection, localization, classification and tracking of magnetic objects |
| US20110267046A1 (en) * | 2010-04-14 | 2011-11-03 | Commissariat A L'energie Atomique Et Aux Energies Alternatives | Method and device for compensation in a measurement of a magnetic field, object-localizing method and system, recording medium for these methods |
| CN103900564A (en) * | 2014-03-03 | 2014-07-02 | 哈尔滨工程大学 | Submergence assisted geomagnetic anomaly inversion velocity measurement/underwater continuous positioning method |
| CN106546235A (en) * | 2016-11-02 | 2017-03-29 | 哈尔滨工程大学 | A kind of locating magnetic objects method compensated based on carrier |
| CN107272069A (en) * | 2017-06-13 | 2017-10-20 | 哈尔滨工程大学 | Magnetic target method for tracing based on magnetic anomaly gradient |
| CN108318017A (en) * | 2017-12-27 | 2018-07-24 | 中国船舶重工集团公司第七0研究所 | A kind of differential digital magnetic compass eliminates the data processing method of random magnetic disturbances |
| CN108519082A (en) * | 2018-04-11 | 2018-09-11 | 上海理工大学 | Magnetic marker space gesture recognition method based on gimbal |
| CN108873086A (en) * | 2018-06-05 | 2018-11-23 | 哈尔滨工程大学 | A method of using geomagnetic total field gradient array to locating magnetic objects |
| CN109579828A (en) * | 2018-12-24 | 2019-04-05 | 中国船舶重工集团公司第七〇九研究所 | A kind of positioning system and its application method based on arc array of magnetic field |
| CN111190229A (en) * | 2020-01-16 | 2020-05-22 | 哈尔滨工业大学 | Magnetic target detection method |
| US20200326388A1 (en) * | 2019-04-15 | 2020-10-15 | Harbin Institute Of Technology | Method for Analyzing Magnetic Detection Blind Zone |
| CN111982089A (en) * | 2020-07-28 | 2020-11-24 | 陕西宝成航空仪表有限责任公司 | Real-time calibration and compensation method for magnetic compass total error |
| CN112344925A (en) * | 2020-10-11 | 2021-02-09 | 南京理工大学 | Underwater target correlation detection positioning method based on normalized magnetic moment |
-
2021
- 2021-12-29 CN CN202111637776.0A patent/CN114440870B/en active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7932718B1 (en) * | 2009-03-12 | 2011-04-26 | The United States Of America As Represented By The Secretary Of The Navy | System and method using magnetic anomaly field magnitudes for detection, localization, classification and tracking of magnetic objects |
| US20110267046A1 (en) * | 2010-04-14 | 2011-11-03 | Commissariat A L'energie Atomique Et Aux Energies Alternatives | Method and device for compensation in a measurement of a magnetic field, object-localizing method and system, recording medium for these methods |
| CN103900564A (en) * | 2014-03-03 | 2014-07-02 | 哈尔滨工程大学 | Submergence assisted geomagnetic anomaly inversion velocity measurement/underwater continuous positioning method |
| CN106546235A (en) * | 2016-11-02 | 2017-03-29 | 哈尔滨工程大学 | A kind of locating magnetic objects method compensated based on carrier |
| CN107272069A (en) * | 2017-06-13 | 2017-10-20 | 哈尔滨工程大学 | Magnetic target method for tracing based on magnetic anomaly gradient |
| CN108318017A (en) * | 2017-12-27 | 2018-07-24 | 中国船舶重工集团公司第七0研究所 | A kind of differential digital magnetic compass eliminates the data processing method of random magnetic disturbances |
| CN108519082A (en) * | 2018-04-11 | 2018-09-11 | 上海理工大学 | Magnetic marker space gesture recognition method based on gimbal |
| CN108873086A (en) * | 2018-06-05 | 2018-11-23 | 哈尔滨工程大学 | A method of using geomagnetic total field gradient array to locating magnetic objects |
| CN109579828A (en) * | 2018-12-24 | 2019-04-05 | 中国船舶重工集团公司第七〇九研究所 | A kind of positioning system and its application method based on arc array of magnetic field |
| US20200326388A1 (en) * | 2019-04-15 | 2020-10-15 | Harbin Institute Of Technology | Method for Analyzing Magnetic Detection Blind Zone |
| CN111190229A (en) * | 2020-01-16 | 2020-05-22 | 哈尔滨工业大学 | Magnetic target detection method |
| CN111982089A (en) * | 2020-07-28 | 2020-11-24 | 陕西宝成航空仪表有限责任公司 | Real-time calibration and compensation method for magnetic compass total error |
| CN112344925A (en) * | 2020-10-11 | 2021-02-09 | 南京理工大学 | Underwater target correlation detection positioning method based on normalized magnetic moment |
Non-Patent Citations (3)
| Title |
|---|
| 陈正想 等: "数字磁罗盘磁干扰补偿方法研究" * |
| 陈雷 等: "基于模糊自适应互补滤波的姿态解算算法" * |
| 马剑飞 等: "基于正交基函数的矢量磁梯度异常探测" * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114440870B (en) | 2023-06-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109001787B (en) | Attitude angle resolving and positioning method and fusion sensor thereof | |
| Poddar et al. | A comprehensive overview of inertial sensor calibration techniques | |
| CN106500693B (en) | A kind of AHRS algorithm based on adaptive extended kalman filtering | |
| CN104698485B (en) | Integrated navigation system and air navigation aid based on BD, GPS and MEMS | |
| CN106500721B (en) | A kind of underwater robot dual redundant attitude detection system | |
| CN110567492A (en) | Low-cost MEMS inertial sensor system-level calibration method | |
| CN109443342A (en) | NEW ADAPTIVE Kalman's UAV Attitude calculation method | |
| CN111272158B (en) | Dynamic azimuth angle resolving method of MEMS electronic compass in complex magnetic disturbance scene | |
| CN110737194B (en) | Multi-MEMS sensor combination attitude measurement method | |
| Sabet et al. | Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS | |
| CN107860382B (en) | Method for measuring attitude by applying AHRS under geomagnetic anomaly condition | |
| CN116817896A (en) | Gesture resolving method based on extended Kalman filtering | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| CN112577518A (en) | Inertial measurement unit calibration method and device | |
| CN114459466A (en) | MEMS multi-sensor data fusion processing method based on fuzzy control | |
| Wang et al. | Application of gravity passive aided strapdown inertial navigation in underwater vehicles | |
| CN114440871A (en) | Nine-axis magnetic compass data fusion method based on adaptive complementary filtering | |
| CN114440870B (en) | Nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering | |
| Hemanth et al. | Calibration of 3-axis magnetometers | |
| CN114264304B (en) | High-precision horizontal attitude measurement method and system for complex dynamic environment | |
| CN114674345B (en) | Inertial navigation/camera/laser velocimeter online joint calibration method | |
| 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 | |
| CN116299735A (en) | Interference magnetic field compensation method of geomagnetic vector measurement system based on BP neural network | |
| Narayanan | Performance analysis of attitude determination algorithms for low cost attitude heading reference systems |
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 |