CN112033405B - Indoor environment magnetic anomaly real-time correction and navigation method and device - Google Patents

Indoor environment magnetic anomaly real-time correction and navigation method and device Download PDF

Info

Publication number
CN112033405B
CN112033405B CN202010794600.5A CN202010794600A CN112033405B CN 112033405 B CN112033405 B CN 112033405B CN 202010794600 A CN202010794600 A CN 202010794600A CN 112033405 B CN112033405 B CN 112033405B
Authority
CN
China
Prior art keywords
correction
magnetic
time
axis
acceleration
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010794600.5A
Other languages
Chinese (zh)
Other versions
CN112033405A (en
Inventor
薛原
黄雄
张挺
刘宇
刘善涛
孟来泉
张南
张韵
孟腾飞
姜伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Mogao Technology Co ltd
Original Assignee
Beijing Mogao Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Mogao Technology Co ltd filed Critical Beijing Mogao Technology Co ltd
Priority to CN202010794600.5A priority Critical patent/CN112033405B/en
Publication of CN112033405A publication Critical patent/CN112033405A/en
Application granted granted Critical
Publication of CN112033405B publication Critical patent/CN112033405B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
    • G01C17/38—Testing, calibrating, or compensating of compasses
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/06—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving measuring of drift angle; involving correction for drift
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

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

Abstract

The invention provides a real-time correction and navigation method and a device for indoor environmental magnetic anomaly, wherein the method comprises the following steps: determining a magnetic abnormal quantity Δ m (t) at time t; correcting the geomagnetic vector according to the magnetic abnormal quantity delta m (t) at the time t to obtain a geomagnetic vector correction quantity Mb at the time tCorrection(t); using the geomagnetic vector correction amount Mb at the time tCorrection(t) determining a rotation matrix c (t); determining a heading from the rotation matrix c (t). The invention carries out online estimation on the magnetic anomaly and compensates the magnetic anomaly in real time, so that the course can be corrected by utilizing the output of the magnetic field intensity in the magnetic anomaly area, and the problem of course divergence of the traditional algorithm under the indoor magnetic anomaly condition is avoided.

Description

Indoor environment magnetic anomaly real-time correction and navigation method and device
Technical Field
The invention belongs to the technical field of indoor navigation, and particularly relates to a method and a device for correcting and navigating indoor environment magnetic anomaly in real time.
Background
In an indoor environment, a building structure can have certain and stable influence on the intensity of a geomagnetic field and a declination. The sources of this effect are typically: household appliances, reinforcing steel bars, electric wires, and the like. Because the geomagnetic intensity is relatively small, the heading cannot be corrected frequently due to indoor magnetic anomaly, and the divergence of the MEMS navigation in a heading channel is caused.
In the prior art, under the condition that magnetic anomaly exists, the course is not corrected, so that the course is diverged when the magnetic anomaly exists in a magnetic anomaly environment for a long time. Aiming at the problems, when the magnetic sensor is in an indoor magnetic abnormal environment for a long time, the corrected magnetic vector can be effectively used for correcting the course, so that the speed of course divergence is greatly reduced.
Disclosure of Invention
Aiming at the problems, the invention provides a real-time correction and navigation method for indoor environmental magnetic anomaly, which comprises the following steps:
determining a magnetic abnormal quantity Δ m (t) at time t;
correcting the geomagnetic vector according to the magnetic abnormal quantity delta m (t) at the time t to obtain a geomagnetic vector correction quantity Mb at the time tCorrection(t)；
Using the geomagnetic vector correction amount Mb at the time tCorrection(t) determining a rotation matrix c (t);
determining a heading from the rotation matrix c (t).
Further, the method specifically comprises the following steps:
step a, the MEMS sensor is static, the initial course is determined by using magnetic output Mb (0), the initial roll and pitch are determined by using accelerometer output, wherein Mb (0) is expressed as the projection of a geomagnetic vector on a body coordinate, namely the magnetic output value of the MEMS sensor at the initial moment;
b, calculating a magnetic abnormal quantity delta m (t) at the time t by using a basic quaternion integral algorithm;
step c, correcting the geomagnetic vector at the time t to obtain a corrected geomagnetic vector MbCorrection，MbCorrection(t)＝Mb(t)-CT(t)ΔMtWhere C (t) is a rotation matrix calculated at time t using only gyro data, CT(t) denotes C (t) a transposed matrix at time t, Mb (t) denotes the projection of the geomagnetic vector on the volume coordinate, i.e. the magnetic output value of the MEMS sensor at time t, Δ MtExpressed as the average of N periods of magnetic anomalies greater than a set threshold;
and d, correcting (t) by using the corrected geomagnetic vector Mb, combining the acceleration output and the gyroscope output, recalculating the rotation matrix Cnew (t) and determining the course.
Further, the calculation of the magnetic abnormal quantity Δ m (t) at the time t in the step b comprises the following steps:
step b1, calculating a geomagnetic vector Me (0) ═ C (0) × Mb (0) at an initial time, and juxtaposing a magnetic abnormal amount Δ M ═ 0, wherein C (0) represents a rotation matrix from a body coordinate system to a geodetic coordinate system at the initial time, and the rotation matrix C (0) from the body coordinate system to the geodetic coordinate system at the initial time is obtained by quaternion;
b2, acquiring 9-axis sensor data, and calculating a magnetic vector Me (t) ═ C (t) × Mb (t) at the time t on the earth coordinate, wherein C (t) is a rotation matrix calculated by only using gyro data at the time t, and Mb (t) is a projection of the geomagnetic vector on the body coordinate, namely a magnetic output value of the MEMS sensor at the time t;
and b3, calculating the magnetic abnormal quantity Δ m (t) ═ Me (t) — Me (0) at the time t, and storing the magnetic abnormal quantity in a buffer.
Further, an average value of the N periodic magnetic anomalies
Figure BDA0002625078560000021
If tmp is greater than the set threshold, updating the magnetic anomaly delta Mt＝tmp。
Further, the 9-axis sensor data includes angular velocities, accelerations, and magnetic vectors of three xyz axes in a body coordinate system obtained by the MEMS sensor, where the angular velocities of the three xyz axes are referred to as gyro data; the angular velocity data is obtained by a gyroscope, the acceleration data is obtained by an accelerometer, the magnetic vector data is obtained by a magnetometer, and the magnetometer can be a magnetic sensor.
Further, the algorithm also comprises a correction method for an accelerometer in the MEMS sensor, and the method comprises the following steps:
step e1, judging whether the acceleration is abnormal, and judging whether the acceleration is abnormal when the module value sqrt (ax) of the triaxial accelerometer2+ay2+az2) When the difference between the acceleration value and 1g is larger, the accelerometer correction is not carried out, wherein 1g represents a gravity acceleration;
step e2, acceleration normalization, a ═ axayaz]T，ANewA/norm (a), where ax is the acceleration of the x-axis, ay is the acceleration of the y-axis, az is the acceleration of the z-axis, and norm is the matrix norm;
step e3, calculating the acceleration deviation Ea ═ ANewX V, wherein V ═ CT[001]TExpressed as the projection of the gravitational acceleration in the body coordinate system.
Further, the algorithm also comprises a correction method for the magnetometer in the MEMS sensor, and the method comprises the following steps:
step f1, magnetometer normalization: m ═ mxmymz]T，MNewM/norm (M), where mx denotes a magnetometer of x axis, my denotes a magnetometer of y axis, and mz denotes a magnetometer of z axis;
step f2, calculating the magnetic field deviation Em ═ MNewX W, wherein W ═ CTMe (t) is expressed as the projection of the magnetometer in the body coordinate system.
Further, the algorithm comprises a correction method for human body posture strapdown solution, and the method comprises the following steps:
step g1, initializing quaternion q and initializing gyro deviation δ ═ δx δy δz]T＝[0 0 0]TWherein, deltaxGyro deviation, δ, expressed as the x-axisyGyro deviation, delta, expressed as y-axiszGyro deviation, expressed as the z-axis;
step g2, calculating correction rate of three axes of xyz, w ═ w0+ δ wherein w0＝[wx wy wz]TExpressed as three-axis angular rate, wx expressed as x-axis angular rate, wy expressed as y-axis angular rate, and wz expressed as z-axis angular rate;
step g3, calculating the corrected
Figure BDA0002625078560000042
Figure BDA0002625078560000041
Wherein q is [ q ]0q1 q2 q3]T。
Further, the algorithm comprises nonlinear complementary correction on the attitude quaternion, deltaCorrection＝Kp*e+KIEdt, where e ═ Em + Ea, Kp represents the P term coefficient in PID control, i.e. the proportional term coefficient, KIExpressed as I term coefficients in PID control, i.e., integral term coefficients.
Furthermore, the present invention also provides a device for correcting and navigating the indoor environmental magnetic anomaly in real time, which is characterized in that the device comprises:
a magnetic abnormal amount determination unit for determining a magnetic abnormal amount Δ m (t) at time t;
a geomagnetic vector correction amount determination unit for correcting the geomagnetic vector according to the magnetic abnormal amount Δ m (t) at the time t to obtain a geomagnetic vector correction amount Mb at the time tCorrection(t)；
A rotation matrix C (t) determining unit for utilizing the geomagnetic vector correction amount Mb at the time tCorrection(t) determining a rotation matrix c (t);
and the heading determining unit is used for determining the heading according to the rotation matrix C (t).
According to the method and the device for correcting and navigating the indoor environment magnetic anomaly in real time, the magnetic anomaly is estimated on line and compensated in real time, so that the course can be corrected by utilizing the output of the magnetic field intensity in a magnetic anomaly area, and the problem of course divergence of the conventional algorithm under the indoor magnetic anomaly condition is solved; meanwhile, the method adopts twice inertial navigation algorithm calculation, only uses the basic quaternion integral algorithm for the first time, aims to solve the magnetic anomaly vector, and can obtain higher attitude precision by using the basic quaternion method in consideration of short time, thereby being feasible. And resolving the magnetic anomaly by using a first navigation algorithm. And then, compensating the geomagnetic signals by using the known magnetic anomaly information, and then updating the rotation matrix C by reusing the 9-axis navigation algorithm to determine the heading.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and those skilled in the art can also obtain other drawings according to the drawings without creative efforts.
FIG. 1 is a schematic diagram illustrating a flow of a real-time indoor environment magnetic anomaly correction and navigation method according to an embodiment of the present invention;
FIG. 2 illustrates a graph of quaternion q in an embodiment of the invention;
fig. 3 is a schematic diagram illustrating an indoor environment magnetic anomaly real-time correction and navigation apparatus according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be obtained by a person skilled in the art without making any creative effort based on the embodiments in the present invention, belong to the protection scope of the present invention.
The invention mainly aims at the requirement of indoor MEMS navigation. MEMS sensors are typically placed on a person for measuring body gestures. When people move indoors, magnetic field vectors of all places are different, and under the condition, magnetic field measurement cannot be introduced to correct an algorithm, so that the course gradually diverges.
The invention measures 3-axis acceleration and 3-axis angular velocity through a 9-axis MEMS sensor, and finds 3-axis magnetic vectors. The euler angles of the space can be obtained by using the angular velocity integration. The characteristics of the MEMS gyroscope are: the precision is higher in a short period, and the attitude angle can be obtained through integration; while long-term integration may lead to divergence due to zero drift. Therefore, the accelerometer and the magnetometer are simultaneously utilized to correct the roll attitude angle, the pitch attitude angle and the heading attitude angle, and the problem of integral divergence can be avoided. Meanwhile, the indoor magnetic anomaly changes slowly, and the speed of people walking indoors is not too fast. Based on the principle, an algorithm for correcting the magnetic anomaly on line is provided, and the indoor magnetic anomaly can be monitored in real time; and after the abnormality is found, compensation processing is carried out, and the processed magnetic vector can still carry out course correction on the MEMS sensor. Therefore, the problem of course divergence caused by indoor magnetic anomaly is avoided.
A real-time correction and navigation method for indoor environmental magnetic anomaly comprises the following steps, as shown in FIG. 1:
the method comprises the following steps of (1) enabling an MEMS sensor to be static, determining an initial course by utilizing magnetic output Mb (0), and determining initial rolling and pitching by utilizing accelerometer output, wherein the Mb (0) is expressed as the projection of a geomagnetic vector on a body coordinate, namely the magnetic output value of the MEMS sensor at an initial moment;
step (2) of calculating a geomagnetic vector Me (0) ═ C (0) × Mb (0) at an initial time, and juxtaposing a magnetic abnormal amount Δ M ═ 0, wherein C (0) is expressed as a rotation matrix from a body coordinate system to a ground coordinate system at the initial time;
acquiring 9-axis sensor data, and calculating a magnetic vector Me (t) ═ C (t) × Mb (t) at the time t on the earth coordinate, wherein C (t) is a rotation matrix calculated by only using gyroscope data at the time t, and Mb (t) is a projection of a geomagnetic vector on the body coordinate, namely a magnetic output value of the MEMS sensor at the time t;
step (4), calculating a magnetic abnormal quantity Δ m (t) ═ Me (t) — Me (0) at time t, and storing the magnetic abnormal quantity in a cache;
step (5) calculating the average value of the magnetic anomalies in N periods
Figure BDA0002625078560000061
If tmp is greater than the set threshold, updating the magnetic anomaly delta Mt＝tmp；
Step (6) of correcting the geomagnetic vector at time t, MbCorrection(t)＝Mb(t)-CT(t) Δ M, wherein CT(t) denotes C (t) the transposed matrix at time t;
step (7) of using the corrected magnetic vector MbCorrection(t) recalculating rotation matrix C by combining acceleration output and gyro outputNew(t)；
And (8) jumping to the step (3) to perform arithmetic operation again at the time of t + 1.
The invention also provides a device for correcting and navigating the magnetic anomaly of the indoor environment in real time, as shown in fig. 3, the device comprises:
a magnetic abnormal amount determination unit for determining a magnetic abnormal amount Δ m (t) at time t;
a geomagnetic vector correction amount determination unit configured to correct a geomagnetic vector according to the magnetic abnormal amount Δ m (t) at the time t to obtain a geomagnetic vector correction amount Mb (t) at the time t;
a rotation matrix c (t) determining unit configured to determine a rotation matrix c (t) by using the geomagnetic vector correction amount Mb at the time t to correct (t);
and the heading determining unit is used for determining the heading according to the rotation matrix C (t).
The algorithm comprises a correction method for human body posture strapdown calculation, and the method comprises the following steps:
step g1, initializing quaternion q and initializing gyro deviation δ ═ δx δy δz]T＝[0 0 0]TWherein, deltaxGyro deviation, δ, expressed as the x-axisyGyro deviation, delta, expressed as y-axiszGyro bias, expressed as the z-axis;
step g2, calculating the correction rate of xyz triaxial, w ═ w0+ δ wherein w0＝[wx wy wz]TExpressed as three-axis angular rate, wx expressed as x-axis angular rate, wy expressed as y-axis angular rate, and wz expressed as z-axis angular rate;
step g3, calculating the corrected
Figure BDA0002625078560000071
Figure BDA0002625078560000072
Wherein q is [ q ]0q1 q2 q3]T。
The algorithm also comprises a correction method for an accelerometer in the MEMS sensor, and the method comprises the following steps:
step e1, judging whether the acceleration is abnormal, and judging whether the acceleration is abnormal when the module value sqrt (ax) of the triaxial accelerometer2+ay2+az2) When the difference between the acceleration value and 1g is larger, the accelerometer correction is not carried out, wherein 1g represents a gravity acceleration;
step e2, acceleration normalization, a ═ axayaz]T，ANewA/norm (a), where ax is represented as acceleration of x-axis, ay is represented as acceleration of y-axis, az is represented as acceleration of z-axis, and norm is represented as matrix norm;
step e3, calculating the acceleration deviation Ea ═ ANewX V, wherein V ═ CT[001]TRepresented as a projection of the gravitational acceleration in the body coordinate system.
The algorithm also comprises a correction method for the magnetometer in the MEMS sensor, and the method comprises the following steps:
step f1, magnetometer normalization: m ═ mxmymz]T，MNewM/norm (M), whichIn (1), mx represents the magnetometer of the x axis, my represents the magnetometer of the y axis, and mz represents the magnetometer of the z axis;
step f2, calculating the magnetic field deviation Em ═ MNewX W, wherein W ═ CTMe (t) is expressed as the projection of the magnetometer in the body coordinate system.
The algorithm comprises nonlinear complementary correction of attitude quaternion, deltaCorrection＝Kp*e+KIEdt, where e ═ Em + Ea, Kp represents the P term coefficient in PID control, i.e. the proportional term coefficient, KIExpressed as an I term coefficient in PID control, namely an integral term coefficient; meaning that the gyro zero point is corrected by means of feedback control according to the deviation of the accelerometer and the magnetometer.
The gyroscope measures angular velocity, which has a high dynamic characteristic, that is, it measures the derivative of the angle, that is, the angular velocity, and the angular velocity is integrated with respect to time to obtain the angle. Modern MEMS sensors typically measure angular velocity in 3 axes simultaneously; the MEMS gyroscope has good dynamic response characteristics (wide bandwidth), but an integral algorithm is adopted when the attitude is calculated, so that accumulated errors are generated. The magnetic sensor and the accelerometer measure the attitude without accumulated errors, but have poor dynamic response (narrow bandwidth) and relatively large noise. In consideration of the characteristic complementation of the gyroscope, the magnetic sensor and the accelerometer on the frequency domain, the data of the three sensors can be fused in a complementary filtering mode, and the attitude measurement precision is improved. The magnetic sensor is easily interfered by the outside, and the magnetic field environment in the room is worse due to the influence of the outside magnetic interference.
The accelerometer has good low-frequency characteristics and can measure low-speed static acceleration. When the accelerometer is held by hands to rotate freely, the component values of the gravity acceleration on three axes are observed. When the accelerometer is in free fall, the output of the accelerometer is 0, the accelerometer only measures the gravity acceleration, and the 3-axis accelerometer outputs the component size of the gravity acceleration on 3 axes of a body coordinate system where the accelerometer is located. The direction and magnitude of the gravitational acceleration are fixed. Through the relation, the angle relation between the plane of the accelerometer and the ground can be obtained. If the accelerometer is rotating about the axis of gravitational acceleration, the measurements do not change, i.e. the accelerometer cannot sense this horizontal rotation.
The magnetic sensor can measure the magnetic field, and in the absence of other magnetic fields, only the magnetic field of the earth is measured, and the geomagnetism is also fixedly connected with the R coordinate system, and through the relationship, the relationship between the plane A and the ground plane can be obtained. (plane a: a plane perpendicular to the direction of the magnetic field), similarly, if the axis along the direction of the magnetic field is rotated, the measured value does not change, and the rotation cannot be sensed.
In comprehensive consideration, the accelerometer and the magnetic sensor are sensors which are extremely susceptible to external interference, and can only obtain a 2-dimensional angular relationship, but the change of a measured value with time is relatively small, and a 3-dimensional angular relationship can be obtained by combining the accelerometer and the magnetic sensor. The gyroscope can integrate to obtain a three-dimensional angle relation, has good dynamic performance and small external interference, but the change of a measured value is large along with time. It can be seen that the advantages and disadvantages are complementary and can be combined together to achieve good effect.
Quaternion q ═ q0, q1, q2, q3]Is a four-dimensional vector, wherein [ q1, q2, q 3)]It is understood that one axis of rotation of space, q0, is related to the angle θ of rotation about this axis.
Figure BDA0002625078560000091
The quaternions referred to in this patent are all unit quaternions, i.e., | | q | | ═ 1
When the unit quaternion represents the rotation, if the unit rotation axis is defined as (x, y, z) and the rotation angle is θ, the quaternion is
Figure BDA0002625078560000092
As shown in fig. 2, [0,1,0,0] is a vector pointing to the x-axis, [0,0,1,0] is a vector pointing to the y-axis, [0,0,0,1] is a vector pointing to the z-axis, and [0.5,0.5,0.5,0.5] is a vector obtained by rotating OP about the vector by 30 degrees.
When quaternions [ q0, q1, q2, q3] indicate relative rotation, q1, q2, q3 correspond to x, y, z axes, respectively, and q0 corresponds to the angle of rotation. Therefore, when the human body rotates an angle in the horizontal plane, it is equivalent to rotating around the z-axis, so that the corresponding quaternion q1 and q2 components are zero, and q0 represents the rotating angle, i.e. the quaternion should be [ q0,0,0, q3 ]. Similarly, when rotating around the x-axis, the corresponding quaternion is [ q0, q1,0,0 ]; when rotating around the y-axis, the corresponding quaternion is [ q0,0, q1, q2 ].
Quaternion can also represent the attitude angle of the rigid body, if an euler rotation (x, y, z) is given, where x, y, z are roll, pitch, and heading angles, respectively, then the corresponding quaternion is:
q0＝cos(x/2)*cos(y/2)cos(z/2)-sin(x/2)sin(y/2)*sin(z/2)
q1＝cos(x/2)*sin(y/2)*sin(z/2)+sin(x/2)*cos(y/2)*cos(z/2)
q2＝cos(x/2)*sin(y/2)*cos(z/2)+sin(x/2)*cos(y/2)*sin(z/2)
q3＝cos(x/2)*cos(y/2)*sin(z/2)-sin(x/2)*sin*y/2)*cos(z/2)。
although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (7)

