CN110887480A - Flight attitude estimation method and system based on MEMS sensor - Google Patents

Flight attitude estimation method and system based on MEMS sensor Download PDF

Info

Publication number
CN110887480A
CN110887480A CN201911262990.5A CN201911262990A CN110887480A CN 110887480 A CN110887480 A CN 110887480A CN 201911262990 A CN201911262990 A CN 201911262990A CN 110887480 A CN110887480 A CN 110887480A
Authority
CN
China
Prior art keywords
initial
formula
follows
gyroscope
quaternion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911262990.5A
Other languages
Chinese (zh)
Other versions
CN110887480B (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.)
Low Speed Aerodynamics Institute of China Aerodynamics Research and Development Center
Original Assignee
North Wuxi Micro Sensing Science And Technology Ltd
Low Speed Aerodynamics Institute of China Aerodynamics Research and Development Center
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 North Wuxi Micro Sensing Science And Technology Ltd, Low Speed Aerodynamics Institute of China Aerodynamics Research and Development Center filed Critical North Wuxi Micro Sensing Science And Technology Ltd
Priority to CN201911262990.5A priority Critical patent/CN110887480B/en
Publication of CN110887480A publication Critical patent/CN110887480A/en
Application granted granted Critical
Publication of CN110887480B publication Critical patent/CN110887480B/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/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/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/20—Instruments for performing navigational calculations

Landscapes

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

Abstract

The invention discloses a flight attitude estimation method and a system based on an MEMS sensor, wherein the method comprises the following steps: collecting data of an accelerometer and a magnetometer, and determining a carrier coordinate system; converting the carrier coordinate system into a ground coordinate system, and solving an error; using the data output by the error correction gyroscope; updating quaternion by using the corrected gyroscope data to obtain a new attitude angle; and performing Kalman filtering by using the new attitude angle to obtain a flight attitude estimation result. According to the method and the system for estimating the flight attitude based on the MEMS sensor, provided by the invention, the carrier coordinate system is calculated through the data of the accelerometer and the magnetometer, then the error is solved, and the output of the gyroscope is corrected by using the error, so that the output of the gyroscope is more accurate, the problem of accumulated drift error generated by the gyroscope is effectively solved, and the accuracy of the output result of the Kalman filter is increased; and quaternion is used for participating in calculation, so that the overall calculation amount is reduced, and the calculation cost is reduced.

Description

