CN113607167B - Self-adaptive attitude estimation method for navigation attitude system and smooth switching algorithm thereof - Google Patents
Self-adaptive attitude estimation method for navigation attitude system and smooth switching algorithm thereof Download PDFInfo
- Publication number
- CN113607167B CN113607167B CN202110277314.6A CN202110277314A CN113607167B CN 113607167 B CN113607167 B CN 113607167B CN 202110277314 A CN202110277314 A CN 202110277314A CN 113607167 B CN113607167 B CN 113607167B
- Authority
- CN
- China
- Prior art keywords
- attitude
- measurement
- kalman filter
- gps
- algorithm
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000008569 process Effects 0.000 claims abstract description 34
- 230000004927 fusion Effects 0.000 claims abstract description 16
- 238000001914 filtration Methods 0.000 claims abstract description 6
- 238000005259 measurement Methods 0.000 claims description 111
- 239000011159 matrix material Substances 0.000 claims description 46
- 230000001133 acceleration Effects 0.000 claims description 22
- 239000013598 vector Substances 0.000 claims description 21
- 230000005484 gravity Effects 0.000 claims description 15
- 230000003044 adaptive effect Effects 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 6
- 230000005358 geomagnetic field Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 5
- 230000026676 system process Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 2
- 238000002360 preparation method Methods 0.000 claims 1
- 238000004088 simulation Methods 0.000 abstract description 9
- 238000002474 experimental method Methods 0.000 abstract description 2
- 238000004891 communication Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000000737 periodic effect Effects 0.000 description 3
- CYTYCFOTNPOANT-UHFFFAOYSA-N Perchloroethylene Chemical compound ClC(Cl)=C(Cl)Cl CYTYCFOTNPOANT-UHFFFAOYSA-N 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000011084 recovery Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000005856 abnormality Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 230000003750 conditioning effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses a self-adaptive attitude estimation method for a navigation attitude system and a smooth switching algorithm thereof. The algorithm realizes the switching between the low order and the high order according to the GPS state, and comprises the following steps: under the condition that the GPS state is good, estimating attitude information by adopting a high-order integrated navigation fusion algorithm; stopping the high-order integrated navigation algorithm and switching to the low-order attitude estimation algorithm when the GPS signal is interfered; and after the GPS signal is recovered, taking the low-order attitude estimation state as an initial value of a high-order filtering algorithm, and realizing smooth switching of an attitude fusion algorithm. The ARM architecture-based attitude and heading reference system is designed for the algorithm, the reliability of the design algorithm is verified through simulation and experiments, the attitude angle error in the switching process is smaller than 0.5 degrees, the convergence time is smaller than 0.2s, and the attitude estimation precision of the unmanned aerial vehicle in the GPS positioning state or not is effectively improved.
Description
Technical Field
The invention relates to a self-adaptive attitude estimation method for an avionic attitude system and a smooth switching algorithm thereof, which are suitable for the technical field of avionic navigation.
Technical Field
With the development of micro-electromechanical technology, a triaxial MEMS gyroscope, an accelerometer and a magnetometer become the most dominant inertial measurement elements. However, the existing MEMS devices have certain drawbacks: the MEMS gyroscope has drift, and the gyroscope is independently used for estimating the attitude to have accumulated errors; the MEMS accelerometer is greatly influenced by linear acceleration and vibration, and a large error can occur when the accelerometer is independently used for estimating the attitude angle; MEMS magnetometers are susceptible to magnetic field disturbances generated by the surrounding environment, thereby affecting the heading angle output. Because the errors are random and cannot be eliminated in advance, the accurate attitude angle cannot be obtained by the method alone, and the influence caused by various errors must be compensated by a reasonable data fusion method to obtain stable and reliable attitude information.
Disclosure of Invention
Object of the Invention
The invention aims to improve the accuracy of attitude estimation of a system under the condition of GPS signal loss and ensure that an unmanned aerial vehicle can still fly stably when a GPS signal device is lost.
Technical proposal
In order to achieve the above purpose, the present invention adopts the following technical scheme: an adaptive attitude estimation method for a voyage system, the method comprising the implementation steps of:
Step 1, under the condition that the GPS state is good, a low-order EKF attitude estimation algorithm based on quaternion and a high-order integrated navigation fusion algorithm based on GPS speed and position are operated simultaneously, and the attitude information is estimated by actually adopting the high-order integrated navigation algorithm;
Step 2, stopping the high-order integrated navigation algorithm and switching to adopting a low-order attitude estimation algorithm when the GPS signal is interfered;
And step3, after the GPS signal is recovered, taking the quaternion value obtained by the low-order attitude estimation algorithm as the initial value of the high-order filtering algorithm, and realizing the smooth switching of the attitude fusion algorithm.
Furthermore, the quaternion-based low-order EKF attitude estimation algorithm is specifically implemented by adopting a triaxial accelerometer, a triaxial gyroscope and a magnetometer, and applying a low-order Kalman filter to realize data fusion of each sensor and calculate attitude and heading information of the unmanned aerial vehicle.
Furthermore, the quaternion-based low-order EKF attitude estimation algorithm comprises the following steps of: in the low-order Kalman filter based on quaternion, the system state quantity X is set as an attitude quaternion:
X＝[q0 q1 q2 q3]T (1)
wherein q 0、q1、q2、q3 is a gesture quaternion;
According to the kalman filter principle, the state equation defining the discrete system is as follows:
Xk＝AkXk-1+ωk-1 (2)
Wherein X k、Xk-1 represents the system state quantity at k-time and k-time respectively, A k is a state transition matrix from k-1 time to k-time, ω k-1 represents the system process noise sequence at k-1 time, and the state equation corresponding to the Kalman filter is:
representing the state of a priori estimates of the system k times, Representing the state of the posterior estimation at time k-1 of the system, wherein:
Wherein, Representing an angular rate vector matrix:
Wherein: omega x represents the yaw axis angular rate, omega y represents the pitch axis angular rate, and omega z represents the yaw axis angular rate.
Furthermore, in the prior estimation process of the Kalman filter state, only a gyroscope is used for carrying out angle updating; a posterior estimate is then developed, which comprises two phases:
The first stage is based on accelerometer data and the second stage is based on magnetometer data, and the formula of the correction equation is shown as follows according to the Kalman filter principle:
Wherein Z k represents the actual measurement value at the moment k, specifically the data detected by an accelerometer and a magnetometer; The expected measurement value of k moment is calculated according to the prior estimation state of the system; k k represents the kalman gain at time K, which is derived from equation (7):
In the method, in the process of the invention, The method comprises the steps of representing a priori estimated covariance matrix, wherein Q k-1 is a process noise matrix, P k-1 and P k respectively represent a last time and a current time of posterior estimated covariance matrix, R k represents a measured noise matrix, H k has two forms and respectively represents jacobian matrices H k1 and H k2 for biasing quaternion by a desired gravity vector or a magnetic field vector, and I represents an identity matrix;
h 1 represents the expected gravity vector measurement value under the body axis, which is obtained by a direction cosine matrix And calculating the gravity vector under the NED coordinate system, and setting the modulus |g| of the gravity acceleration as a constant value of 9.807m/s 2, then:
From the above, it can be obtained:
H k1 represents a jacobian matrix that deflects the state X by a gravity vector measurement H 1;
Similarly, h 2 represents a desired magnetic field vector measurement value under the body axis, and the local geomagnetic bias angle ψ mag and the magnetic dip angle θ mag are queried, and the magnetic field is normalized to obtain:
Then:
H k2 represents a jacobian matrix that deflects the state X by the magnetic field vector measurement H 2.
Furthermore, the attitude estimation algorithm is specifically that a Kalman filter is applied to continuously correct an inertial navigation system according to the position and the speed measured by the GPS receiver, so that the aim of estimating the attitude, the speed and the position of the unmanned aerial vehicle is fulfilled.
Further, the process of the high-order integrated navigation algorithm based on GPS measurement specifically comprises the following steps:
in Gao Jieka Kalman filter based on GPS measurement, the set system state quantity is:
Wherein q 0、q1、q2、q3 is a gesture quaternion, v N、vE、vd is a north-oriented speed, an east-oriented speed and a ground-oriented speed of the NED coordinate system respectively, P N、PE、PD is a north-oriented position, an east-oriented position and a ground-oriented position of the NED coordinate system respectively, and bias x、biasy、biasz is a random drift estimated value of the gyroscope around a transverse rolling shaft, a pitching shaft and a heading shaft respectively;
updating a priori estimates x k(1∶4),xk(1:4) of the first four states in the state quantity to be states consisting of q 0、q1、q2、q3 using equation (4); body acceleration and direction cosine matrix by using kinematics equation The a priori estimate x k(5:10) is obtained and the update procedure is as follows:
x k(1:4) is the state consisting of q 0、q1、q2、q3
Wherein a x、ay、az is an acceleration measurement value under a machine body coordinate system, namely an accelerometer measurement value after dimension transformation, and dt represents recursion time estimated by a system in advance;
At this time, the system state transition matrix is:
The measurement signal is a triaxial speed V GNSS in the NED coordinate system, a triaxial position P GNSS in the NED coordinate system, a triaxial geomagnetic field measurement value in the body axis system is m b, and a heading angle ψ m calculated therefrom, wherein ψ m is given by the formula (16):
The corresponding measurement matrix is:
When the speed and the position are fused, the measured values are the speed V GNSS and the position P GNSS measured by the GPS receiver, and the measured matrixes are H VEL (k) and H POS (k); when course angle fusion is performed, the measurement value is a course angle psi m measured by a magnetometer, and the measurement matrix is H MAG (k).
The application also provides a self-adaptive attitude estimation smooth switching algorithm for the navigation attitude system, which comprises the following steps:
firstly, constructing a low-order Kalman filter based on quaternion and a Gao Jieka Kalman filter based on GPS measurement at the same time when a system initially operates, and enabling the low-order Kalman filter and the Gao Jieka Kalman filter to operate synchronously to wait for convergence;
Then, in the case of a velocity and position measurement signal, the state of the system output is the estimated value of Gao Jieka kalman filter based on GPS measurement, as shown in equation (18):
[φ θ ψ]＝[φ2 θ2 ψ2] (18)
Wherein phi, theta and phi respectively represent the roll angle, the pitch angle and the course angle output by the system, phi 2、θ2、ψ2 represents the Gao Jieka Kalman filter estimated value based on GPS measurement;
Under the condition of losing the speed and position measurement signals, stopping the posterior estimation process of the Gao Jieka Kalman filter based on GPS measurement, and simultaneously stopping the prior estimation process; outputting an estimated value of a low-order kalman filter based on the quaternion as shown in formula (19):
[φ θ ψ]＝[φ1 θ1 ψ1] (19)
wherein phi 1、θ1、ψ1 represents a low-order Kalman filter based on quaternion;
When the speed and position measurement signals are recovered, the prior estimation process and the posterior estimation process of the Gao Jieka Kalman filter based on GPS measurement are restored, at the moment, the covariance matrix estimated by the system is near the covariance matrix when the system is stable, the attitude quaternion estimated value of the low-order Kalman filter based on the quaternion is used as the initial estimated value of the attitude quaternion when the Gao Jieka Kalman filter based on the GPS measurement is restored, the current measurement speed and the position signal of the GPS are used as the initial estimated value of the speed and the position when the Gao Jieka Kalman filter based on the GPS measurement is restored, and at the moment, the state output by the system is the estimated value of the Gao Jieka Kalman filter based on the GPS measurement, as shown in a formula (20):
Where x 1 represents the low order Kalman filter state quantity based on quaternion and x 2 represents the Gao Jieka Kalman filter state quantity based on GPS measurements.
Further, the switching algorithm is applied to the following attitude and heading reference system: the navigation system comprises a navigation computer, wherein the navigation computer takes ARM CortexM as a main processor, is used for acquiring sensor data and carrying out two sets of gesture estimation filtering algorithm operations, and is used for completing self-adaptive smooth switching of the algorithm when detecting that a GPS breaks down/recovers;
The system also comprises an inertial sensor, wherein the inertial sensor is an ADIS16470 inertial measurement unit and is used for measuring triaxial angular rate and triaxial acceleration information respectively;
the magnetometer is used for measuring triaxial geomagnetic field information, and further comprises a magnetometer RM 3100;
And the GPS satellite navigation system is NEO-M8N and is used for measuring the triaxial position and triaxial speed information.
Advantageous effects
The invention has the following advantages: the algorithm ensures the accuracy of the attitude estimation under the condition of losing the speed and position measurement, and when the speed and position measurement is obtained again, the switching process of the system estimation value is accurate and stable, and no great abrupt change occurs, thereby being beneficial to the system stability.
Description of the drawings
In order to more clearly illustrate the technical solutions of the present invention, the drawings that are needed in the present invention will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a block diagram of an ARM architecture-based avionic system;
FIG. 2 is a low-order Kalman filter estimate based on quaternion with acceleration and deceleration;
FIG. 3 is a GPS anomaly Gao Jieka Kalman filter estimate;
FIG. 4 is a flow chart of an adaptive smooth handoff algorithm;
FIG. 5 is a simulation diagram of an adaptive smooth handoff algorithm;
FIG. 6 is a graph of a high-order integrated navigation algorithm estimate and reference based on GPS measurements;
FIG. 7 is a low-order integrated navigation algorithm estimate and reference based on quaternion.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.
Example 1
The invention adopts the following technical scheme: an adaptive attitude estimation method for a voyage system, the method comprising the implementation steps of:
Step 1, under the condition that the GPS state is good, a low-order EKF attitude estimation algorithm based on quaternion and a high-order integrated navigation fusion algorithm based on GPS speed and position are operated simultaneously, and the attitude information is estimated by actually adopting the high-order integrated navigation algorithm;
Step 2, stopping the high-order integrated navigation algorithm and switching to adopting a low-order attitude estimation algorithm when the GPS signal is interfered;
And step3, after the GPS signal is recovered, taking the quaternion value obtained by the low-order attitude estimation algorithm as the initial value of the high-order filtering algorithm, and realizing the smooth switching of the attitude fusion algorithm.
Further, as shown in fig. 1, the navigation attitude system selects STM32 series MCU as a navigation computer of the system; the gyroscope and the accelerometer adopt ADIS16470, and an SPI interface is adopted to output data and receive configuration information; the GPS adopts NEO-M8N, and outputs navigation data based on NEMA format and navigation data based on UBX format through SPI interface; the magnetometer adopts RM3100 to output triaxial geomagnetic signals through an SPI interface of the control chip. Because the interface level of the selected radio station is the RS-232 level in order to reduce the interference, the serial port communicated with the upper computer needs to convert the TTL level into the RS-232 level.
Further, the low-order attitude estimation algorithm based on the quaternion comprises the following steps: in the low-order Kalman filter based on quaternion, the system state quantity is set as an attitude quaternion:
X＝[q0 q1 q2 q3]T (1)
According to the kalman filter principle, the state equation defining the discrete system is as follows:
Xk＝AkXk-1+ωk-1 (2)
Where ω k-1 represents the system process noise sequence at time k-1. The state equation corresponding to the kalman filter is:
representing the state of a priori estimates of the system k times, The state of the posterior estimation at time k-1 of the system is shown. Wherein:
Wherein, Can be expressed as:
Wherein: omega x represents the yaw axis angular rate, omega y represents the pitch axis angular rate, and omega z represents the yaw axis angular rate.
Equation (3) describes an a priori estimation process using only gyroscopes to update the angle, and subsequently requires correction of the equation using a kalman filter. There are two phases in the system, one correction equation for each phase: the first phase uses accelerometer data and the second phase uses magnetometer data. According to the Kalman filter principle, the formula of the correction equation is as follows:
where Z k represents the actual measurement of time k, in this system an accelerometer and magnetometer. The expected measurement value of k moment is calculated according to the prior estimation state of the system; k k represents the Kalman gain at time K, and can be obtained by the formula (7).
In the method, in the process of the invention,The prior estimation covariance matrix is represented, Q k-1 is a process noise matrix, P k-1 and P k respectively represent the last and the current posterior estimation covariance matrices, R k represents a measurement noise matrix, H k has two forms, jacobian matrices H k1 and H k2 respectively representing expected gravity vectors or magnetic field vectors for biasing quaternions, and I represents an identity matrix.
H 1 represents the expected gravity vector measurement value under the body axis, and can be obtained by a direction cosine matrixAnd calculating the gravity vector under the NED coordinate system, and setting the modulus |g| of the gravity acceleration as a constant value of 9.807m/s 2, then:
From the above, it can be obtained:
Similarly, h 2 represents a desired magnetic field vector measurement value under the body axis, and the local geomagnetic bias angle ψ mag and the magnetic dip angle θ mag can be queried, and the magnetic field is normalized to obtain:
Then:
And simultaneously applying a triaxial acting force and a triaxial rotating moment to the six-degree-of-freedom model, so that the six-degree-of-freedom model generates periodic rotation and periodic acceleration and deceleration, and a low-order Kalman filter based on quaternion is adopted to estimate the gesture, and the simulation situation is shown in figure 2. Simulation results show that when the unmanned aerial vehicle has an acceleration and deceleration process, the system accelerometer is used for measuring not only the gravity acceleration, but also the body acceleration, so that a larger error occurs in the measurement process of the system, and further, a larger error occurs in the posterior estimation process, and finally, the whole gesture estimation process has a larger deviation, and the maximum deviation reaches +/-8 degrees, so that the system requirement cannot be met.
The low-order Kalman filter based on the quaternion is updated all the time under any condition, so that the unmanned aerial vehicle attitude can still be accurately estimated under the conditions that the system loses speed and position measurement are ensured. Meanwhile, when the speed and the position measurement are obtained again, the gesture quaternion estimated by the low-order Kalman filter is used as an initial value when the high-order Kalman filter is restored to operate, and jump does not occur in the switching process; after the Gao Jieka Kalman filter is suspended, the system covariance is near the expected convergence value, so that the convergence process of the Gao Jieka Kalman filter is stable, the convergence speed is high, and the problem of attitude estimation under the conditions of speed measurement and position measurement is solved.
However, because the measurement of the pitch angle and the roll angle of the system depends on the accelerometer, and the accelerometer measurement can superimpose the acceleration of the machine body and the vibration of the machine body, when the machine body continuously accelerates and decelerates, the measured pitch angle and roll angle have larger errors, and further, the estimated pitch angle and roll angle have larger errors, so that a high-order integrated navigation algorithm with GPS measurement needs to be introduced.
Further, the high-order integrated navigation algorithm based on GPS measurement comprises the following steps:
in Gao Jieka Kalman filter based on GPS measurement, the set system state quantity is:
where q 0、q1、q2、q3 is the attitude quaternion, v N、vE、vd is the NED coordinate system north-down speed, east-direction speed, and earth-direction speed, P N、PE、PD is the NED coordinate system north-down position, east-direction position, and earth-direction position, and bias x、biasy、biasz is the gyroscope random drift estimate around the roll axis, pitch axis, and heading axis, respectively.
As described in the quaternion-based low order attitude estimation algorithm, the prior estimates x k(1∶4) of the first four states in the state quantity (i.e., quaternions) can be updated using equation (4). Using the kinematic equation, the body acceleration and direction cosine matrix C "b obtains a priori estimates x k(5:10), the update process is as follows:
wherein a x、ay、az is the acceleration measurement value under the machine body coordinate system, namely the accelerometer measurement value after dimension transformation, and dt represents the recursion time estimated by the system a priori.
At this time, the system state transition matrix should be:
at this time, the measurement signal is the triaxial speed V GNSS in the NED coordinate system, the triaxial position P GNSS in the NED coordinate system, the triaxial geomagnetic field measurement value in the body axis system is m b, and the heading angle ψ m is calculated therefrom, wherein ψ m can be obtained from the formula (16).
The corresponding measurement matrix is:
as described in the low-order attitude estimation algorithm based on quaternion, when speed and position fusion is performed, the measurement values are the speed V GNSS and the position P GNSS measured by the GPS receiver, and the measurement matrices are H VEL (k) and H POS (k); when course angle fusion is performed, the measurement value is a course angle psi m measured by a magnetometer, and the measurement matrix is H MAG (k).
When the model loses GPS speed and position measurement in a certain time, the Gao Jieka Kalman filter posterior estimation link based on GPS measurement is greatly affected, and the simulation result is shown in figure 3. At 450s, the system loses GPS speed and position measurement, at the moment, the posterior estimation of the system cannot be updated, the state quantity estimated by the system has larger error, and the maximum attitude estimation error can reach 35 degrees. And meanwhile, the GPS speed and position measurement is re-captured at 750s, a larger jump value appears in the convergence process, the maximum jump value reaches 80 degrees, and the convergence time is longer than 10s.
Example 2
The application also discloses a self-adaptive attitude estimation smooth switching algorithm for the navigation attitude system, namely a low-order EKF attitude estimation algorithm based on quaternion and a high-order integrated navigation fusion algorithm based on GPS speed and position, which comprises the following steps:
Firstly, when the system is initially operated, a low-order Kalman filter based on quaternion and a Gao Jieka Kalman filter based on GPS measurement are simultaneously constructed, and are operated synchronously, and the convergence of the low-order Kalman filter and the Gao Jieka Kalman filter is waited.
Then, in the case of velocity and position measurement, the state of the system output is the estimated value of Gao Jieka kalman filter based on GPS measurement, as shown in formula (18):
[φ θ ψ]＝[φ2 θ2 ψ2] (18)
Wherein phi, theta and phi respectively represent the roll angle, the pitch angle and the course angle output by the system, phi 2、θ2、ψ2 represents the Gao Jieka Kalman filter estimated value based on GPS measurement.
Under the condition of losing speed and position measurement, the posterior estimation process of the Gao Jieka Kalman filter based on GPS measurement is stopped, and the prior estimation process is stopped, so that the covariance matrix of the estimation is prevented from diverging. And outputs an estimated value of the low-order kalman filter based on the quaternion as in formula (19):
[φ θ ψ]＝[φ1 θ1 ψ1] (19)
where phi 1、θ1、ψ1 denotes a low-order kalman filter based on quaternions.
When the speed and position measurement is obtained again, the prior estimation process and the posterior estimation process of the Gao Jieka Kalman filter based on the GPS measurement are restored to operate, at this time, the covariance matrix estimated by the system is near the covariance matrix when the system is stable, the attitude quaternion estimated value of the low-order Kalman filter based on the quaternion is used as the initial estimated value of the attitude quaternion when the Gao Jieka Kalman filter based on the GPS measurement is restored to operate, the current measurement speed and the position of the GPS are used as the initial estimated value of the speed and the position when the Gao Jieka Kalman filter based on the GPS measurement is restored to operate, and at this time, the state output by the system is the estimated value of the Gao Jieka Kalman filter based on the GPS measurement, as shown in a formula (20):
Where x 1 represents the low order Kalman filter state quantity based on quaternion and x 2 represents the Gao Jieka Kalman filter state quantity based on GPS measurements. At this time, because the estimated covariance matrix of Gao Jieka kalman filter based on GPS measurement is near the covariance matrix of steady state, and the system state quantity is near the true value, the system will not generate larger correction amount, and will not have longer convergence time, the state after switching is near the true value immediately, the state after switching can be used to control unmanned plane at any time without generating larger deviation, and meanwhile, the stability at the moment of switching is guaranteed.
The smooth switching algorithm is applied to the following navigation attitude system:
STM32H743 series MCU is selected as a navigation computer of the system, the processor is ARMCortex-M7 kernel based on ST company, and is packaged as LQFP144, the main frequency is up to 480MHz, and the processor is provided with on-chip FLASH of 2M and on-chip RAM of 1M. Meanwhile, the system is provided with an L1 Cache first-level Cache for accelerating a low-speed memory; the sensor is provided with 6 SPI interfaces, so that communication with the sensor is facilitated; and simultaneously, an SDIO interface is also supported, so that data can be conveniently stored in the SD card.
The sensor in the navigation attitude system adopts ADIS16470 of Adenode semiconductor company, and each inertial sensor built in the MIMU realizes the leading industryThe perfect combination of the technology and the signal conditioning technology can provide optimized dynamic performance, and the sensitivity, bias, alignment and linear acceleration (gyro bias) of each sensor are calibrated when leaving the factory. In addition, the MIMU has strong impact resistance and good bias stability and noise density in motion.
The navigation attitude system selects the RM3100 magnetometer of PNI company, which has the measuring range of +/-800 uT, the noise of less than 15nT, the resolution ratio of higher than 13nT, and the system can automatically recover after being interfered by strong magnetic, does not need to calibrate again, and the SPI communication interface used by the system has high speed and short occupied system time.
The navigation attitude system selects a GPS satellite navigation system as a NEO-M8N satellite navigation chip of U-BLOX company. The chip can simultaneously receive GPS signals and BDS signals, has small volume, low power consumption and quick star searching, and is suitable for various environments. The communication interface can be set as UART, IIC, SPI and the like, and can output navigation data based on NEMA format and navigation data based on UBX format, so that the NEO-M8N type GPS satellite navigation chip is selected.
The technical scheme of the invention is specifically described by combining the attached chart, a six-degree-of-freedom model based on MATLAB/SIMULINK is simulated, and an algorithm test verifies a navigation attitude system based on an ARM architecture.
As shown in fig. 4, which is a self-adaptive smooth switching flow chart, three-axis acting force and three-axis rotating moment are simultaneously applied to the six-degree-of-freedom model, so that periodic acceleration and deceleration are also generated while the six-degree-of-freedom model is periodically rotated, the GPS speed and position observation is disconnected at 150s, the GPS speed and position observation is restored at 450s, and the simulation result is shown in fig. 5. As can be seen from the simulation results on the right of FIG. 5, in the stage of 0-150 s, the GPS speed and position observation is normal, and Gao Jieka Kalman attitude estimation values are output; in the time period of 150-450 s, the GPS speed and position are observed abnormally, and a low-order Kalman attitude estimated value is output; after 450s, the GPS speed and position observation is recovered, and the low-order attitude estimation value is used as a high-order initial value to calculate and output Gao Jieka Kalman attitude estimation value. As can be seen from the left side of fig. 5, the left graph of fig. 5 is a comparison of the true value and the estimated value, and the two graphs almost coincide together because the error is small, so the right graph is added, and the right graph represents the difference between the true value and the estimated value. In the process of algorithm switching, jump of the attitude estimation value is smaller than 0.5 degrees, convergence time is smaller than 0.2s, the requirement of autonomous flight of the unmanned aerial vehicle is completely met, and the unmanned aerial vehicle can accurately estimate the state under the condition of losing speed and position observation. According to the whole simulation result, the unmanned aerial vehicle self-adaptive smooth switching gesture fusion algorithm provided by the chapter not only ensures the accuracy of gesture estimation of the system under the condition of continuous acceleration and deceleration, but also maintains the accuracy of gesture estimation during GPS data abnormality; and the convergence speed and stability of a high-order integrated navigation algorithm based on GPS measurement during GPS signal recovery are improved, and important guarantee is provided for stability enhancement control of an unmanned aerial vehicle system.
In order to verify the effectiveness of the present invention, comparative experiments and analyses were performed as follows:
The flight test of algorithm verification adopts a certain six-axis multi-rotor unmanned plane, self-grinding attitude measurement equipment consists of a gyroscope, an accelerometer (ADIS 16470), a GPS (NEO-M8N) and a magnetometer (RM 3100), and a certain commercial attitude sensor is used as reference for comparison, and a remote controller is used for switching whether the GPS measurement state is lost. Experimental data are shown in fig. 6 and 7. in the figure, phi 1、θ1、ψ1 represents the estimated attitude of a commercial attitude sensor, phi 2、θ2、ψ2 represents the estimated attitude of a Gao Jieka Kalman filter which stops running after losing GPS signals, phi 3、θ3、ψ3 represents the estimated attitude of a Gao Jieka Kalman filter which only stops speed and position measurement updating after losing GPS signals, Phi 4、θ4、ψ4 denotes the estimated pose of the low-order Kalman filter. The GPS speed and position measurement signals are disconnected about 407s by adopting a mode of manually disconnecting the GPS speed and position observation, and the GPS signals are recovered about 482 s. As shown by simulation results, in the normal stage of GPS speed and position observation, phi 3、θ3、ψ3 has high estimation accuracy compared with phi 4、θ4、ψ4, so that a high-order estimation value is output in the stage; in the stage of disconnecting the GPS speed and position, phi 3、θ3、ψ3 starts to have larger deviation, which can reach 40 DEG at maximum, while phi 4、θ4、ψ4 can keep up with phi 1、θ1、ψ1, thus outputting a low-order estimated value in this stage; At the moment of recovering the measurement of the speed and the position of the GPS, phi 3、θ3、ψ3 has larger jump, the maximum jump can reach 50 degrees, the convergence time is more than 5s, phi 2、θ2、ψ2 is immediately recovered to the vicinity of the reference value, the error is less than 0.5 degrees, the convergence time is less than 0.2s, and no large posture estimation jump occurs, thereby meeting the requirement of unmanned plane state estimation switching.
According to experimental results, the self-adaptive attitude estimation smooth switching algorithm of the navigation attitude system provided by the patent not only ensures the accuracy of attitude estimation of the system under the condition of continuous acceleration and deceleration, but also maintains the attitude estimation during the abnormal period of GPS signals; and the convergence speed and stability of a high-order integrated navigation algorithm based on GPS measurement during GPS signal recovery are improved, and important guarantee is provided for stability enhancement control of an unmanned aerial vehicle system.
It will be understood by those skilled in the art that all or part of the steps for implementing the above embodiments may be implemented by hardware, or may be implemented by a program for instructing relevant hardware, where the program may be stored in a computer readable storage medium, and the above storage medium may be a read-only memory, a magnetic disk or an optical disk, etc.
The foregoing description of the preferred embodiments of the application is not intended to be limiting, but rather is intended to cover all modifications, equivalents, alternatives, and improvements that fall within the spirit and scope of the application.
Claims (4)
1. An adaptive attitude estimation method for a navigation attitude system, characterized by comprising the following implementation steps:
Step 1, under the condition that the GPS state is good, a low-order EKF attitude estimation algorithm based on quaternion and a high-order integrated navigation fusion algorithm based on GPS speed and position are operated simultaneously, and the attitude information is estimated by actually adopting the high-order integrated navigation algorithm; the low-order EKF attitude estimation algorithm based on the quaternion comprises the following steps: in the low-order Kalman filter based on quaternion, the system state quantity X is set as an attitude quaternion:
X＝[q0 q1 q2 q3]T (1)
wherein q 0、q1、q2、q3 is a gesture quaternion;
According to the kalman filter principle, the state equation defining the discrete system is as follows:
Xk＝AkXk-1+ωk-1 (2)
Wherein X k、Xk-1 represents the system state quantity at k-time and k-time respectively, A k is a state transition matrix from k-1 time to k-time, ω k-1 represents the system process noise sequence at k-1 time, and the state equation corresponding to the Kalman filter is:
representing the state of a priori estimates of the system k times, Representing the state of the posterior estimation at time k-1 of the system, wherein:
Wherein, Representing an angular rate vector matrix:
wherein: omega x represents the yaw axis angular rate, omega y represents the pitch axis angular rate, and omega z represents the yaw axis angular rate;
The process of the high-order integrated navigation algorithm based on GPS measurement specifically comprises the following steps:
in Gao Jieka Kalman filter based on GPS measurement, the set system state quantity is:
Wherein q 0、q1、q2、q3 is a gesture quaternion, v N、vE、vd is a north-oriented speed, an east-oriented speed and a ground-oriented speed of the NED coordinate system respectively, P N、PE、PD is a north-oriented position, an east-oriented position and a ground-oriented position of the NED coordinate system respectively, and bias x、biasy、biasz is a random drift estimated value of the gyroscope around a transverse rolling shaft, a pitching shaft and a heading shaft respectively;
Updating a priori estimates x k(1:4),xk(1:4) of the first four states in the state quantity to be states consisting of q 0、q1、q2、q3 using equation (4); body acceleration and direction cosine matrix by using kinematics equation The a priori estimate x k(5:10) is obtained and the update procedure is as follows:
x k(1:4) is the state consisting of q 0、q1、q2、q3
Wherein a x、ay、az is an acceleration measurement value under a machine body coordinate system, namely an accelerometer measurement value after dimension transformation, and dt represents recursion time estimated by a system in advance;
At this time, the system state transition matrix is:
The measurement signal is a triaxial speed V GNSS in the NED coordinate system, a triaxial position P GNSS in the NED coordinate system, a triaxial geomagnetic field measurement m b in the body axis system, and a heading angle ψ m calculated therefrom, wherein ψ m is given by the formula (16):
The corresponding measurement matrix is:
When the speed and the position are fused, the measured values are the speed V GNSS and the position P GNSS measured by the GPS receiver, and the measured matrixes are H VEL (k) and H POS (k); when course angle fusion is carried out, the measurement value is a course angle psi m measured by a magnetometer, and the measurement matrix is H MAG (k);
Step 2, stopping the high-order integrated navigation algorithm and switching to adopting a low-order attitude estimation algorithm when the GPS signal is interfered;
Step3, after the GPS signal is recovered, taking the quaternion value obtained by the low-order attitude estimation algorithm as the initial value of the high-order filtering algorithm, and realizing the smooth switching of the attitude fusion algorithm;
The handover algorithm comprises the steps of:
firstly, constructing a low-order Kalman filter based on quaternion and a Gao Jieka Kalman filter based on GPS measurement at the same time when a system initially operates, and enabling the low-order Kalman filter and the Gao Jieka Kalman filter to operate synchronously to wait for convergence;
Then, in the case of a velocity and position measurement signal, the state of the system output is the estimated value of Gao Jieka kalman filter based on GPS measurement, as shown in equation (18):
[φ θ ψ]＝[φ2 θ2 ψ2] (18)
Wherein, psi, theta and psi respectively represent the roll angle, the pitch angle and the course angle output by the system, phi 2、θ2、ψ2 represents the estimated value of the Gao Jieka Kalman filter based on GPS measurement;
Under the condition of losing the speed and position measurement signals, stopping the posterior estimation process of the Gao Jieka Kalman filter based on GPS measurement, and simultaneously stopping the prior estimation process; outputting an estimated value of a low-order kalman filter based on the quaternion as shown in formula (19):
[φ θ ψ]＝[φ1 θ1 ψ1] (19)
wherein phi 1、θ1、ψ1 represents a low-order Kalman filter based on quaternion;
When the speed and position measurement signals are recovered, the prior estimation process and the posterior estimation process of the Gao Jieka Kalman filter based on GPS measurement are restored, at the moment, the covariance matrix estimated by the system is near the covariance matrix when the system is stable, the attitude quaternion estimated value of the low-order Kalman filter based on the quaternion is used as the initial estimated value of the attitude quaternion when the Gao Jieka Kalman filter based on the GPS measurement is restored, the current measurement speed and the position signal of the GPS are used as the initial estimated value of the speed and the position when the Gao Jieka Kalman filter based on the GPS measurement is restored, and at the moment, the state output by the system is the estimated value of the Gao Jieka Kalman filter based on the GPS measurement, as shown in a formula (20):
Where x 1 represents the low-order Kalman filter state quantity based on quaternion, and x 2 represents the Gao Jieka Kalman filter state quantity based on GPS measurement;
The switching algorithm is applied to the following attitude and heading reference system: the navigation system comprises a navigation computer, wherein the navigation computer takes ARM CortexM as a main processor, is used for acquiring sensor data and carrying out two sets of gesture estimation filtering algorithm operations, and is used for completing self-adaptive smooth switching of the algorithm when detecting that a GPS breaks down/recovers;
The system also comprises an inertial sensor, wherein the inertial sensor is an ADIS16470 inertial measurement unit and is used for measuring triaxial angular rate and triaxial acceleration information respectively;
the magnetometer is used for measuring triaxial geomagnetic field information, and further comprises a magnetometer RM 3100;
And the GPS satellite navigation system is NEO-M8N and is used for measuring the triaxial position and triaxial speed information.
2. The self-adaptive attitude estimation method for the attitude and heading system according to claim 1, wherein the quaternion-based low-order EKF attitude estimation algorithm is specifically that a three-axis accelerometer, a three-axis gyroscope and a magnetometer are adopted, a low-order kalman filter is applied to realize data fusion of each sensor, and attitude and heading information of the unmanned aerial vehicle are calculated.
3. An adaptive attitude estimation method for a voyage system according to claim 1, wherein,
The prior estimation process of the Kalman filter state only uses a gyroscope to update angles; a posterior estimate is then developed, which comprises two phases:
The first stage is based on accelerometer data and the second stage is based on magnetometer data, and the formula of the correction equation is shown as follows according to the Kalman filter principle:
Wherein Z k represents the actual measurement value at the moment k, specifically the data detected by an accelerometer and a magnetometer; The expected measurement value of k moment is calculated according to the prior estimation state of the system; k k represents the kalman gain at time K, which is derived from equation (7):
In the method, in the process of the invention, The method comprises the steps of representing a priori estimated covariance matrix, wherein Q k-1 is a process noise matrix, P k-1 and P k respectively represent a last time and a current time of posterior estimated covariance matrix, R k represents a measured noise matrix, H k has two forms and respectively represents jacobian matrices H k1 and H k2 for biasing quaternion by a desired gravity vector or a magnetic field vector, and I represents an identity matrix;
h 1 represents the expected gravity vector measurement value under the body axis, and is calculated by the direction cosine matrix C n b and the gravity vector under the NED coordinate system, and if the modulus |g| of the gravity acceleration is a constant value of 9.807m/s 2, then:
The preparation method comprises the following steps:
H k1 represents a jacobian matrix that deflects the state X by a gravity vector measurement H 1;
Similarly, h 2 represents a desired magnetic field vector measurement value under the body axis, and the local geomagnetic bias angle ψ mag and the magnetic dip angle θ mag are queried, and the magnetic field is normalized to obtain:
Then:
H k2 represents a jacobian matrix that deflects the state X by the magnetic field vector measurement H 2.
4. The adaptive attitude estimation method for a navigation attitude system according to claim 1, wherein the attitude estimation algorithm is specifically that a kalman filter is applied to continuously correct an inertial navigation system according to the position and the speed measured by a GPS receiver, so as to achieve the purpose of estimating the attitude, the speed and the position of the unmanned aerial vehicle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110277314.6A CN113607167B (en) | 2021-03-15 | 2021-03-15 | Self-adaptive attitude estimation method for navigation attitude system and smooth switching algorithm thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110277314.6A CN113607167B (en) | 2021-03-15 | 2021-03-15 | Self-adaptive attitude estimation method for navigation attitude system and smooth switching algorithm thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113607167A CN113607167A (en) | 2021-11-05 |
| CN113607167B true CN113607167B (en) | 2024-07-05 |
Family
ID=78303307
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110277314.6A Active CN113607167B (en) | 2021-03-15 | 2021-03-15 | Self-adaptive attitude estimation method for navigation attitude system and smooth switching algorithm thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113607167B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107525503A (en) * | 2017-08-23 | 2017-12-29 | 王伟 | Adaptive cascade kalman filter method based on double antenna GPS and MIMU combination |
| CN111964688A (en) * | 2020-07-10 | 2020-11-20 | 北京航空航天大学 | Attitude estimation method combining unmanned aerial vehicle dynamic model and MEMS sensor |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112013836B (en) * | 2020-08-14 | 2022-02-08 | 北京航空航天大学 | Attitude heading reference system algorithm based on improved adaptive Kalman filtering |
-
2021
- 2021-03-15 CN CN202110277314.6A patent/CN113607167B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107525503A (en) * | 2017-08-23 | 2017-12-29 | 王伟 | Adaptive cascade kalman filter method based on double antenna GPS and MIMU combination |
| CN111964688A (en) * | 2020-07-10 | 2020-11-20 | 北京航空航天大学 | Attitude estimation method combining unmanned aerial vehicle dynamic model and MEMS sensor |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113607167A (en) | 2021-11-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109916395B (en) | Gesture autonomous redundant combined navigation algorithm | |
| US20050240347A1 (en) | Method and apparatus for adaptive filter based attitude updating | |
| US10627237B2 (en) | Offset correction apparatus for gyro sensor, recording medium storing offset correction program, and pedestrian dead-reckoning apparatus | |
| CN112066985B (en) | Initialization method, device, medium and electronic equipment for combined navigation system | |
| JP2012173190A (en) | Positioning system and positioning method | |
| CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
| CN102654404A (en) | Method for improving resolving precision and anti-jamming capability of attitude heading reference system | |
| CN111207745A (en) | Inertia measurement method suitable for vertical gyroscope of large maneuvering unmanned aerial vehicle | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN107402007A (en) | A kind of method for improving miniature AHRS modules precision and miniature AHRS modules | |
| CN113959462B (en) | Quaternion-based inertial navigation system self-alignment method | |
| US20220252399A1 (en) | Composite sensor and angular velocity correction method | |
| Leccadito et al. | A kalman filter based attitude heading reference system using a low cost inertial measurement unit | |
| Guan et al. | Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system | |
| Allgeuer et al. | Robust sensor fusion for robot attitude estimation | |
| Ding et al. | A quaternion based error state Kalman filter for attitude estimation using low-cost MEMS MARG sensors | |
| CN113607167B (en) | Self-adaptive attitude estimation method for navigation attitude system and smooth switching algorithm thereof | |
| CN115655271B (en) | Large-range attitude angle extraction method under dynamic condition | |
| CN115727842B (en) | Unmanned aerial vehicle rapid alignment method, unmanned aerial vehicle rapid alignment system, computer equipment and storage medium | |
| CN114964214B (en) | Extended Kalman filtering attitude calculation method of attitude heading reference system | |
| Kortunov et al. | Integrated mini INS based on MEMS sensors for UAV control | |
| Hua et al. | Velocity-aided attitude estimation for accelerated rigid bodies | |
| CN113932803B (en) | Inertial/geomagnetic/satellite integrated navigation system suitable for high-dynamic aircraft | |
| CN111025908B (en) | Attitude and heading reference system based on adaptive maneuvering acceleration extended Kalman filter | |
| CN113984042B (en) | Series combined navigation method applicable to high-dynamic aircraft |
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 |