1. A real-time correction and navigation method for indoor environmental magnetic anomaly is characterized by comprising the following steps:
determining a magnetic abnormal quantity Δ m (t) at time t;
correcting the geomagnetic vector according to the magnetic abnormal quantity delta m (t) at the time t to obtain a geomagnetic vector correction quantity Mb at the time tCorrection(t)；
Using the geomagnetic vector correction amount Mb at the time tCorrection(t) determining a rotation matrix c (t);
determining a heading according to the rotation matrix C (t);
the method also comprises a correction method for the accelerometer in the MEMS sensor, and the correction method for the accelerometer in the MEMS sensor comprises the following steps:
step (ii) ofe1, judging whether the acceleration is abnormal, and judging whether the acceleration is abnormal when the module value sqrt (ax) of the triaxial accelerometer2+ay2+az2) When the difference between the acceleration value and 1g is larger, the accelerometer correction is not carried out, wherein 1g represents a gravity acceleration;
step e2, acceleration normalization, a ═ ax ay az]T，ANewA/norm (a), where ax is the acceleration of the x-axis, ay is the acceleration of the y-axis, az is the acceleration of the z-axis, and norm is the matrix norm;
step e3, calculating the acceleration deviation Ea ═ ANewX V, wherein V ═ CT[0 0 1]TExpressed as the projection of the gravitational acceleration on a body coordinate system;
the method also comprises a correction method for the magnetometer in the MEMS sensor, and the correction method for the magnetometer in the MEMS sensor comprises the following steps:
step f1, magnetometer normalization: m ═ mxmy mz]T，MNewM/norm (M), where mx denotes a magnetometer of x axis, my denotes a magnetometer of y axis, and mz denotes a magnetometer of z axis;
step f2, calculating the magnetic field deviation Em ═ MNewX W, wherein W ═ CTMe (t) is expressed as the projection of the magnetometer in the body coordinate system;
the method comprises a correction method for human body posture strapdown calculation, and the correction method for the human body posture strapdown calculation comprises the following steps:
step g1, initializing quaternion q and initializing gyro deviation δ ═ δx δy δz]T＝[0 0 0]TWherein, deltaxGyro deviation, δ, expressed as the x-axisyGyro deviation, delta, expressed as y-axiszGyro deviation, expressed as the z-axis;
step g2, calculating correction rate of three axes of xyz, w ═ w0+ δ wherein w0＝[wx wy wz]TExpressed as three-axis angular rate, wx expressed as x-axis angular rate, wy expressed as y-axis angular rate, and wz expressed as z-axis angular rate;
step g3, calculating the corrected
Figure FDA0003519210370000021
Figure FDA0003519210370000022
Wherein q is [ q ]0 q1q2 q3]T。
2. The indoor environment magnetic anomaly real-time correction and navigation method according to claim 1, characterized by specifically comprising the steps of:
step a, the MEMS sensor is static, the initial course is determined by using magnetic output Mb (0), the initial roll and pitch are determined by using accelerometer output, wherein Mb (0) is expressed as the projection of a geomagnetic vector on a body coordinate, namely the magnetic output value of the MEMS sensor at the initial moment;
b, calculating a magnetic abnormal quantity delta m (t) at the time t by using a basic quaternion integral algorithm;
step c, correcting the geomagnetic vector at the time t to obtain a corrected geomagnetic vector MbCorrection，MbCorrection(t)＝Mb(t)-CT(t)ΔMtWhere C (t) is a rotation matrix calculated at time t using only gyro data, CT(t) denotes C (t) a transposed matrix at time t, Mb (t) denotes the projection of the geomagnetic vector on the volume coordinate, i.e. the magnetic output value of the MEMS sensor at time t, Δ MtExpressed as the average of N periods of magnetic anomalies greater than a set threshold;
step d of using the corrected geomagnetic vector MbCorrection(t) recalculating rotation matrix C by combining acceleration output and gyro outputNew(t), determining a heading.
3. The indoor environment magnetic anomaly real-time correction and navigation method according to claim 2, wherein the calculation of the magnetic anomaly quantity Δ m (t) at the time t in the step b comprises the following steps:
step b1, calculating a geomagnetic vector Me (0) ═ C (0) × Mb (0) at an initial time, and juxtaposing a magnetic abnormal amount Δ M ═ 0, wherein C (0) represents a rotation matrix from a body coordinate system to a geodetic coordinate system at the initial time, and the rotation matrix C (0) from the body coordinate system to the geodetic coordinate system at the initial time is obtained by quaternion;
b2, acquiring 9-axis sensor data, and calculating a magnetic vector Me (t) ═ C (t) × Mb (t) at the time t on the earth coordinate, wherein C (t) is a rotation matrix calculated by only using gyro data at the time t, and Mb (t) is a projection of the geomagnetic vector on the body coordinate, namely a magnetic output value of the MEMS sensor at the time t;
and b3, calculating the magnetic abnormal quantity Δ m (t) ═ Me (t) — Me (0) at the time t, and storing the magnetic abnormal quantity in a buffer.
4. The indoor environment magnetic anomaly real-time correction and navigation method according to claim 2, wherein the average value of the N periodic magnetic anomalies
Figure FDA0003519210370000031
If tmp is greater than the set threshold, updating the magnetic anomaly delta Mt＝tmp。
5. The indoor environment magnetic anomaly real-time correction and navigation method according to claim 3, characterized in that: the 9-axis sensor data comprises angular velocities, accelerations and magnetic vectors of three xyz axes under a body coordinate system obtained by the MEMS sensor, wherein the angular velocities of the three xyz axes are called gyro data;
the angular velocity data is obtained through a gyroscope, the acceleration data is obtained through an accelerometer, the magnetic vector data is obtained through a magnetometer, and the magnetometer is a magnetic sensor.
6. The indoor environment magnetic anomaly real-time correction and navigation method according to claim 1, characterized in that: the method comprises nonlinear complementary correction of attitude quaternion, deltaCorrection＝Kp*e+KIJjjj edt where e ═ Em + Ea, Kp denotes the P term coefficient in PID control, i.e. proportional term coefficient, KIExpressed as I term coefficients in PID control, i.e., integral term coefficients.
7. The utility model provides a real-time correction of indoor environment magnetic anomaly and navigation head, its characterized in that the device includes:
a magnetic abnormal amount determination unit for determining a magnetic abnormal amount Δ m (t) at time t;
a geomagnetic vector correction amount determination unit for correcting the geomagnetic vector according to the magnetic abnormal amount Δ m (t) at the time t to obtain a geomagnetic vector correction amount Mb at the time tCorrection(t)；
A rotation matrix C (t) determining unit for utilizing the geomagnetic vector correction quantity Mb at the time tCorrection(t) determining a rotation matrix c (t);
the course determining unit is used for determining the course according to the rotation matrix C (t);
the device is also used for correcting an accelerometer in the MEMS sensor, and the correction of the accelerometer in the MEMS sensor comprises the following steps:
step e1, judging whether the acceleration is abnormal, and judging whether the acceleration is abnormal when the module value sqrt (ax) of the triaxial accelerometer2+ay2+az2) When the difference between the acceleration value and 1g is larger, the accelerometer correction is not carried out, wherein 1g represents a gravity acceleration;
step e2, acceleration normalization, a ═ ax ay az]T，ANewA/norm (a), where ax is the acceleration of the x-axis, ay is the acceleration of the y-axis, az is the acceleration of the z-axis, and norm is the matrix norm;
step e3, calculating the acceleration deviation Ea ═ ANewX V, wherein V ═ CT[0 0 1]TExpressed as the projection of the gravitational acceleration in a body coordinate system;
the device is also used for correcting the magnetometer in the MEMS sensor, and the correcting of the magnetometer in the MEMS sensor comprises the following steps:
step f1, magnetometer normalization: m ═ mxmy mz]T，MNewM/norm (M), where mx denotes a magnetometer of x axis, my denotes a magnetometer of y axis, and mz denotes a magnetometer of z axis;
step f2, calculating the magnetic field deviation Em ═ MNewX W, wherein W ═ WCTMe (t) is expressed as the projection of the magnetometer in the body coordinate system;
the device is also used for correcting the human body posture strap-down calculation, and the correction of the human body posture strap-down calculation comprises the following steps:
step g1, initializing quaternion q and initializing gyro deviation δ ═ δx δy δz]T＝[0 0 0]TWherein, deltaxGyro deviation, δ, expressed as the x-axisyGyro deviation, delta, expressed as y-axiszGyro deviation, expressed as the z-axis;
step g2, calculating correction rate of three axes of xyz, w ═ w0+ δ, wherein w0 ═ wx wy wz]TExpressed as three-axis angular rate, wx expressed as x-axis angular rate, wy expressed as y-axis angular rate, and wz expressed as z-axis angular rate;
step g3, calculating the corrected
Figure FDA0003519210370000051
Figure FDA0003519210370000052
Wherein q is [ q ]0 q1q2 q3]T。
CN202010794600.5A 2020-08-10 2020-08-10 Indoor environment magnetic anomaly real-time correction and navigation method and device Active CN112033405B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010794600.5A CN112033405B (en) 2020-08-10 2020-08-10 Indoor environment magnetic anomaly real-time correction and navigation method and device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010794600.5A CN112033405B (en) 2020-08-10 2020-08-10 Indoor environment magnetic anomaly real-time correction and navigation method and device