Flight attitude estimation method and system based on MEMS sensor
Technical Field
The application relates to the technical field of aircraft attitude estimation, in particular to a flight attitude estimation method and system based on an MEMS sensor.
Background
The flight attitude refers to the state of the three axes of the aircraft in the air relative to a certain reference line or a certain reference plane or a certain fixed coordinate system. The aircraft has various flight attitudes during flying in the air, and the flight attitudes determine the moving direction of the aircraft, thereby influencing the flying height and the flying direction. The estimation of the flight attitude plays a crucial role in the flight process of the aircraft, and the flight attitude can be estimated to ensure that the flight course of the aircraft is correct and the flight safety is ensured.
The MEMS sensor is a core device of an Attitude and Heading Reference System (AHRS) and a Micro inertial navigation system (Micro-INS), the traditional method for estimating the flight attitude based on the MEMS sensor is to calculate and estimate the data of an accelerometer and a gyroscope in the MEMS sensor and carry out Kalman filtering iteration, and the method generally has the following problems that (1) the gyroscope in the MEMS sensor generates accumulated errors after long-time movement, which means that the device characteristics of the gyroscope cannot be eliminated, drift caused by the accumulated errors influences the output angle value and influences the accuracy of a Kalman filter system equation; (2) the method is generally applicable to steady-state motion scenes, including a static mode and a constant rotating speed motion mode, and cannot be applied to actual scenes. Under the complex motion mode, the error caused by the accumulated drift of the gyroscope is more obvious, and the output result is not accurate.
Disclosure of Invention
The present invention is directed to a method and system for estimating a flight attitude based on a MEMS sensor, which may solve one or more of the above-mentioned problems of the prior art.
According to one aspect of the present invention, a method for estimating a flight attitude based on a MEMS sensor is provided, comprising the steps of:
collecting data output by an accelerometer and a magnetometer, and determining a carrier coordinate system;
converting the carrier coordinate system into a ground coordinate system, and solving an error according to the corresponding gravity and the vector of the earth magnetic field;
using the data output by the error correction gyroscope;
updating quaternion by using the corrected data output by the gyroscope to obtain a new attitude angle;
and performing Kalman filtering by using the new attitude angle to obtain a flight attitude estimation result.
The beneficial effects are as follows: according to the invention, a carrier coordinate system is calculated through data of the accelerometer and the magnetometer, an error is calculated according to a vector of corresponding gravity and an earth magnetic field, and the error is used for correcting the output of the gyroscope, so that the output of the gyroscope is more accurate, the problem of accumulated drift error generated by the gyroscope is effectively solved, and the accuracy of the output result of the Kalman filter is increased; quaternion is used for participating in calculation, so that the overall calculation amount is reduced, the calculation cost is reduced, and the calculation speed is increased; by solving the problem of accumulated drift error generated by a gyroscope, the method provided by the invention can be applied to complex motion modes in actual motion scenes.
In some embodiments, collecting data output by the accelerometer and the magnetometer, and determining the carrier coordinate system comprises:
according to the data output by the accelerometer, determining an initial roll angle phi and an initial pitch angle theta, wherein the formula is as follows:
Figure BDA0002312076340000021
in the formula，ax、ayAnd azInitial data output for the accelerometer;
the transformation matrix between the carrier coordinate system and the bottom surface coordinate system is T, and when ψ is 0, the formula of the transformation matrix T is as follows:
Figure BDA0002312076340000022
can be obtained by operation
Figure BDA0002312076340000023
In the formula, phi is an initial roll angle, and theta is an initial pitch angle;
the initial data output by the magnetometer is compensated by the initial data output by the accelerometer, namely the compensated magnetometer data is obtained according to the initial roll angle phi, the initial pitch angle theta and the initial data output by the magnetometer, and the calculation formula is as follows:
Figure BDA0002312076340000024
in the formula, magxAnd magyOutput data for compensated magnetometer, mx、myAnd mzInitial data output for the magnetometer;
calculating an initial heading angle psi by using the compensated magnetometer output data, wherein the calculation formula is as follows:
Figure BDA0002312076340000025
the initial attitude angles were obtained as follows:
Figure BDA0002312076340000026
where φ is the initial roll angle, θ is the initial pitch angle, and ψ is the initial heading angle.
The beneficial effects are as follows: and the initial data output by the magnetometer is compensated by the initial data output by the accelerometer, so that the accuracy of the initial attitude angle is further improved.
In some embodiments, converting the carrier coordinate system into a ground coordinate system, and determining the error according to the gravity vector and the geomagnetic vector of the corresponding reference includes:
the corresponding initial quaternion is obtained from α, γ, and β, as follows:
Figure BDA0002312076340000031
and solving a conversion matrix T by using the initial quaternion, wherein the conversion matrix T is as follows:
Figure BDA0002312076340000032
the initial data output by the accelerometer is normalized, and the processing formula is as follows:
Figure BDA0002312076340000033
and calculating direction vectors of gravity and a magnetic field, wherein the formula is as follows:
Figure BDA0002312076340000034
using the treated ax′、ay' and az' calculation error from the direction vectors of gravity and magnetic field, formula:
Figure BDA0002312076340000035
in the formula, ex、eyAnd ezTo define the initial value of error, ax′、ay' and az' normalization of the processed accelerometer output data, vx、vyAnd vzThe direction vectors of gravity and the magnetic field.
In some embodiments, the data output using the error correction gyroscope uses the following equation:
Figure BDA0002312076340000036
in the formula, gx、gyAnd gzInitial angular velocity, K, of the gyroscope outputPTo scale factor, KiThe adjustment coefficients are integrated.
The beneficial effects are as follows: and the data output by the gyroscope is compensated, so that the measurement precision is improved.
In some embodiments, the scaling factor K isP1 and integral adjustment coefficient Ki＝0。
In some embodiments, performing a quaternion update using the corrected gyroscope output data, obtaining the new attitude angle comprises:
using corrected angular velocity gx′、gy' and gz' with initial quaternion q0、q1、q2And q is3Calculating a derivative of the initial quaternion;
Figure BDA0002312076340000041
in the formula (I), the compound is shown in the specification,
Figure BDA0002312076340000042
and
Figure BDA0002312076340000043
is the derivative of the initial quaternion;
and (3) integrating the derivative of the initial quaternion to obtain a new quaternion, wherein the formula is as follows:
Figure BDA0002312076340000044
in the formula, q0′、q1′、q2' and q3' is a new quaternion;
and (3) bringing a new quaternion into the conversion matrix T to obtain the conversion matrix T as follows:
Figure BDA0002312076340000045
calculating the attitude angle according to the formula
Figure BDA0002312076340000046
Where φ ' is the new roll angle, θ ' is the new pitch angle, and ψ ' is the new heading angle.
In some embodiments, using the new attitude angle for kalman filtering to obtain the flight attitude estimation result comprises:
the state equation is determined as follows:
Figure BDA0002312076340000051
in the formula, XkThe state vector at the moment k, q is a discrete time model obtained by using a Runge-Kutta method, t is sampling time, M is an operation matrix, and the calculation formula of M is as follows:
Figure BDA0002312076340000052
wherein, gx′、gy' and gz' is the corrected gyroscope output, t is the sampling time;
the discrete-time model q is as follows:
Figure BDA0002312076340000053
in the formula, t is sampling time, Ω b represents a component of angular velocity on the navigation mark coordinate system, and can be obtained from output data corrected by a gyroscope, and the matrix type of Ω b is represented as the following formula:
Figure BDA0002312076340000054
in the formula, gx′、gy' and gz' is the corrected gyroscope output;
determining a measurement matrix H (k), wherein the measurement matrix H (k) has the following formula:
Figure BDA0002312076340000055
wherein g represents the acceleration of gravity, q0′、q1′、q2' and q3' denotes a new quaternion;
performing basic algorithm arrangement of Kalman filtering, wherein the algorithm flow is as follows:
the state update equation:
Figure BDA0002312076340000056
one-step prediction mean square error equation:
P(k,k-1)＝Φ(k,k-1)P(k,k-1)ΦT (k,k-1)+Q(k)
filter gain matrix equation:
K(k)＝P(k,k-1)HT (k)[H(k)P(k,k-1)HT (k)+R(k)]-1
the state estimation calculation equation:
Figure BDA0002312076340000057
estimating mean square error equation
P(k)＝[I-K(k)H(k)]K(k,K-1)[I-K(k)H(k)]T+K(k)R(k)KT (k)。
In a second aspect, an embodiment of the present invention provides a system for determining a flying attitude based on a MEMS sensor, including: the data output ends of the accelerometer, the magnetometer and the gyroscope are connected with the input end of the processor, and the data output end of the processor is connected with the input end of the filter.
In addition, in the technical scheme of the invention, the technical scheme can be realized by adopting the conventional means in the field unless being particularly described.
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 flowchart of a method for estimating a flying attitude based on a MEMS sensor according to an embodiment of the present application.
Fig. 2 is a schematic structural diagram of a system for estimating a flying attitude based on a MEMS sensor according to another embodiment of the present application.
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 derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Example 1:
fig. 1 shows a method for estimating a flying attitude based on a MEMS sensor according to an embodiment of the present invention, which includes the following steps:
s11: and collecting data output by the accelerometer and the magnetometer, and determining a carrier coordinate system.
In an alternative embodiment, collecting data output by the accelerometer and the magnetometer and determining the carrier coordinate system comprises:
according to the data output by the accelerometer, determining an initial roll angle phi and an initial pitch angle theta, wherein the formula is as follows:
Figure BDA0002312076340000061
in the formula, ax、ayAnd azInitial data output for the accelerometer;
the transformation matrix between the carrier coordinate system and the bottom surface coordinate system is T, and when ψ is 0, the formula of the transformation matrix T is as follows:
Figure BDA0002312076340000071
can be obtained by operation
Figure BDA0002312076340000072
In the formula, phi is an initial roll angle, and theta is an initial pitch angle;
according to the initial attitude angle information and magnetometer data, parameters required for acquiring course angle information can be obtained, and the formula is as follows:
Figure BDA0002312076340000073
in the formula, Xh、YhAnd ZhMeans that when the carrier coordinate system coincides with the ground coordinate, Xh、YhAnd ZhThe components of the earth magnetic field in the three-axis direction of the carrier coordinate system are shown;
the heading angle ψ can be calculated using the following formula:
Figure BDA0002312076340000074
according to Xh、YhCan obtain one of the heading angle psiThe general calculation formula is:
Figure BDA0002312076340000075
the initial data output by the magnetometer is compensated by the initial data output by the accelerometer, namely the compensated magnetometer data is obtained according to the initial roll angle phi, the initial pitch angle theta and the initial data output by the magnetometer, and the calculation formula is as follows:
Figure BDA0002312076340000076
in the formula, magxAnd magyOutput data for compensated magnetometer, mx、myAnd mzInitial data output for the magnetometer;
calculating an initial heading angle psi by using the compensated magnetometer output data, wherein the calculation formula is as follows:
Figure BDA0002312076340000077
the initial attitude angles were obtained as follows:
Figure BDA0002312076340000081
where φ is the initial roll angle, θ is the initial pitch angle, and ψ is the initial heading angle.
S12: and converting the carrier coordinate system into a ground coordinate system, and solving the error according to the corresponding gravity and the vector of the earth magnetic field.
In an alternative embodiment, the converting the carrier coordinate system into a ground coordinate system, and the calculating the error according to the gravity vector and the geomagnetic vector of the corresponding reference includes:
the corresponding initial quaternion is obtained from α, γ, and β, as follows:
Figure BDA0002312076340000082
and solving a conversion matrix T by using the initial quaternion, wherein the conversion matrix T is as follows:
Figure BDA0002312076340000083
the initial data output by the accelerometer is normalized, and the processing formula is as follows:
Figure BDA0002312076340000084
and calculating direction vectors of gravity and a magnetic field, wherein the formula is as follows:
Figure BDA0002312076340000085
using ax′、ay' and az' calculation error from the direction vectors of gravity and magnetic field, formula:
Figure BDA0002312076340000086
in the formula, ex、eyAnd ezTo define the initial value of error, ax′、ay' and az' normalization of the processed accelerometer output data, vx、vyAnd vzThe direction vectors of gravity and the magnetic field.
S13: the data output by the gyroscope is corrected using the error.
In an alternative embodiment, the data output using the error correction gyroscope uses the following equation:
Figure BDA0002312076340000091
in the formula, gx、gyAnd gzInitial angular velocity, K, of the gyroscope outputPTo scale factor, KiThe adjustment coefficients are integrated.
In the alternativeIn the examples, the scaling factor K P1 and integral adjustment coefficient Ki＝0。
S14: and updating the quaternion by using the corrected data output by the gyroscope to obtain a new attitude angle.
In an alternative embodiment, performing quaternion update using the corrected data output by the gyroscope, and obtaining a new attitude angle includes:
using corrected angular velocity gx′、gy' and gz' with initial quaternion q0、q1、q2And q is3Calculating a derivative of the initial quaternion;
Figure BDA0002312076340000092
in the formula (I), the compound is shown in the specification,
Figure BDA0002312076340000093
and
Figure BDA0002312076340000094
is the derivative of the initial quaternion;
and (3) integrating the derivative of the initial quaternion to obtain a new quaternion, wherein the formula is as follows:
Figure BDA0002312076340000095
in the formula, q0′、q1′、q2' and q3' is a new quaternion;
and (3) bringing a new quaternion into the conversion matrix T to obtain the conversion matrix T as follows:
Figure BDA0002312076340000096
Figure BDA0002312076340000101
and (3) calculating the attitude angle according to the following formula:
Figure BDA0002312076340000102
where φ ' is the new roll angle, θ ' is the new pitch angle, and ψ ' is the new heading angle.
S15: and performing Kalman filtering by using the new attitude angle to obtain a flight attitude estimation result.
The kalman filter is a highly efficient recursive filter that regenerates an estimate of an unknown variable based on the values of each measurement at different times, taking into account the joint distribution at each time, and is therefore more accurate than an estimation based on a single measurement.
In an alternative embodiment, the obtaining of the flight attitude estimation result by using the new attitude angle to perform kalman filtering includes:
the state equation is determined as follows:
Figure BDA0002312076340000103
in the formula, XkThe state vector at the moment k, q is a discrete time model obtained by using a Runge-Kutta method, t is sampling time, M is an operation matrix, and the calculation formula of M is as follows:
Figure BDA0002312076340000104
wherein, gx′、gy' and gz' is the corrected gyroscope output, t is the sampling time;
the discrete-time model q is as follows:
Figure BDA0002312076340000105
in the formula, t is sampling time, Ω b represents a component of angular velocity on the navigation mark coordinate system, and can be obtained from output data corrected by a gyroscope, and the matrix type of Ω b is represented as the following formula:
Figure BDA0002312076340000106
in the formula, gx′、gy' and gz' is the corrected gyroscope output;
determining a measurement matrix H (k), wherein the measurement matrix H (k) has the following formula:
Figure BDA0002312076340000111
wherein g represents the acceleration of gravity, q0′、q1′、q2' and q3' denotes a new quaternion;
performing basic algorithm arrangement of Kalman filtering, wherein the algorithm flow is as follows:
the state update equation:
Figure BDA0002312076340000112
one-step prediction mean square error equation:
P(k,k-1)＝Φ(k,k-1)P(k,k-1)ΦT (k,k-1)+Q(k)
filter gain matrix equation:
K(k)＝P(k,k-1)HT (k)[H(k)P(k,k-1)HT (k)+R(k)]-1
the state estimation calculation equation:
Figure BDA0002312076340000113
estimating mean square error equation
P(k)＝[I-K(k)H(k)]K(k,K-1)[I-K(k)H(k)]T+K(k)R(k)KT (k)。
According to the invention, a carrier coordinate system is calculated through data of the accelerometer and the magnetometer, an error is calculated according to a vector of corresponding gravity and an earth magnetic field, and the error is used for correcting the output of the gyroscope, so that the output of the gyroscope is more accurate, the problem of accumulated drift error generated by the gyroscope is effectively solved, and the accuracy of the output result of the Kalman filter is increased; quaternion is used for participating in calculation, so that the overall calculation amount is reduced, the calculation cost is reduced, and the calculation speed is increased; by solving the problem of accumulated drift error generated by a gyroscope, the method provided by the invention can be applied to complex motion modes in actual motion scenes.
Example 2:
fig. 2 shows a system for determining a flying attitude based on a MEMS sensor according to embodiment 2 of the present invention, which includes an accelerometer 1, a magnetometer 2, a gyroscope 3, a processor 4, and a filter 5, wherein data output terminals of the accelerometer 1, the magnetometer 2, and the gyroscope 3 are connected to an input terminal of the processor 4, and a data output terminal of the processor 4 is connected to an input terminal of the filter 5. The processor 4 is used for acquiring and processing the data measured by the accelerometer 1, the magnetometer 2 and the gyroscope 3, and sending the processed result to the filter 5 for estimating the flight attitude.
The system for determining a flying attitude based on an MEMS sensor according to embodiment 2 may be configured to execute the method for estimating a flying attitude based on an MEMS sensor according to embodiment 1 of the present invention, and accordingly achieve the technical effects achieved by the method for estimating a flying attitude based on an MEMS sensor according to embodiment 1 of the present invention, which are not described herein again.
The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and the parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.
Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware. With this understanding in mind, the above-described technical solutions may be embodied in the form of a software product, which can be stored in a computer-readable storage medium such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the methods described in the embodiments or some parts of the embodiments.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; 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 (8)

