CN115096336A

CN115096336A - Environmental magnetic field interference determination method based on nine-axis MEMS MARG sensor and computer system

Info

Publication number: CN115096336A
Application number: CN202210774533.XA
Authority: CN
Inventors: 张俊; 刘海川; 李宪法
Original assignee: Tianying Technology Shanghai Co ltd
Current assignee: Tianying Technology Shanghai Co ltd
Priority date: 2022-07-01
Filing date: 2022-07-01
Publication date: 2022-09-23

Abstract

The invention relates to an environmental magnetic field interference determination method based on a nine-axis MEMS MARG sensor and a computer system, wherein the method comprises the following steps: s3, comparing the first attitude change information and the second attitude change information respectively calculated by the gyroscope and the magnetometer, if the change direction of the first attitude change information is consistent with that of the second attitude change information, and the attitude difference is within the error range, executing step S4, and if not, executing step S5; s4, judging that the magnetometer data are valid; and S5, determining that the magnetometer data are invalid. According to the technical scheme, whether the magnetometer is interfered by an environmental magnetic field or not is judged by comparing and judging the first attitude change information and the second attitude change information, and then whether magnetometer data are used for attitude angle estimation or only accelerometer and gyroscope data are used for attitude angle estimation or not is determined. Situations where magnetometer information is inaccurate or even completely unavailable in soft magnetic interference environments can be avoided.

Description

Environmental magnetic field interference determination method based on nine-axis MEMS MARG sensor and computer system

Technical Field

The application relates to the field of attitude estimation methods, in particular to an environmental magnetic field interference judgment method based on a nine-axis MEMS MARG sensor and a computer system.

Background

A MARG Sensor (abbreviated as MARG) is a device for measuring the three-axis attitude angle, acceleration and magnetometer of an object, and is generally applied to the fields of unmanned aerial vehicles, aviation, human body attitude tracking and the like. The attitude measurement system composed of a single gyroscope has good dynamic performance, but measurement drift can generate accumulated errors, and the attitude measurement system is not suitable for long-time navigation attitude measurement. The electronic compass measurement system composed of the accelerometer and the magnetometer can also calculate the attitude angle information of the carrier, and usually, in actual use, the data of the gyroscope, the accelerometer and the magnetometer are fused by using a fusion algorithm to obtain relatively accurate attitude angle information.