Publications (2)

Publication Number Publication Date
CN112033405A CN112033405A (en) 2020-12-04
CN112033405B true CN112033405B (en) 2022-06-17

Family

ID=73577729

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010794600.5A Active CN112033405B (en) 2020-08-10 2020-08-10 Indoor environment magnetic anomaly real-time correction and navigation method and device

Country Status (1)

Country Link
CN (1) CN112033405B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113267182B (en) * 2021-04-28 2024-04-30 广东国天时空科技有限公司 Correction method based on geomagnetic measurement value and geomagnetic detection device
CN113865571A (en) * 2021-08-20 2021-12-31 无锡宇宁智能科技有限公司 Method and device for improving application precision of mobile phone compass and readable storage medium

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103630139B (en) * 2013-12-17 2015-12-02 哈尔滨工程大学 A kind of full attitude determination method of underwater carrier measured based on earth magnetism gradient tensor
US9857179B2 (en) * 2014-12-30 2018-01-02 Northrop Grumman Systems Corporation Magnetic anomaly tracking for an inertial navigation system
CN106017459B (en) * 2016-05-19 2019-03-19 极翼机器人(上海)有限公司 A kind of method of diamagnetic interference
CN107655476B (en) * 2017-08-21 2021-04-20 南京航空航天大学 Pedestrian high-precision foot navigation method based on multi-information fusion compensation
CN107860382B (en) * 2017-11-07 2021-04-06 吉林大学 Method for measuring attitude by applying AHRS under geomagnetic anomaly condition
CN108731676B (en) * 2018-05-04 2021-06-15 北京摩高科技有限公司 Attitude fusion enhanced measurement method and system based on inertial navigation technology
CN109000639B (en) * 2018-06-05 2022-01-07 哈尔滨工程大学 Attitude estimation method and device of multiplicative error quaternion geomagnetic tensor field auxiliary gyroscope
CN108534772B (en) * 2018-06-24 2021-07-02 西宁泰里霍利智能科技有限公司 Attitude angle acquisition method and device
CN110398702B (en) * 2019-06-19 2021-07-23 北京摩高科技有限公司 Real-time online magnetic calibration method based on multi-sensor fusion
CN111141283A (en) * 2020-01-19 2020-05-12 杭州十域科技有限公司 Method for judging advancing direction through geomagnetic data