1. The flight attitude estimation method based on the MEMS sensor is characterized by comprising the following steps of:
collecting data output by an accelerometer and a magnetometer, and determining a carrier coordinate system;
converting the carrier coordinate system into a ground coordinate system, and solving an error according to the vector of the corresponding gravity and the earth magnetic field;
using the error correction gyroscope output data;
updating quaternion by using the corrected data output by the gyroscope to obtain a new attitude angle;
and performing Kalman filtering by using the new attitude angle to obtain a flight attitude estimation result.
2. The method of claim 1, wherein the collecting data from outputs of an accelerometer and a magnetometer and determining a carrier coordinate system comprises:
according to the data output by the accelerometer, determining an initial roll angle phi and an initial pitch angle theta, wherein the formula is as follows:
Figure FDA0002312076330000011
in the formula, ax、ayAnd azInitial data output for the accelerometer;
the transformation matrix between the carrier coordinate system and the bottom surface coordinate system is T, and when ψ is 0, the formula of the transformation matrix T is as follows:
Figure FDA0002312076330000012
can be obtained by operation
Figure FDA0002312076330000013
In the formula, phi is an initial roll angle, and theta is an initial pitch angle;
the initial data output by the magnetometer is compensated by the initial data output by the accelerometer, namely the compensated magnetometer data is obtained according to the initial roll angle phi, the initial pitch angle theta and the initial data output by the magnetometer, and the calculation formula is as follows:
Figure FDA0002312076330000014
in the formula, magxAnd magyOutput data for compensated magnetometer, mx、myAnd mzInitial data output for the magnetometer;
calculating an initial heading angle psi by using the compensated magnetometer output data, wherein the calculation formula is as follows:
Figure FDA0002312076330000015
the initial attitude angles were obtained as follows:
Figure FDA0002312076330000021
where φ is the initial roll angle, θ is the initial pitch angle, and ψ is the initial heading angle.
3. The method of claim 2, wherein the carrier coordinate system is converted into a ground coordinate system, and the error calculation based on the gravity vector and the geomagnetic vector of the corresponding reference comprises:
the corresponding initial quaternion is obtained from α, γ, and β, as follows:
Figure FDA0002312076330000022
and obtaining a conversion matrix T by using the initial quaternion, wherein the conversion matrix T is as follows:
Figure FDA0002312076330000023
the initial data output by the accelerometer is normalized, and the processing formula is as follows:
Figure FDA0002312076330000024
and calculating direction vectors of gravity and a magnetic field, wherein the formula is as follows:
Figure FDA0002312076330000025
using ax′、ay' and az' calculation error from the direction vectors of gravity and magnetic field, formula:
Figure FDA0002312076330000026
in the formula, ex、eyAnd ezTo define the initial value of error, ax′、ay' and az' normalization of the processed accelerometer output data, vx、vyAnd vzThe direction vectors of gravity and the magnetic field.
4. The MEMS sensor-based attitude estimation method of claim 3, wherein the data output using the error-corrected gyroscope uses the following equation:
Figure FDA0002312076330000031
in the formula, gx、gyAnd gzInitial angular velocity, K, of the gyroscope outputPTo scale factor, KiThe adjustment coefficients are integrated.
5. The MEMS-sensor based attitude estimation method of claim 4, wherein the scaling factor K isP1 and the integral adjustment coefficient Ki＝0。
6. The method of claim 4, wherein performing quaternion update using the corrected gyroscope output data to obtain new attitude angles comprises:
using corrected angular velocity gx′、gy' and gz' with initial quaternion q0、q1、q2And q is3Calculating a derivative of the initial quaternion;
Figure FDA0002312076330000032
in the formula (I), the compound is shown in the specification,
Figure FDA0002312076330000033
and
Figure FDA0002312076330000034
is the derivative of the initial quaternion;
and (3) performing integral calculation by using the derivative of the initial quaternion to obtain a new quaternion, wherein the formula is as follows:
Figure FDA0002312076330000035
in the formula, q0′、q1′、q2' and q3' is a new quaternion;
and (3) bringing a new quaternion into the conversion matrix T to obtain the conversion matrix T as follows:
Figure FDA0002312076330000041
calculating the attitude angle according to the formula
Figure FDA0002312076330000042
Where φ ' is the new roll angle, θ ' is the new pitch angle, and ψ ' is the new heading angle.
7. The method of claim 6, wherein the Kalman filtering with the new attitude angle to obtain the estimated result of the attitude comprises:
the state equation is determined as follows:
Figure FDA0002312076330000043
in the formula, XkIs the state vector at time k, q is the discrete time model obtained using the Runge-Kutta method, t isAnd (3) sample time, wherein M is an operation matrix, and the calculation formula of M is as follows:
Figure FDA0002312076330000044
wherein, gx′、gy' and gz' is the corrected gyroscope output, t is the sampling time;
the discrete-time model q is as follows:
Figure FDA0002312076330000045
in the formula, t is sampling time, Ω b represents a component of angular velocity on the navigation mark coordinate system, and can be obtained from output data corrected by a gyroscope, and the matrix type of Ω b is represented as the following formula:
Figure FDA0002312076330000046
in the formula, gx′、gy' and gz' is the corrected gyroscope output;
determining a measurement matrix H (k), wherein the measurement matrix H (k) has the following formula:
Figure FDA0002312076330000051
wherein g represents the acceleration of gravity, q0′、q1′、q2' and q3' denotes a new quaternion;
performing basic algorithm arrangement of Kalman filtering, wherein the algorithm flow is as follows:
the state update equation:
Figure FDA0002312076330000052
one-step prediction mean square error equation:
P(k,k-1)＝Φ(k,k-1)P(k,k-1)ΦT (k,k-1)+Q(k)
filter gain matrix equation:
K(k)＝P(k,k-1)HT (k)[H(k)P(k,k-1)HT (k)+R(k)]-1
the state estimation calculation equation:
Figure FDA0002312076330000053
estimating mean square error equation
P(k)＝[I-K(k)H(k)]K(k,K-1)[I-K(k)H(k)]T+K(k)R(k)KT (k)。
8. The flying attitude estimation system based on the MEMS sensor is characterized by comprising an accelerometer (1), a magnetometer (2), a gyroscope (3), a processor (4) and a filter (5),
the data output ends of the accelerometer (1), the magnetometer (2) and the gyroscope (3) are connected with the input end of the processor (4),
and the data output end of the processor (4) is connected with the input end of the filter (5).
CN201911262990.5A 2019-12-11 2019-12-11 Flight attitude estimation method and system based on MEMS sensor Active CN110887480B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911262990.5A CN110887480B (en) 2019-12-11 2019-12-11 Flight attitude estimation method and system based on MEMS sensor

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911262990.5A CN110887480B (en) 2019-12-11 2019-12-11 Flight attitude estimation method and system based on MEMS sensor

