CN114440870B - Nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering - Google Patents
Nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering Download PDFInfo
- Publication number
- CN114440870B CN114440870B CN202111637776.0A CN202111637776A CN114440870B CN 114440870 B CN114440870 B CN 114440870B CN 202111637776 A CN202111637776 A CN 202111637776A CN 114440870 B CN114440870 B CN 114440870B
- Authority
- CN
- China
- Prior art keywords
- magnetic
- axis
- axis magnetic
- compass
- magnetic 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.)
- Active
Links
- 230000005291 magnetic effect Effects 0.000 title claims abstract description 305
- 238000000034 method Methods 0.000 title claims abstract description 36
- 230000000295 complement effect Effects 0.000 title claims abstract description 28
- 238000001914 filtration Methods 0.000 title claims abstract description 18
- 239000000126 substance Substances 0.000 claims abstract description 48
- 238000004364 calculation method Methods 0.000 claims abstract description 13
- 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 9
- 230000001133 acceleration Effects 0.000 claims description 39
- 239000013598 vector Substances 0.000 claims description 16
- 238000012886 linear function Methods 0.000 claims description 14
- 230000002452 interceptive effect Effects 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 10
- 230000005358 geomagnetic field Effects 0.000 claims description 8
- 238000009434 installation Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 description 7
- 230000010354 integration Effects 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
- 238000010606 normalization Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 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
- 230000000694 effects Effects 0.000 description 1
- 230000005294 ferromagnetic effect Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000005259 measurement Methods 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
- 230000001052 transient effect Effects 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)
- Gyroscopes (AREA)
- Measuring Magnetic Variables (AREA)
Abstract
The invention discloses a nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering. And then, a self-adaptive particle swarm optimization algorithm is adopted to find out the magnetic dipole moment which enables the relative distance change between the magnetic interference substance and the nine-axis magnetic compass carrier to take the minimum value, and the parameters of the complementary filter are solved through fuzzy control, so that the frequency characteristics of the three-axis accelerometer and the three-axis 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.
Description
Technical Field
The invention relates to the technical field of navigation, in particular to a nine-axis magnetic compass gesture acquisition 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 singlechip and corresponding peripheral circuits and is mainly used for measuring the carrier posture.
Although the attitude angle obtained by integrating the triaxial angular velocity measured by the triaxial gyroscope has higher short-time precision, errors can accumulate along with the time due to the defects of low precision, large drift and the like of the MEMS gyroscope, and the attitude angle of the carrier measured by the triaxial accelerometer and the triaxial magnetic sensor can not accumulate along with the time, but when the carrier moves, the signals of the accelerometer cannot accurately reflect the attitude information of the carrier due to the influence of the movement acceleration.
Disclosure of Invention
In view of the above, the invention provides a nine-axis magnetic compass gesture acquisition method based on self-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 gesture angle through solution.
The invention adopts the following specific technical scheme:
a nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering comprises the following steps:
step one, establishing a linear function of the position of the magnetic interference substance, the magnetic dipole moment of the magnetic interference substance and the interference magnetic field of the magnetic interference substance;
secondly, adopting a self-adaptive particle swarm optimization algorithm, utilizing the position of the magnetic interference substance to represent the position parameters of particles, and calculating the position parameters of each particle according to the linear function in the first step; finding out the magnetic dipole moment which enables the change of the relative distance between the magnetic interference substance and the nine-axis magnetic compass carrier to be minimum, and calculating according to the magnetic dipole moment to obtain an interference magnetic field at the installation position of the nine-axis magnetic compass;
step three, compensating the magnetic field measured by the nine-axis magnetic compass by utilizing the interference magnetic field in the step two;
step four, after compensating the magnetic field measured by the nine-axis magnetic compass, collecting the difference between the modulus value of the acceleration vector measured by the three-axis accelerometer of the nine-axis magnetic compass and the modulus value of the gravitational acceleration as the input of fuzzy control, and obtaining the proportional parameter of the complementary filter and the integral parameter of the complementary filter;
and fifthly, calculating a real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the fourth step, and calculating a carrier attitude angle of the nine-axis magnetic compass according to the real-time attitude quaternion.
Further, in the first step, the linear function is:
wherein,,
indicating the position of the magnetic interfering substance->
Representing the magnetic dipole moment of the magnetically disturbing substance +.>
Representing the magnetic field, k, of a magnetically interfering substance 1 、k 2 Is a linear parameter of a linear function.
In the second step, the determination method of the minimum value of the relative distance variation between the magnetic interference substance and the nine-axis magnetic compass carrier is as follows:
wherein,,
k represents the time sequence of a nine-axis magnetic compass, < >>
Representing the position of the magnetic disturbance target relative to the nine-axis magnetic compass carrier at different moments +.>
Magnetic sensor for nine-axis magnetic compass with different momentsMeasured triaxial magnetic field +.>
Representing the magnetic dipole moment, min, of a magnetically interfering substance]Representing a minimum function.
Further, in the second step, the obtaining the disturbing magnetic field at the installation position of the nine-axis magnetic compass according to the magnetic dipole moment calculation is as follows:
wherein,,
indicating disturbing magnetic field +.>
Indicating the position of the magnetic disturbance target relative to the nine-axis magnetic compass carrier,/->
Representing the magnetic dipole moment of the magnetically disturbing substance +.>
Representation->
Is a unit vector of (a).
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.
In the fifth step, the calculating the real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the fourth step is as follows: calculating the system compensation quantity of the nine-axis magnetic compass according to the proportion parameter and the integral parameter, calculating according to the system compensation quantity to obtain a differential representation form of the attitude quaternion, and calculating according to the differential representation form of the attitude quaternion to obtain the real-time attitude quaternion;
the real-time attitude quaternion is determined in the following manner:
wherein Q is k Represent real-time gesture quaternion, Q k-1 The gesture quaternion of the k-1 moment is represented, t represents a time step, and k represents a time sequence of the nine-axis magnetic compass;
Further, in the fifth step, the carrier attitude angle of the nine-axis magnetic compass obtained by calculating according to the real-time attitude quaternion is:
wherein [ q 0 q 1 q 2 q 3 ] k ＝Q k Represent real-time gesture quaternion, q 0 ,q 1 ,q 2 ,q 3 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 disturbing magnetic field is compensated, calculating a pitch angle initial value and a roll angle initial value of the nine-axis magnetic compass through triaxial accelerometer data, and calculating a heading angle initial value of the nine-axis magnetic compass through triaxial magnetic sensor data;
and calculating according to the pitch angle initial value, the roll angle initial value and the course angle initial value to obtain an attitude angle quaternion in the initial state of the nine-axis magnetic compass.
The beneficial effects are that:
(1) A nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering is used for actually calculating 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, compensating the magnetic interference and ensuring the accuracy of geomagnetic field measured by a three-axis magnetic sensor of the nine-axis magnetic compass. By adopting a self-adaptive particle swarm optimization algorithm, the magnetic dipole moment which enables the change of the relative distance between the magnetic interference substance and the nine-axis magnetic compass carrier to be measured to be minimum is found, so that the minimum change of the relative distance between the magnetic interference substance and the nine-axis magnetic compass carrier can be ensured, and the stability of the system is ensured. 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, the problem of divergence of the calculated result is avoided, and meanwhile, the problems of errors and zero drift in the attitude settlement process are avoided. Solving the attitude angle by adopting the quaternion method can avoid the problem that singular points exist in the attitude angle calculated by the traditional method.
(2) The position of the magnetic interference substance, the linear function of the magnetic dipole moment of the magnetic interference substance and the magnetic field of the magnetic interference substance are established, the property that the magnetic field and the magnetic dipole moment are not collinear in a coplanar manner is fully considered, and the real-time position of the magnetic interference substance can be rapidly and efficiently solved.
(3) When the magnetic interference substance is used for calculating the interference magnetic field at the installation position of the nine-axis magnetic compass, 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 nine-axis magnetic compass gesture acquisition method is further reduced.
Drawings
Fig. 1 is a flow chart of attitude angle calculation in the nine-axis magnetic compass attitude acquisition method of the present invention.
Fig. 2 is a flowchart of the calculation of the complementary filter parameters in the nine-axis magnetic compass gesture acquisition method of the present invention.
FIG. 3 is a graph showing membership functions of differences between the modulus of the acceleration vector and the modulus of the gravitational acceleration measured by the triaxial accelerometer of the nine-axis magnetic compass.
Detailed Description
The invention provides a nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering. And then, a self-adaptive particle swarm optimization algorithm is adopted to find out the magnetic dipole moment which enables the relative distance change between the magnetic interference substance and the nine-axis magnetic compass carrier to take the minimum value, and the parameters of the complementary filter are solved through fuzzy control, so that the frequency characteristics of the three-axis accelerometer and the three-axis 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 will now be described in detail by way of example with reference to the accompanying drawings.
As shown in fig. 1, a flow chart of attitude angle calculation in the nine-axis magnetic compass attitude acquisition method of the present invention is shown. A nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering comprises the following steps:
step one, establishing a linear function of the position of the magnetic interference substance, the magnetic dipole moment of the magnetic interference substance and the interference magnetic field of the magnetic interference substance.
The linear function is:
wherein,,
indicating the position of the magnetic interfering substance->
Representing the magnetic dipole moment of the magnetically disturbing substance +.>
Representing the magnetic field, k, of a magnetically interfering substance 1 、k 2 Is a linear parameter of a linear function.
Before the disturbance magnetic field is compensated, the pitch angle initial value and the roll angle initial value of the nine-axis magnetic compass are calculated through triaxial acceleration data, and the course angle initial value of the nine-axis magnetic compass is calculated through triaxial magnetic sensor data.
And calculating a pitch angle and a roll angle according to the coordinate transformation relation of the gravity acceleration at the initial moment under the carrier coordinate system and the geographic coordinate system, calculating a horizontal component of the geomagnetic field by using the pitch angle, the roll angle and the measured triaxial magnetic field data, and calculating an azimuth angle by using an inverse trigonometric function. 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 mainly comprises 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, and the accelerometer only has measured value output in the Z-axis direction, and the measured value is the earth gravity acceleration g. Then there are:
in the formula (1),
for three components of the gravity acceleration measured by the triaxial accelerometer in the three axes of the carrier coordinate system, the pitch angle beta and the roll angle gamma can be calculated by expanding the (1):
the carrier coordinate system is not coincident with the geographic coordinate system. At this time, the components of the earth magnetic field on each axis are superimposed, and the components of the earth magnetic field measured by the triaxial magnetic sensor on the three axes of the carrier coordinate system should be first calculated to obtain their projections in the horizontal plane, so as to solve the heading angle α.
[m x m y m z ]Three components, m, of the geomagnetic field measured by the triaxial magnetic sensor in the carrier coordinate system x ' and m y ' are their projections in a horizontal plane,in the practical application process, the nine-axis magnetic compass is inevitably interfered by various ferromagnetic substances outside, and the long-term accuracy of the azimuth angle of the nine-axis magnetic compass is ensured by accurately measuring the geomagnetic field by means of a magnetic sensor.
And step two, a self-adaptive particle swarm optimization algorithm is adopted to find out the magnetic dipole moment which enables the change of the relative distance between the magnetic interference substance and the nine-axis magnetic compass carrier to be minimum, and the position of the magnetic interference substance is obtained through calculation according to the linear function of the step one.
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 change of the relative distance in the same time
The same applies.
The three-axis magnetic fields measured by the magnetic sensors in the nine-axis magnetic compass at equal time intervals are sequentially as follows
Based on the measured triaxial magnetic field, the positions of the interfering targets relative to the carrier are respectively
Solving the target position and magnetic moment can ultimately translate into a nonlinear optimization problem.
The minimum value of the relative distance variation between the magnetic interference substance and the nine-axis magnetic compass carrier is expressed as follows:
wherein,,
k represents the time sequence of a nine-axis magnetic compass, < >>
Representing magnetic interference targets at different momentsPosition relative to nine-axis magnetic compass carrier, +.>
Three-axis magnetic field measured by magnetic sensor of nine-axis magnetic compass with different moments +.>
Representing the magnetic dipole moment, min, of a magnetically interfering substance]Representing a minimum function.
By classical electromagnetic theory, when the detection point and the target position are larger than 2.5 times of the maximum target size, the magnetic interference substance at one point in space can be equivalent to a magnetic dipole model, and the magnetic dipole moment is
Then at a distance from the magnetic interfering substance
The magnetic potential and the magnetic field are respectively:
as can be seen from the formula, the three vectors are located on the same plane, namely:
wherein k is 1 And k is equal to 2 The solving method of (2) is as follows:
equation (8) is entered into equation (7):
due to
And->
Non-collinear, available in geometric principles:
solving the system of linear equations in equation (10) to obtain k 1 And k is equal to 2 。
Firstly, deducing a calculation method of a known target magnetic moment and a target position when a magnetic field is generated according to a target magnetic dipole model, converting a target inversion problem into a nonlinear optimization problem by assuming that a magnetic target and a carrier do relative linear motion in a short time and the variation of relative distances in the same time are equal, solving the nonlinear optimization problem through a particle swarm optimization algorithm to obtain the target magnetic moment, calculating the target relative position, and finally, calculating an interference magnetic field generated by the target at the carrier position according to a target parameter obtained by calculation to realize magnetic interference compensation.
And thirdly, 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 second step, and compensating the interference magnetic field.
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 second step, wherein the interference magnetic field is as follows:
wherein,,
indicating disturbing magnetic field +.>
Indicating magnetic interference purposePosition of the target relative to nine-axis magnetic compass carrier, < >>
Representing the magnetic dipole moment of the magnetically disturbing substance +.>
Representation->
Is a unit vector of (a).
The disturbing magnetic field is compensated for: 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 value of the acceleration vector and the modulus value of the gravitational acceleration measured by the triaxial accelerometer of the nine-axis magnetic compass 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 a 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 complementary filtering of the system in real time according to the output of the fuzzy control.
When the attitude quaternion output of the system at the initial moment is known, the attitude quaternion of the system can be calculated in real time according to the output of the gyroscope.
The gyroscope is utilized to solve the attitude angle, so that the dynamic response is fast, but accumulated errors, zero drift and the like can be generated in the process of resolving the attitude. The magnetic sensor and the accelerometer have no accumulated error, but have poor transient characteristics, and when the posture of the carrier changes drastically, the requirement of real-time posture calculation cannot be met. Therefore, the advantages of the method can be combined, the gesture angle measured by the gyroscope is subjected to high-pass filtering by utilizing the complementary filtering principle, the gesture angle measured by the magnetic sensor and the accelerometer is subjected to low-pass filtering, and finally, signals with good high frequency range and low frequency range are obtained.
According to the definition of the coordinate transformation matrix, the gravity acceleration of the gravity vector of the geographic coordinate system rotated to the carrier coordinate system is as follows:
The acceleration measured by the triaxial accelerometer in the carrier coordinate system is normalized and then is marked as a b Acceleration g under a carrier coordinate system obtained by coordinate transformation of the acceleration g and gravitational acceleration b The error vector in between can be represented by the cross product of these two vectors as:
e acc (k)＝g b ×a b (12)
this cross product vector is still located on the carrier coordinate system. The gyro integration error is also on the machine body coordinate system, and the size of the cross product is directly proportional to the gyro integration error, so that the gyro integration error is just corrected.
The error vector can likewise be calculated from the magnetic field measurements in the magnetic field carrier coordinate system:
e mag (k)＝m b ×H b (13)
wherein m is b Representing magnetic field value H under carrier coordinate system obtained by coordinate change of geomagnetic field after normalization b The value of the triaxial magnetic field normalization under the carrier coordinate system measured by the triaxial magnetic sensor is shown.
Solving the differential equation in equation (13) using the first-order longlattice-kutta method:
wherein the method comprises the steps of
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, so that the final attitude angle output is obtained.
The effect of the complementary filter is equal to K p 、K i If the selection is not proper, the error of the attitude estimation is large, and even the problem of divergence of the calculated result occurs, the selection needs to be performed according to the output frequency characteristics of the accelerometer and the gyroscope. K taking into account the frequency characteristics of the accelerometer and gyroscope itself p And K i Can be designed according to classical control theory.
The input-output relation of the system in the s domain is as follows:
wherein,,
representing low-pass filter and high-pass filter coefficients, respectively, and F 1 (s)+F 2 (s) =1 satisfies the complementary filtering equation, R 1 (s) represents the output of the accelerometer and the magnetic sensor in the s domain, R 2 (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 K p ＝2ξω n ，/>
ζ is the damping coefficient, ω, of the system n Is the natural vibration angular frequency.
When the system is in a static state or a low acceleration motion state, the precision of the attitude angle measured according to the acceleration is higher, K pacc Can be larger, whereas K is higher when the system is in high acceleration motion pacc A smaller value is required.
Definition:
the difference between the model value representing the acceleration vector and the model value of the gravitational acceleration is approximately zero when the carrier is stationary, and is relatively large when the carrier is in a maneuver state such as acceleration, so that the dynamic condition of the carrier can be reflected.
The err is used as the input of fuzzy control to detect the maneuvering state of the carrier, and the output K of the self-adaptive module pacc Switching frequencies are low-pass and high-pass filters. It can be seen that err approaches zero when the carrier is stationary, at which time K pacc Taking a larger value, the value of err is smaller when the carrier is in low dynamics, at which time K pacc Taking a moderate value, when the carrier is in high dynamic state, the err value is larger, and at the moment, K is larger pacc Take a smaller value. Let the fuzzy set of the fuzzy variable err be { zero, small, large }, the quantized domain of argument be { zero, small, big }, the fuzzy set of err be { large, medium, small }, the quantized domain of argument be { big, medium, small }. Adjustment coefficient K pacc The fuzzy control rule of (2) is as follows:
the membership functions of err are shown in fig. 3, and the defuzzification method adopts a center method.
Output coefficient K of fuzzy control module pacc K can be calculated by combining the damping coefficient xi of the system iacc The triaxial magnetic field after magnetic interference compensation is not influenced by external magnetic interference, K pmag And K imag The fixed value may be kept unchanged.
From the above, the parameters of the complementary filter include the scale parameter K p And integral parameter K i In the present invention, the ratio parameter K p Scale parameter K comprising a triaxial accelerometer pacc And the scale parameter K of the triaxial magnetic sensor pmag The method comprises the steps of carrying out a first treatment on the surface of the Integral parameter K i Integration including a tri-axial accelerometerParameter K iacc And integral parameter K of triaxial magnetic sensor imag 。
And fifthly, calculating a real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter of the fourth step, and calculating a carrier attitude angle of the nine-axis magnetic compass according to the real-time attitude quaternion.
And D, calculating real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the fourth step, wherein the real-time attitude quaternion is as follows: calculating the system compensation quantity of the nine-axis magnetic compass according to the proportional parameter and the integral parameter, calculating according to the system compensation quantity to obtain a differential representation form of the attitude quaternion, and calculating according to the differential representation form of the attitude quaternion to obtain a real-time attitude quaternion;
the real-time gesture quaternion is formulated as:
wherein Q is k Represent real-time gesture quaternion, Q k-1 The gesture quaternion of the k-1 moment is represented, t represents a time step, and k represents a time sequence of the nine-axis magnetic compass;
And calculating according to the real-time attitude quaternion to obtain a carrier attitude angle of the nine-axis magnetic compass, wherein the carrier attitude angle is as follows:
wherein [ q 0 q 1 q 2 q 3 ] k ＝Q k Represent real-time gesture quaternion, q 0 ,q 1 ,q 2 ,q 3 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 three-axis acceleration data, the three-axis angular velocity data and the three-axis magnetic field data of the nine-axis magnetic compass can be utilized to obtain the attitude angle data of the installed carrier through self-adaptive complementary filtering data fusion.
The above specific embodiments merely describe the design principle of the present invention, and the shapes of the components in the description may be different, and the names are not limited. Therefore, the technical scheme described in the foregoing embodiments can be modified or replaced equivalently by those skilled in the art; such modifications and substitutions do not depart from the spirit and technical scope of the invention, and all of them should be considered to fall within the scope of the invention.
Claims (8)
1. The nine-axis magnetic compass gesture acquisition method based on self-adaptive complementary filtering is characterized by comprising the following steps of:
step one, establishing a linear function of the position of the magnetic interference substance, the magnetic dipole moment of the magnetic interference substance and the interference magnetic field of the magnetic interference substance;
secondly, adopting a self-adaptive particle swarm optimization algorithm, utilizing the position of the magnetic interference substance to represent the position parameters of particles, and calculating the position parameters of each particle according to the linear function in the first step; finding out the magnetic dipole moment which enables the change of the relative distance between the magnetic interference substance and the nine-axis magnetic compass carrier to be minimum, and calculating according to the magnetic dipole moment to obtain an interference magnetic field at the installation position of the nine-axis magnetic compass;
step three, compensating the magnetic field measured by the nine-axis magnetic compass by utilizing the interference magnetic field in the step two;
step four, after compensating the magnetic field measured by the nine-axis magnetic compass, collecting the difference between the modulus value of the acceleration vector measured by the three-axis accelerometer of the nine-axis magnetic compass and the modulus value of the gravitational acceleration as the input of fuzzy control, and obtaining the proportional parameter of the complementary filter and the integral parameter of the complementary filter;
and fifthly, calculating a real-time attitude quaternion of the nine-axis magnetic compass according to the proportional parameter and the integral parameter in the fourth step, and calculating a carrier attitude angle of the nine-axis magnetic compass according to the real-time attitude quaternion.
2. The nine-axis magnetic compass gesture obtaining method as claimed in claim 1, wherein in the first step, the linear function is:
wherein,,
indicating the position of the magnetic interfering substance->
Representing the magnetic dipole moment of the magnetically disturbing substance +.>
Representing the magnetic field, k, of a magnetically interfering substance 1 、k 2 Is a linear parameter of a linear function.
3. The method for acquiring the posture of a nine-axis magnetic compass according to claim 1, wherein in the second step, the minimum value of the relative distance variation between the magnetic interference substance and the nine-axis magnetic compass carrier is determined by:
wherein,,
k represents the time sequence of a nine-axis magnetic compass, < >>
Representing the position of the magnetic disturbance target relative to the nine-axis magnetic compass carrier at different moments +.>
Three-axis magnetic field measured by magnetic sensor of nine-axis magnetic compass with different moments +.>
Representing the magnetic dipole moment, min, of a magnetically interfering substance]Representing a minimum function.
4. The nine-axis magnetic compass gesture obtaining method as claimed in claim 1, wherein in the second step, the obtaining the disturbing magnetic field at the nine-axis magnetic compass mounting position according to the magnetic dipole moment calculation is:
wherein,,
indicating disturbing magnetic field +.>
Indicating the position of the magnetic disturbance target relative to the nine-axis magnetic compass carrier,/->
Representing the magnetic dipole moment of the magnetically disturbing substance +.>
Representation->
Is a unit vector of (a).
5. The nine-axis magnetic compass gesture obtaining method as claimed in claim 1, wherein in the 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 method for acquiring the posture of the nine-axis magnetic compass according to claim 1, wherein in the fifth step, the real-time posture quaternion of the nine-axis magnetic compass calculated according to the proportional parameter and the integral parameter in the fourth step is: calculating the system compensation quantity of the nine-axis magnetic compass according to the proportion parameter and the integral parameter, calculating according to the system compensation quantity to obtain a differential representation form of the attitude quaternion, and calculating according to the differential representation form of the attitude quaternion to obtain the real-time attitude quaternion;
the real-time attitude quaternion is determined in the following manner:
wherein Q is k Represent real-time gesture quaternion, Q k-1 The gesture quaternion of the k-1 moment is represented, t represents a time step, and k represents a time sequence of the nine-axis magnetic compass;
7. The method for acquiring the posture of the nine-axis magnetic compass according to claim 1, wherein in the fifth step, the carrier posture angle of the nine-axis magnetic compass calculated according to the real-time posture quaternion is:
wherein [ q 0 q 1 q 2 q 3 ] k ＝Q k Represent real-time gesture quaternion, q 0 ,q 1 ,q 2 ,q 3 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 nine-axis magnetic compass attitude acquisition method according to claim 1, characterized in that, before compensating the disturbing magnetic field, a pitch angle initial value and a roll angle initial value of the nine-axis magnetic compass are calculated by three-axis accelerometer, and a heading angle initial value of the nine-axis magnetic compass is calculated by three-axis magnetic sensor data;
and calculating according to the pitch angle initial value, the roll angle initial value and the course angle initial value to obtain an attitude angle quaternion in the initial state of the nine-axis magnetic compass.
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 CN114440870A (en) | 2022-05-06 |
| CN114440870B true 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 (11)
| 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 |
| 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 |
| 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 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2959019B1 (en) * | 2010-04-14 | 2012-06-08 | Commissariat Energie Atomique | METHOD AND DEVICE FOR COMPENSATING A MEASUREMENT OF A MAGNETIC FIELD, METHOD AND SYSTEM FOR LOCATING AN OBJECT, RECORDING MEDIUM FOR THESE METHODS |
| CN110007350B (en) * | 2019-04-15 | 2020-08-25 | 哈尔滨工业大学 | Analysis method for magnetic detection method blind area |
-
2021
- 2021-12-29 CN CN202111637776.0A patent/CN114440870B/en active Active
Patent Citations (11)
| 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 |
| 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 |
| 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 |
|---|
| 陈正想 等.数字磁罗盘磁干扰补偿方法研究.《水雷战与舰船防护》.2007,第15卷(第3期),6-8. * |
| 陈雷 等.基于模糊自适应互补滤波的姿态解算算法.《兵器装备工程学报》.2017,第38卷(第4期),183-187. * |
| 马剑飞 等.基于正交基函数的矢量磁梯度异常探测.《兵工学报》.2021,第42卷(第11期),2433-2443. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114440870A (en) | 2022-05-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Poddar et al. | A comprehensive overview of inertial sensor calibration techniques | |
| CN106679649B (en) | Hand motion tracking system and method | |
| CN104736963B (en) | mapping system and method | |
| Wang et al. | Estimation of information sharing error by dynamic deformation between inertial navigation systems | |
| CN108731676B (en) | Attitude fusion enhanced measurement method and system based on inertial navigation technology | |
| CN111272158B (en) | Dynamic azimuth angle resolving method of MEMS electronic compass in complex magnetic disturbance scene | |
| CN106403952A (en) | Method for measuring combined attitudes of Satcom on the move with low cost | |
| CN110567492A (en) | Low-cost MEMS inertial sensor system-level calibration method | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| CN110737194B (en) | Multi-MEMS sensor combination attitude measurement method | |
| CN110779514B (en) | Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation | |
| Chou et al. | Two‐step optimal filter design for the low‐cost attitude and heading reference systems | |
| CN114459466A (en) | MEMS multi-sensor data fusion processing method based on fuzzy control | |
| CN114111843A (en) | Initial alignment method for optimal movable base of strapdown inertial navigation system | |
| 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 | |
| CN105303201A (en) | Method and system for handwriting recognition based on motion induction | |
| Štebe et al. | Trajectory tracking of an oscillating movement with a low-cost IMU in geodetic surveying applications | |
| Ben et al. | Real-time heave motion measurement by adaptive band-pass filter based on strapdown INS | |
| CN112882118B (en) | Method and system for estimating gravity vector of movable base under earth-fixed coordinate system and storage medium | |
| Ye et al. | Heading angle estimation using rotating magnetometer for mobile robots under environmental magnetic disturbances | |
| Zhang et al. | Implementation and complexity analysis of orientation estimation algorithms for human body motion tracking using low-cost sensors | |
| Guo et al. | Multi-sensor Data Fusion Using Adaptive Kalman Filter | |
| Narayanan | Performance analysis of attitude determination algorithms for low cost attitude heading reference systems | |
| Chu et al. | Magnetic orientation system based on magnetometer, accelerometer and gyroscope |
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 |