In the world of our life, the magnetic field of the earth is distributed all over, and in nature, the horizontal component of the magnetic field of the earth environment is derived from the magnetic south pole of the earth and points to the magnetic north pole (the magnetic north is the north indicated by the south needle, which is mainly because the two poles of the magnetic field of the earth are not coincident with the two poles of the north and the south on the earth, so that the north indicated by the compass is the magnetic north and not the true north. The environmental magnetic field around us can be understood as both "hard magnetic" and "soft magnetic". "hard" means a magnet that possesses permanent magnetic properties, such as the earth's magnetic field being a "hard" magnet. "soft magnetic" means a magnet that is magnetized so that it has magnetic properties, but later the magnetic properties may disappear. When the earth magnetic field passes through a metal object, particularly a building with a reinforced concrete structure, the original magnetic field is disturbed to form an indoor magnetic field with another law. Meanwhile, the earth magnetic field is only weak 0.5 gauss under normal conditions, while a common mobile phone loudspeaker still has a magnetic field of about 4 gauss when the distance between the mobile phone loudspeaker and the mobile phone loudspeaker is 2 cm, and a mobile phone motor has a magnetic field of about 6 gauss when the distance between the mobile phone loudspeaker and the mobile phone motor is 2 cm, so that the measurement of the magnetometer is easily interfered by electronic equipment, building structures and the like. In practice, a user is usually recommended to correct the sensor before each use of the sensor, and common correction methods include a plane calibration method, a three-dimensional 8-character calibration method, a ten-plane calibration method and the like. The methods can effectively analyze and correct the 'hard magnetic' and 'soft magnetic' interferences of the use environment. These correction methods have a good correction effect on "hard magnetic" disturbances, since they are relatively stable, but are less satisfactory for "soft magnetic" disturbances, in particular those of the surrounding electronic equipment. Therefore, the nine-axis MEMS MARG sensor can obtain better accuracy in outdoor environment, while in indoor environment, due to various "soft magnetic" interferences existing around, when a user leaves a specific position and enters a different area of the "soft magnetic" interference environment, the operation of the sensor is interfered, and the attitude angle information is inaccurate or even completely unavailable.

Thus, when "soft magnetic" interference is used by conventional sensor fusion algorithms such as complementary filtering, kalman filtering, gradient descent filtering, etc., it is a prerequisite for such algorithms to be designed so that the magnetometer can provide accurate measurement data after calibration. However, a number of studies have shown that although the magnetometer is introduced to reduce the error, it may increase the error. In addition, the reduced error of the sensor fusion algorithm is mainly caused by the sensor deviation of the gyroscope. This error can be modeled because, despite its non-linear and unpredictable nature, it can be approximated as a constant linear drift, the magnitude of which falls within a known range. In contrast, the error introduced by magnetic interference, at least in any practical case, cannot be modeled because it is more unstable and unpredictable and is not as constrained as the sensor error. Furthermore, the error introduced by magnetic interference may be larger than the error accumulated by the magnetometer after it has been ignored.

Therefore, there is a need for an ambient magnetic field disturbance determination method that does not affect the measurement of attitude angle information by magnetic disturbance.

Disclosure of Invention

The invention aims to overcome the defects in the prior art and provide an environmental magnetic field interference judgment method and a computer system based on a nine-axis MEMS MARG sensor, which can avoid attitude angle information measurement errors caused by inaccurate magnetometer information in a soft magnetic interference environment.

In order to achieve the above object, the present invention has the following configurations:

the application comprises an environmental magnetic field interference judgment method based on a nine-axis MEMS MARG sensor, which comprises the following steps:

s1, resolving a static three-dimensional attitude through an accelerometer and a magnetometer in the MARG sensor; (ii) a

S2, when the sensor moves, the attitude change of the sensor after unit time delta t is respectively calculated through a gyroscope and a magnetometer to be used as first attitude change information and second attitude change information;

s3, comparing the first posture change information and the second posture change information, if the change direction of the first posture change information is consistent with the change direction of the second posture change information, and the posture difference between the first posture change information and the second posture change information is within the error range, executing step S4, if not consistent or the posture difference is not within the sampling error range, executing step S5;

s4, judging that the magnetometer is not interfered and the data is valid;

and S5, judging that the magnetometer is interfered and the data is invalid.

In a preferred method for determining the ambient magnetic field disturbance based on the nine-axis MEMS MARG sensor, the step S1 specifically includes the following steps:

s1-1, calculating the data information of a pitch angle and a roll angle in the initial two-dimensional posture of the MARG sensor according to the gravity acceleration by using an accelerometer;

and S1-2, measuring the course angle information of the sensor by using a magnetometer, and calculating to obtain the initial three-dimensional posture of the sensor by combining the initial two-dimensional posture.

In a preferred method for determining the ambient magnetic field disturbance based on the nine-axis MEMS MARG sensor, in step S2, when the sensor is in motion, a gyroscope is used to calculate a change in the attitude of the sensor after a unit time Δ t has elapsed, as first attitude change information; and simultaneously, calculating the attitude change of the sensor after the unit time delta t according to the initial three-dimensional attitude by using a magnetometer to serve as second attitude change information.

In a preferred method for determining ambient magnetic field disturbance based on a nine-axis MEMS MARG sensor, a step S2-0 is further included between step S1 and step S2, an accelerometer is used to perform zero acceleration determination, and the sensor is considered to be in a quasi-static state when a triaxial normalization value of the accelerometer is smaller than a set threshold.

In a preferred method for determining ambient magnetic field disturbance based on a nine-axis MEMS MARG sensor, the method before S3 further includes the steps of:

s3-0, calculating the module value of the triaxial magnetic field data of the magnetometer, judging whether the module value exceeds the set error range of the sensor, and if so, executing S5; if not, go to step S3.

In a preferred method for determining ambient magnetic field interference based on the nine-axis MEMS MARG sensor, the step S4 is specifically to determine that the magnetometer is not interfered, the data is valid, and the accelerometer, the gyroscope, and the magnetometer are used to jointly calculate the attitude change of the sensor.

In a preferred method for determining ambient magnetic field interference based on the nine-axis MEMS MARG sensor, S5 specifically includes determining that the magnetometer is interfered, making the data invalid, and calculating the attitude change of the sensor using an accelerometer and a gyroscope.

In a preferred method for determining the environmental magnetic field interference based on the nine-axis MEMS MARG sensor, the sensor communicates with an embedded MCU or a host computer through a communication interface, and measurement information of an accelerometer, a gyroscope and a magnetometer in the sensor is stored in a corresponding memory.

The present application also includes a computer system comprising a storage device storing computer instructions, said computer system being coupled to and controlling said sensor; the instructions, when executed by a computer, implement a nine-axis MEMS MARG sensor-based ambient magnetic field disturbance determination method, the method comprising the steps of:

S2, when the sensor is moving, calculating the attitude change of the sensor after unit time Δ t as first attitude change information and second attitude change information by a gyroscope and a magnetometer, respectively;

s4, judging that the magnetometer is not interfered and the data is valid;

and S5, judging that the magnetometer is interfered and the data is invalid.

The application relates to an environmental magnetic field interference judging method based on a nine-axis MEMS MARG sensor and a computer system, wherein the method comprises the following steps: s1, calculating the static three-dimensional posture of the MARG sensor; s2, when the sensor is moving, calculating the sensor attitude change after unit time Δ t by a gyroscope and a magnetometer respectively as first attitude change information and second attitude change information; s3, comparing the first posture change information and the second posture change information, if the change direction of the first posture change information is consistent with that of the second posture change information, executing step S4, if not, executing step S5; s4, judging that the magnetometer data are valid; and S5, determining that the magnetometer data are invalid. According to the technical scheme, whether the magnetometer is interfered by an environmental magnetic field or not is judged by comparing and judging the first attitude change information and the second attitude change information, and then whether magnetometer data are used for attitude angle estimation or only accelerometer and gyroscope data are used for attitude angle estimation or not is determined. Situations where magnetometer information is inaccurate or even completely unavailable in soft magnetic interference environments can be avoided.

The technical scheme of the application also provides a technical scheme for judging whether the magnetometer is usable or not, which comprises the step of calculating the mode of the triaxial magnetic field data of the magnetometer, wherein the mode of the magnetometer is required to be a weighing value when the strength of the ambient magnetic field measured by the magnetometer is not interfered when the ambient magnetic field is not interfered. And when the mode of the magnetometer exceeds the error range of the sensor, the magnetometer is judged to be interfered by the external dynamic magnetic field. Step S3 is performed directly without the magnetometer data. The technical scheme can also avoid the influence of the interference of the external dynamic magnetic field on the attitude angle estimation of the magnetometer.

Drawings

Fig. 1 is a diagram of steps of a method for determining ambient magnetic field interference according to a first embodiment of the present application;

FIG. 2 is a diagram of steps of a method for determining ambient magnetic field interference according to a second preferred embodiment of the present application;

FIG. 3 is a schematic diagram of a preferred apparatus structure for applying the method for disturbing an environmental magnetic field and a computer system of the present application.

Detailed Description

The technical solutions in the embodiments of the present disclosure will be clearly and completely described below with reference to the accompanying drawings, and it is to be understood that the described embodiments are only some embodiments of the present disclosure, and not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the disclosed embodiments without making any creative effort, also belong to the protection scope of the present invention.

The invention provides a brand-new environment magnetic field interference determination algorithm based on a nine-axis MEMS MARG gyroscope and a magnetometer, and the algorithm is combined with a traditional fusion algorithm to obtain attitude angle information with higher precision. The conventional sensor fusion process starts with the direction estimation of the gyroscope. First, a gyroscope calculates and generates available attitude angle information (including errors), namely a real yaw value. The error is determined based on the inherent mechanical and electrical characteristics of the sensor, such as noise or bias, which can be modeled and iteratively predicted or assigned a constant correction value, the range of which is typically indicated in the product manual by the MEMS manufacturer. Since this error accumulates over time, in actual use a magnetometer is introduced to correct this error. The magnetometer corrects for this error, and at each time point, the filter will assume that the error associated with the gyroscope's heading estimate is in the opposite direction of the magnetometer's heading (assuming the magnetometer provides a more accurate and non-drifting heading angle reference), and the heading estimate will iteratively drift until a magnetic heading is reached. However, if the magnetometer is interfered, the heading of the magnetometer deviates from the heading of the gyroscope seriously, and at this time, the magnetometer is introduced, not only the error cannot be eliminated, but also the error is increased, and meanwhile, the increase cannot be modeled, so that the output attitude angle information is completely unavailable.

The invention considers that:

(1) the environmental magnetic field is composed of the earth magnetic field and the environmental hard-iron magnetic field, and the output value of the magnetometer is a constant value after calibration, so that whether the magnetometer is interfered by dynamic soft iron can be judged by judging the mode (the size of a magnetic vector) of data of three axes of the magnetometer. When the dynamic soft iron interference is judged, only the gyroscope is used for judging the posture.

(2) The short-time error of the gyroscope is a predictable test that can be modeled, and the cumulative error of the gyroscope during this period is very small and negligible, since the gyroscope can be used as a scale to determine the data from the magnetometer. When the direction of the data of the magnetometer is consistent with the direction of the gyroscope and the data of the magnetometer falls in the error space of the gyroscope, the data of the current magnetometer is considered not to be interfered, and the data of the magnetometer is introduced to calculate the final attitude angle; and when the direction of the data of the magnetometer is inconsistent with the direction of the gyroscope or the data of the magnetometer does not fall in the error space of the gyroscope, the data of the current magnetometer is considered to be interfered, the data of the magnetometer is excluded from calculating the final attitude angle, and the attitude angle is calculated by only using the data of the gyroscope and the data of the accelerometer until the data of the magnetometer is judged to be not interfered again.

The specific content of the invention comprises:

as shown in fig. 1 and 2, the method for determining the ambient magnetic field interference based on the nine-axis MEMS MARG sensor comprises the following steps:

s4, judging that the magnetometer is not interfered and the data is valid;

and S5, judging that the magnetometer is interfered and the data is invalid.

The step S1 specifically includes the steps of,

s1-1, calculating the data information of a pitch angle (pitch) and a roll angle (roll) in the initial two-dimensional posture of the MARG sensor according to the gravity acceleration by using an accelerometer;

s1-2, resolving the data information measured by the magnetometer according to the initial two-dimensional attitude, and obtaining course angle (yaw) information based on the magnetometer so as to obtain the static three-dimensional attitude of the sensor.

As will be appreciated, pose refers to the positional relationship between one coordinate system relative to another coordinate system, as defined by a set of pose angles: heading angle, pitch angle and roll angle.

In a preferred embodiment, the present application uses a three axis accelerometer and a three axis magnetometer to calculate the initial attitude of the carrier. When measuring attitude with an accelerometer, the accelerometer is used as an inclinometer. When the carrier is in a static state or a uniform motion state, the linear acceleration of the carrier is zero, and the output of the accelerometer only has gravity acceleration, namely:

wherein:

is a unit vector of gravity acceleration; θ is expressed as pitch angle; phi is expressed as a roll angle;

from the above formula, it can be seen that only pitch angle (pitch) and roll angle (roll) data information can be obtained when measuring attitude with a three-axis accelerometer. Pitch and roll angles may be expressed as:

θ＝arctan2(a _y ,a _z )

since the accelerometer is not sensitive to rotation of the vertical axis, it is not possible to measure the heading angle (yaw) of the sensor using the accelerometer, and therefore the solution of the present application uses a three-axis magnetometer to calculate the heading angle of the sensor.

The magnetometer is a sensor used for measuring the intensity of the earth magnetic field, and is used for calculating a heading angle in an attitude angle when measuring the attitude, wherein the heading angle is obtained by solving the component of the three-axis magnetometer in the horizontal direction. When the three axis magnetometer is horizontally positioned, the heading angle can be calculated from the measurements of the magnetometer x and y axes:

preferably, the initial attitude of the sensor in actual use is not horizontal, so the magnetometer tilt needs to be compensated in combination with the pitch and roll angles measured by the accelerometer.

Preferably, since the directions of the magnetic and geographical north poles do not coincide, the yaw angle needs to be compensated according to the magnetic yaw angle:

wherein

The declination is positive with north declination being positive, and negative with north declination being negative.

And determining static three-dimensional attitude information according to the pitch angle, the roll angle and the heading angle.

Preferably, the accelerometer may be calibrated according to the gravity acceleration of each region before the measurement of the static three-dimensional attitude information, and the value measured during the accelerometer calibration is used as the local gravity acceleration.

Preferably, the magnetometer needs to be calibrated before the initial three-dimensional attitude measurement. The calibrated magnetometer can be expressed as the following formula (earth stands for the earth's magnetic field and hardiron stands for the hard ferromagnetic field: the effect of permanent magnets in the environment on the magnetometer):

m _real ＝m _earth +m _hardiron ；

in a preferred embodiment, the method between S1 and S2 further comprises the steps of:

and S2-0, performing zero acceleration judgment by using the accelerometer, and considering that the sensor is in a quasi-static state when the triaxial normalization value of the accelerometer is less than a threshold value.

In the quasi-static state, the judgment of the SMD has no meaning because the SMD does not move. The accelerometer is used to determine whether the sensor is in motion. The accelerometer judges the linear acceleration in all directions and also judges the gravity acceleration g, and only judges the gravity acceleration in a static state. The formula is as follows:

the SMD algorithm is therefore activated when the sensor is considered to be in a quasi-stationary state, and only when the sensor is in a moving state. The SMD algorithm step includes steps S2-S5.

In a preferred embodiment, the step S2 is to solve, by using a gyroscope, a change in the posture of the sensor after a unit time Δ t has elapsed as the first posture change information when the sensor is in motion; and simultaneously, calculating the attitude change of the sensor after the unit time delta t according to the initial three-dimensional attitude by using a magnetometer to serve as second attitude change information.

When the accelerometer detects that the sensor starts moving, the SMD algorithm is activated, and measurement and attitude change information are started to be recorded. And taking the static three-dimensional attitude information as a reference, and after a set unit time delta t, taking the attitude change calculated by the gyroscope as first attitude change information and taking the attitude change calculated by the magnetometer as second attitude change information.

A gyroscope is an inertial element that measures the rotation of the carrier. The three-axis gyroscope can sense and output the rotation rate of the carrier around three coordinate axes in real time. And obtaining the real-time attitude angle of the carrier by integrating the angular velocity. The gyroscope itself cannot determine the initial attitude angle, which is provided by the static three-dimensional attitude information described above. And obtaining attitude angle information based on the static three-dimensional attitude information through the components of the three-axis magnetometer in the horizontal direction.

The method uses the acceleration and the magnetometer to calculate the attitude vector observation, and the acceleration measures the gravity vector and the magnetometer measures the magnetic field vector. The method selects a Quest algorithm commonly used in the field of vector observation to perform attitude observation.

Preferably, the second attitude change information is determined by an earth coordinate system, the origin of which is a certain point in the sensor, the earth coordinate system having a Z axis perpendicular to the horizontal surface, a Y axis in the direction of magnetic north, and an X axis in the direction of horizontal east. The first attitude change information is determined by a carrier coordinate system, the carrier coordinate system can be determined according to the actual use scene of the sensor, and the origin of the carrier coordinate is the same as the geographical coordinate system.

In the present application, when comparing the first posture change information and the second posture change information, an earth coordinate system is used as a reference standard, so that the first posture change information needs to be converted into an earth coordinate system for representation.

Preferably, one-time coordinate system conversion is carried out, two pieces of attitude change information are guaranteed to be in the same coordinate system, and in the invention, the attitude is uniformly converted into the earth coordinate system to be compared. Specifically, step S3 is to compare the first posture change information and the second posture change information in the global coordinate system, and determine whether the second posture change information measured by the magnetometer is consistent with the change direction of the first posture change information and has an error, with the first posture change information of the gyroscope as a scale.

And when the magnetometer data and the gyroscope data have the same change direction and fall into the error space of the gyroscope data, the data of the current magnetometer is considered to be not interfered. Step S4 is executed to introduce the magnetometer to perform final attitude angle calculation together with the accelerometer and gyroscope when measuring the attitude angle.

When the variation direction of the magnetometer data is inconsistent with the variation direction of the gyroscope data or the magnetometer data does not fall within the error space of the gyroscope, the current magnetometer data is considered to be interfered, step S5 is executed to exclude the magnetometer data from the calculation of the final attitude angle, and only the gyroscope and the accelerometer data are used for calculating the attitude angle until the magnetometer data is judged to be not interfered again.

The error space is a preset specific value and can be determined by a user according to equipment parameters, specific use scene requirements and the like or determined when the user leaves a factory.

In the technical scheme of the application, the gyroscope is used as the scale to judge the error of the magnetometer, the error of the gyroscope in a short time is a predictable test which can be modeled, and meanwhile, the accumulated error of the gyroscope in the period is very small and can be ignored, so that the gyroscope can be used as the scale to judge the data of the magnetometer.

Preferably, the comparison is performed in two steps, i.e. the comparison of the magnetometer data with the direction of change of the gyroscope data and whether the magnetometer data are within the error space of the gyroscope data. Firstly, comparing the direction of change of magnetometer data and gyroscope data, wherein the comparison content is the direction change process and the final direction. Secondly, whether the magnetometer data are in an error space of the gyroscope data needs to be judged, wherein the error space refers to an error pointed by the changed data and also includes an error in a direction changing process. When the course of the direction change of the magnetometer data does not coincide with the course of the direction change of the gyroscope or is greater than the error space, it is determined that the magnetometer data is not usable, and step S5 is performed. When the course of the direction change of the magnetometer data is consistent with the course of the direction change of the gyroscope, the final pointing direction is consistent, and it is determined that the course of the direction change and the final pointing direction are both within the error range, the magnetometer data is determined to be usable, and step S4 is executed.

The direction change process is consistent and the final direction is consistent, and the consistency is judged to be consistent when the direction change process is within a set threshold value range. Preferably, the comparison of the direction change process may be directly determined by the difference of the corresponding three-axis data, and if the difference of the three-axis data at the same time point is greater than a set threshold, the change directions are determined to be inconsistent, otherwise, the change directions are determined to be consistent.

Preferably, the SMD algorithm is used in the present application to determine the reliability of magnetometer data, and first, the mathematical models of the accelerometer, the gyroscope, and the magnetometer are as follows (measure is a measurement value, real is a true value, noise is a measurement error, and drift is):

a _measure ＝a _real +a _noise +a _drift

ω _measure ＝ω _real +ω _noise +ω _drift

m _measure ＝m _real +m _noise +m _drift

the accelerometer and magnetometer have small long-term drift, and in the invention, only gyroscope data in a short time are used, so that the drift data can be ignored, and therefore, a model can be simplified as follows:

a _measure ＝a _real +a _noise

ω _measure ＝ω _real +ω _noise

m _measure ＝m _real +m _noise

in a preferred embodiment, the yaw angle of the first attitude change information and the yaw angle of the second attitude change information are compared to determine whether the data of the magnetometer is interfered, and the pitch angle and the roll angle are calculated by the accelerometer as follows:

θ＝arctan2(a _y ,a _z )

then, the course angle is calculated through the pitch angle, the roll angle and the sum of the following steps:

by comparing the course angle of the last time point

The attitude is then calculated by the gyroscope:

by converting the quaternion attitude to euler angles, the azimuth can be found to be:

also compare the course angle of the last time pointTo obtain

Thereby when

The magnetometer is considered to be disturbed by the surrounding dynamic soft ferromagnetic field.

In a preferred embodiment, the comparison between the attitude information measured by the activated magnetometer and the attitude information of the gyroscope can be performed by comparing in real time or at intervals of a set time or by detecting the position movement of the sensor, so as to determine whether the magnetometer is still interfered by the dynamic soft iron, and determine whether to introduce the magnetometer to calculate the attitude angle.

In other preferred embodiments, the manner of activating the comparison step further includes monitoring the dynamic soft-iron magnetic field in real time, and if the dynamic soft-iron magnetic field changes, comparing and determining the attitude information measured by the magnetometer with the attitude information of the gyroscope.

In a preferred embodiment, the method further includes step S3-0, calculating a modulus of the triaxial magnetic field data of the magnetometer, determining whether the modulus exceeds the set error range of the sensor, and if so, executing step S5; if not, go to step S3.

The above-mentioned step S3-0 can be performed before step S3 and also before step S2, and the mode of the triaxial magnetic field data of the magnetometer is first calculated, because the mode of the magnetometer should have a balanced value when the strength of the ambient magnetic field measured by the magnetometer is not interfered. And when the mode of the magnetometer exceeds the error range of the sensor, the magnetometer is judged to be interfered by an external dynamic magnetic field. Then no magnetometer data is used and only accelerometers and gyroscopes are used for attitude output. If the mode of the magnetometer does not exceed the error range of the sensor, the step S2 is continued, the mode is compared with the attitude change measured by the gyroscope, and it is further determined whether the magnetometer is interfered by the external dynamic magnetic field.

Preferably, the step S3-0 is not limited to a specific step or a specific time, and may be performed in real time, at a set time interval, at a time point at which a positional shift of the sensor is detected, or at a set specific time point.

Preferably, when the mode of the triaxial magnetic field data of the magnetometer is detected again and does not exceed the sensor error range, the magnetometer is determined not to be interfered by the external dynamic magnetic field, and then step S3 is executed.

Since both the earth magnetic field and the hard ferromagnetic field are constant values, the modulus of the data measured by each axis of the magnetometer is a constant value when the attitude changes, i.e., the earth magnetic field plus the hard ferromagnetic field. A magnetometer is considered to be disturbed by the surrounding dynamic soft ferromagnetic field when the modulus of the reading of the magnetometer at a certain time is larger than the calibration modulus.

The method for judging the interference of the environmental magnetic field can effectively judge whether the environmental magnetic field is interfered or not (1) and effectively relieve attitude estimation disorder caused by the magnetic field interference (2). Firstly, calculating a model of triaxial magnetic field data of the magnetometer to judge whether the magnetometer is interfered by an environmental magnetic field; the second step is to calculate the initial attitude (pitch angle, roll angle) of the sensor through the accelerometer, then calculate the attitude change of the heading angle of the magnetometer by using the initial attitude and compare the attitude change with the yaw angle data acquired by the gyroscope. And comparing the static and dynamic magnetometer data with the scale data respectively to judge whether the magnetometer is interfered by an environmental magnetic field or not, and then determining whether to use the magnetometer data for attitude angle estimation or to use only the accelerometer and gyroscope data for attitude angle estimation. The method can be applied to the enhancement of attitude estimation algorithms such as complementary filtering, Kalman filtering, gradient descending filtering and the like of the traditional sensor fusion algorithm so as to further improve the accuracy of the attitude estimation of the sensor.

Fig. 2 is a diagram illustrating a second embodiment of a preferred ambient magnetic field disturbance determination method, in which an accelerometer and a gyroscope (inertial device) acquire pitch angle and roll angle data in a carrier coordinate system, and the magnetometer may acquire heading angle data in the carrier/earth coordinate system. And converting the pitch angle data, the roll angle data and the heading angle data into data based on an earth coordinate system.

And judging whether the obtained magnetometer data is effective or not through SMD algorithm operation, calculating the modulus of the triaxial magnetic field data of the magnetometer, and judging whether the modulus exceeds the set error range of the sensor or not. And if the data exceeds the threshold value, determining that the magnetometer data are invalid, and if the data does not exceed the threshold value, determining that the magnetometer data are valid.

And after the pitch angle and the roll angle data are converted into a terrestrial coordinate system, judging whether the sensor moves or not through data change, and if so, further judging whether magnetometer data are effective or not during the motion of the sensor. Calculating the attitude change of the sensor after unit time delta t through a gyroscope and a magnetometer respectively to serve as first attitude change information and second attitude change information; comparing the first posture change information with the second posture change information, if the change directions of the first posture change information and the second posture change information are consistent, the magnetometer data are valid, and if the change directions of the first posture change information and the second posture change information are inconsistent, the magnetometer data are invalid.

When magnetometer data are judged to be invalid, only the accelerometer and the gyroscope are used for resolving the attitude, in the resolving process, magnetometer data are continuously collected, and whether the magnetometer is effective or not is judged through SMD algorithm operation, and when the magnetometer data are effective, the attitude is resolved jointly by the magnetometer, the accelerometer and the gyroscope.

In another preferred embodiment, the magnetometer is calibrated before performing step S2, the calibration is to screen and calculate data obtained by the magnetometer to obtain magnetometer data used for generating the second attitude change information, and the screening and calculating method includes:

(1) acquiring data large values and small values of X, Y and Z axial values of the magnetometers and storing the data large values and the data small values in a buffer; the data large value and the data small value are judged by setting a data large value buffer area threshold value and a data small value buffer area threshold value and are stored into corresponding buffers;

(2) respectively setting forgetting factors for discarding the expired data large values and the expired data small values to obtain buffer area data; setting a forgetting factor for each axial value entering the buffer, wherein the initial value of the forgetting factor is N, and each time the later buffer enters a new value, the forgetting factors of other axial values in the buffer are reduced by 1, and the forgetting factors are not reduced continuously when the forgetting factors are reduced to 0. When the buffer enters one axial value every time, the data in the corresponding buffer area is reordered according to the size; judging whether the buffer area is full, if not, not throwing away the axial value; if the data is full, throwing away the maximum axial value and the axial value with the minimum forgetting factor for the data large value buffer area; and for the data small value buffer, throwing away the minimum axial value and the axial value with the minimum forgetting factor. And (4) discarding the expired data with small extreme values and genetic factors to obtain the data of the buffer area.

And finally, calculating data in the buffer area in a weighted average mode to obtain magnetometer data, wherein the magnetometer data is used for other calculation and function execution at the time point and comprises the step of generating second attitude change information for comparing and judging with the first attitude change information to determine whether the magnetometer data is invalid or not.

The expired data and the extreme value are thrown away through the setting of the forgetting factor, the magnetometer is calibrated in real time, whether the data of the magnetometer are valid or not is further judged by the calibrated data, the data validity of the magnetometer can be judged more accurately, and the interference of the expired data and the extreme value on the data validity judgment is avoided.

The present application also includes a computer system comprising a storage device storing computer instructions, as shown in FIG. 3, said computer system being connected to and controlling said sensor; the instructions, when executed by a computer, implement a method for ambient magnetic field disturbance determination based on a nine-axis memmurarg sensor, the method comprising the steps of:

s1, resolving a static three-dimensional attitude according to an accelerometer and a magnetometer of the MARG sensor;

s2, when the sensor is moving, calculating the sensor attitude change after unit time Δ t by a gyroscope and a magnetometer respectively as first attitude change information and second attitude change information;

and S4, when the second attitude change information is consistent with the first attitude change information in direction or falls in an error space of the first attitude change information, and the error space is a preset specific value, the data of the current magnetometer is considered not to be interfered, and the magnetometer, the accelerometer and the gyroscope are introduced to jointly calculate the final attitude angle when the attitude angle is measured.

And S5, when the direction of the second attitude change information is not consistent with the direction of the first attitude change information or the second attitude change information does not fall in the error space of the first attitude change information, the data of the current magnetometer is considered to be interfered, the data of the magnetometer is excluded from the calculation of the final attitude angle, and the calculation of the attitude angle is carried out only by using the data of the gyroscope and the accelerometer until the data of the magnetometer is judged to be not interfered again. The instructions include any of the technical solutions described above in the present application.

As used in this application and the appended claims, the terms "comprises" and "comprising" are generally intended to cover only the explicitly recited steps or elements as not constituting an exclusive list and that the method or apparatus may include other steps or elements.

The components, relative arrangements, functions, and numerical values set forth in these embodiments do not limit the scope of the present invention unless specifically stated otherwise. Also, it is apparent that the dimensions of the various parts shown in the drawings are not drawn to scale in practice for ease of description. Techniques, methods and apparatus that are known to those of ordinary skill in the relevant art have not been described in detail for the time being, but are intended to be part of the specification as appropriate. In all examples shown and discussed herein, any particular value should be construed as exemplary only and not as limiting. Thus, other examples of step-wise embodiments may have a different order of precedence.

The foregoing is illustrative of the present invention and is not to be construed as limiting thereof. Although a few exemplary embodiments of the present invention have been described, those skilled in the art will readily appreciate that many modifications are possible in the exemplary embodiments without materially departing from the novel teachings and advantages of this invention. Accordingly, all such modifications are intended to be included within the scope of this invention as defined in the claims. It is to be understood that the foregoing is illustrative of the present invention and is not to be construed as limited to the specific embodiments disclosed, and that modifications to the disclosed embodiments, as well as other embodiments, are intended to be included within the scope of the appended claims. The invention is defined by the claims and their equivalents.

Claims

1. An environmental magnetic field interference determination method based on a nine-axis MEMS MARG sensor is characterized by comprising the following steps:

s1, resolving a static three-dimensional attitude through an accelerometer and a magnetometer in the MARG sensor;

s2, when the sensor is moving, calculating the attitude change of the sensor after unit time Δ t as first attitude change information and second attitude change information by a gyroscope, an acceleration and a magnetometer, respectively;

s4, judging that the magnetometer is not interfered and the data is valid;

and S5, judging that the magnetometer is interfered and the data is invalid.

2. The method for determining ambient magnetic field disturbance based on a nine-axis MEMS MARG sensor as claimed in claim 1, wherein the step S1 specifically comprises the following steps:

3. The method for determining ambient magnetic field disturbance based on a nine-axis MEMS MARG sensor according to claim 2, wherein S2 is specifically configured to, when the sensor is in motion, use a gyroscope to solve a change in the attitude of the sensor after a unit time Δ t has elapsed as first attitude change information; and simultaneously, calculating the attitude change of the sensor after the unit time delta t according to the initial three-dimensional attitude by using a magnetometer to serve as second attitude change information.

4. The method of claim 1, wherein the step S1 and the step S2 are further performed with the following steps:

and S2-0, performing zero acceleration judgment by using the accelerometer, and considering that the sensor is in a quasi-static state when the triaxial normalization value of the accelerometer is smaller than a set threshold value.

5. The method of claim 1, wherein the step of determining ambient magnetic field disturbance based on a nine-axis MEMS MARG sensor further comprises, before S3, the steps of:

6. The method for determining ambient magnetic field disturbance based on a nine-axis MEMS MARG sensor of claim 1, wherein S4 is specifically configured to determine that a magnetometer is not disturbed and data is valid, and the accelerometer, the gyroscope, and the magnetometer are used to jointly calculate the attitude change of the sensor.

7. The method for determining ambient magnetic field disturbance based on a nine-axis MEMS MARG sensor as claimed in claim 1, wherein S5 is specifically to determine that the magnetometer is disturbed, the data is invalid, and the attitude change of the sensor is resolved by using an accelerometer and a gyroscope.

8. The method for determining ambient magnetic field interference based on a nine-axis MEMS MARG sensor as claimed in claim 1, wherein the sensor communicates with an embedded MCU or a host computer through a communication interface, and the measurement information of the accelerometer, gyroscope and magnetometer in the sensor is stored in a corresponding memory.

9. A computer system comprising a storage device storing computer instructions, wherein said computer system is coupled to and controls said sensor; the instructions, when executed by a computer, implement a nine-axis MEMS MARG sensor-based ambient magnetic field disturbance determination method, the method comprising the steps of:

s3, comparing the first posture change information with the second posture change information, if the change directions of the first posture change information and the second posture change information are consistent and the posture difference between the first posture change information and the second posture change information is within the error range, executing step S4, if the change directions of the first posture change information and the second posture change information are inconsistent or the posture difference is not within the sampling error range, executing step S5;

s4, judging that the magnetometer is not interfered and the data is valid;

and S5, judging that the magnetometer is interfered and the data is invalid.