Publications (2)

Publication Number Publication Date
CN110887480A true CN110887480A (en) 2020-03-17
CN110887480B CN110887480B (en) 2020-06-30

Family

ID=69751440

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911262990.5A Active CN110887480B (en) 2019-12-11 2019-12-11 Flight attitude estimation method and system based on MEMS sensor

Country Status (1)

Country Link
CN (1) CN110887480B (en)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112630813A (en) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system
CN113124819A (en) * 2021-06-17 2021-07-16 中国空气动力研究与发展中心低速空气动力研究所 Monocular distance measuring method based on plane mirror
CN113155129A (en) * 2021-04-02 2021-07-23 北京大学 Holder attitude estimation method based on extended Kalman filtering
CN113865571A (en) * 2021-08-20 2021-12-31 无锡宇宁智能科技有限公司 Method and device for improving application precision of mobile phone compass and readable storage medium
CN114526756A (en) * 2022-01-04 2022-05-24 华南理工大学 Unmanned aerial vehicle airborne multi-sensor correction method and device and storage medium
CN115855038A (en) * 2022-11-22 2023-03-28 哈尔滨工程大学 Short-time high-precision attitude keeping method
CN116182839A (en) * 2023-04-27 2023-05-30 北京李龚导航科技有限公司 Method and device for determining attitude of aircraft, electronic equipment and storage medium
CN117272593A (en) * 2023-08-24 2023-12-22 无锡北微传感科技有限公司 Wind tunnel test data analysis processing method
CN112630813B (en) * 2020-11-24 2024-05-03 中国人民解放军国防科技大学 Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101290229A (en) * 2008-06-13 2008-10-22 哈尔滨工程大学 Silicon micro-navigation attitude system inertia/geomagnetism assembled method
CN106342284B (en) * 2008-08-18 2011-11-23 西北工业大学 A kind of flight carrier attitude is determined method
CN102980577A (en) * 2012-12-05 2013-03-20 南京理工大学 Micro-strapdown altitude heading reference system and working method thereof
CN104880190A (en) * 2015-06-02 2015-09-02 无锡北微传感科技有限公司 Intelligent chip for accelerating inertial navigation attitude fusion
CN105203098A (en) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor
US20180058857A1 (en) * 2016-08-31 2018-03-01 Yost Labs Inc. Local perturbation rejection using time shifting
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device
CN110146077A (en) * 2019-06-21 2019-08-20 台州知通科技有限公司 Pose of mobile robot angle calculation method
CN110207696A (en) * 2019-06-06 2019-09-06 南京理工大学 A kind of nine axis movement sensor attitude measurement methods based on quasi-Newton method

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101290229A (en) * 2008-06-13 2008-10-22 哈尔滨工程大学 Silicon micro-navigation attitude system inertia/geomagnetism assembled method
CN106342284B (en) * 2008-08-18 2011-11-23 西北工业大学 A kind of flight carrier attitude is determined method
CN102980577A (en) * 2012-12-05 2013-03-20 南京理工大学 Micro-strapdown altitude heading reference system and working method thereof
CN104880190A (en) * 2015-06-02 2015-09-02 无锡北微传感科技有限公司 Intelligent chip for accelerating inertial navigation attitude fusion
CN105203098A (en) * 2015-10-13 2015-12-30 上海华测导航技术股份有限公司 Whole attitude angle updating method applied to agricultural machinery and based on nine-axis MEMS (micro-electromechanical system) sensor
US20180058857A1 (en) * 2016-08-31 2018-03-01 Yost Labs Inc. Local perturbation rejection using time shifting
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device
CN110207696A (en) * 2019-06-06 2019-09-06 南京理工大学 A kind of nine axis movement sensor attitude measurement methods based on quasi-Newton method
CN110146077A (en) * 2019-06-21 2019-08-20 台州知通科技有限公司 Pose of mobile robot angle calculation method

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
SHOAIB MANSOOR, UMAR IQBAL BHATTI, AAMIR IQBAL BHATTI, SYED MUHA: "Improved attitude determination by compensation of gyroscopic drift by use of accelerometers and magnetomete", 《MEASUREMENT》 *
YONGJIE Z, HAIYUN Z, TAO Z, ET AL: "A fusion attitude determination method based on quaternion for MEMS gyro/accelerometer/magnetometer", 《CONTROL & DECISION CONFERENCE》 *
宋晋等: "基于MEMS传感器的风洞尾旋姿态测量研究", 《计算机测量与控制》 *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112630813A (en) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system
CN112630813B (en) * 2020-11-24 2024-05-03 中国人民解放军国防科技大学 Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system
CN113155129A (en) * 2021-04-02 2021-07-23 北京大学 Holder attitude estimation method based on extended Kalman filtering
CN113155129B (en) * 2021-04-02 2022-07-01 北京大学 Holder attitude estimation method based on extended Kalman filtering
CN113124819A (en) * 2021-06-17 2021-07-16 中国空气动力研究与发展中心低速空气动力研究所 Monocular distance measuring method based on plane mirror
CN113865571A (en) * 2021-08-20 2021-12-31 无锡宇宁智能科技有限公司 Method and device for improving application precision of mobile phone compass and readable storage medium
CN114526756B (en) * 2022-01-04 2023-06-16 华南理工大学 Unmanned aerial vehicle-mounted multi-sensor correction method, device and storage medium
CN114526756A (en) * 2022-01-04 2022-05-24 华南理工大学 Unmanned aerial vehicle airborne multi-sensor correction method and device and storage medium
CN115855038A (en) * 2022-11-22 2023-03-28 哈尔滨工程大学 Short-time high-precision attitude keeping method
CN115855038B (en) * 2022-11-22 2024-01-09 哈尔滨工程大学 Short-time high-precision posture maintaining method
CN116182839A (en) * 2023-04-27 2023-05-30 北京李龚导航科技有限公司 Method and device for determining attitude of aircraft, electronic equipment and storage medium
CN117272593A (en) * 2023-08-24 2023-12-22 无锡北微传感科技有限公司 Wind tunnel test data analysis processing method
CN117272593B (en) * 2023-08-24 2024-04-05 无锡北微传感科技有限公司 Wind tunnel test data analysis processing method