Also Published As

Publication number Publication date
CN112033405A (en) 2020-12-04

Similar Documents

Publication Publication Date Title
US10747330B2 (en) Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus
CN108731676B (en) Attitude fusion enhanced measurement method and system based on inertial navigation technology
Li et al. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems
CN107560613B (en) Robot indoor track tracking system and method based on nine-axis inertial sensor
US7844415B1 (en) Dynamic motion compensation for orientation instrumentation
CN110887481B (en) Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN112033405B (en) Indoor environment magnetic anomaly real-time correction and navigation method and device
CN106370178B (en) Attitude measurement method and device of mobile terminal equipment
CN112611380B (en) Attitude detection method based on multi-IMU fusion and attitude detection device thereof
CN107402007A (en) A kind of method for improving miniature AHRS modules precision and miniature AHRS modules
CN115046539A (en) Dynamic calibration method for MEMS electronic compass
CN115540860A (en) Multi-sensor fusion pose estimation algorithm
CN106595669B (en) Method for resolving attitude of rotating body
CN110207647B (en) Arm ring attitude angle calculation method based on complementary Kalman filter
US20170343355A1 (en) Method And System For Estimating Relative Angle Between Headings
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN110030991B (en) High-speed rotation angle movement measuring method for flyer integrating gyroscope and magnetometer
CN116182871A (en) Sea cable detection robot attitude estimation method based on second-order hybrid filtering
CN113936044B (en) Method and device for detecting motion state of laser equipment, computer equipment and medium
CN115523919A (en) Nine-axis attitude calculation method based on gyro drift optimization
CN115326062A (en) GNSS INS (Global navigation satellite System for inertial navigation System) integrated navigation method, device and medium based on bionic flapping-wing flying robot
Zhang et al. Implementation and complexity analysis of orientation estimation algorithms for human body motion tracking using low-cost sensors
Xuan Application of real-time Kalman filter with magnetic calibration for MEMS sensor in attitude estimation
CN108507571B (en) IMU attitude capturing method and system under high-speed kinematics

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