Also Published As

Publication number Publication date
CN110887480B (en) 2020-06-30

Similar Documents

Publication Publication Date Title
CN110887480B (en) Flight attitude estimation method and system based on MEMS sensor
CN108827299B (en) Aircraft attitude calculation method based on improved quaternion second-order complementary filtering
CN111551174A (en) High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN110887481B (en) Carrier dynamic attitude estimation method based on MEMS inertial sensor
US11120562B2 (en) Posture estimation method, posture estimation apparatus and computer readable storage medium
CN111721288B (en) Zero offset correction method and device for MEMS device and storage medium
CN110715659A (en) Zero-speed detection method, pedestrian inertial navigation method, device and storage medium
CN106370178B (en) Attitude measurement method and device of mobile terminal equipment
CN109682377A (en) A kind of Attitude estimation method based on the decline of dynamic step length gradient
EP4220086A1 (en) Combined navigation system initialization method and apparatus, medium, and electronic device
CN112683269B (en) MARG attitude calculation method with motion acceleration compensation
CN113155129B (en) Holder attitude estimation method based on extended Kalman filtering
JP7025215B2 (en) Positioning system and positioning method
JP3726884B2 (en) Attitude estimation apparatus and method using inertial measurement apparatus, and program
CN110873563B (en) Cloud deck attitude estimation method and device
CN106767798B (en) Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation
CN111832690B (en) Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm
CN116067370A (en) IMU gesture resolving method, IMU gesture resolving equipment and storage medium
CN109506674B (en) Acceleration correction method and device
CN108871319B (en) Attitude calculation method based on earth gravity field and earth magnetic field sequential correction
CN113432612B (en) Navigation method, device and system for flying object
CN109674480B (en) Human motion attitude calculation method based on improved complementary filtering
CN110058324B (en) Strapdown gravimeter horizontal component error correction method using gravity field model
CN111183335B (en) Course determining method and device for determining course through magnetic sensor
CN108692727B (en) Strapdown inertial navigation system with nonlinear compensation filter

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20210918

Address after: 621000 No.6, south section of the Second Ring Road, Fucheng District, Mianyang City, Sichuan Province

Patentee after: LOW SPEED AERODYNAMIC INSTITUTE OF CHINESE AERODYNAMIC RESEARCH AND DEVELOPMENT CENTER

Address before: 621000 No.6, south section of the Second Ring Road, Fucheng District, Mianyang City, Sichuan Province

Patentee before: LOW SPEED AERODYNAMIC INSTITUTE OF CHINESE AERODYNAMIC RESEARCH AND DEVELOPMENT CENTER

Patentee before: WUXI BEWIS SENSING TECHNOLOGY Co.,Ltd.