CN112945225A - Attitude calculation system and method based on extended Kalman filtering - Google Patents
Attitude calculation system and method based on extended Kalman filtering Download PDFInfo
- Publication number
- CN112945225A CN112945225A CN202110069679.XA CN202110069679A CN112945225A CN 112945225 A CN112945225 A CN 112945225A CN 202110069679 A CN202110069679 A CN 202110069679A CN 112945225 A CN112945225 A CN 112945225A
- Authority
- CN
- China
- Prior art keywords
- data
- attitude
- filtering
- extended kalman
- gyroscope
- 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.)
- Pending
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 85
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000004364 calculation method Methods 0.000 title claims abstract description 49
- 238000013528 artificial neural network Methods 0.000 claims abstract description 43
- 238000005259 measurement Methods 0.000 claims abstract description 39
- 238000012545 processing Methods 0.000 claims abstract description 33
- 230000000295 complement effect Effects 0.000 claims abstract description 27
- 238000012549 training Methods 0.000 claims abstract description 20
- 230000004927 fusion Effects 0.000 claims abstract description 15
- 238000010606 normalization Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 31
- 230000008569 process Effects 0.000 claims description 23
- 230000001133 acceleration Effects 0.000 claims description 18
- 230000005484 gravity Effects 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000006880 cross-coupling reaction Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 4
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000004913 activation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000306 component Substances 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 239000008358 core component Substances 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000003745 diagnosis Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000006386 memory function Effects 0.000 description 1
- 210000002569 neuron Anatomy 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Theoretical Computer Science (AREA)
- Biophysics (AREA)
- General Health & Medical Sciences (AREA)
- Biomedical Technology (AREA)
- Life Sciences & Earth Sciences (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Navigation (AREA)
Abstract
The attitude calculation system and method based on the extended Kalman filtering are used for obtaining a triaxial attitude angle with higher precision by calculating and calibrating data of a gyroscope and an accelerometer; the data processing module carries out smooth filtering and normalization processing on noise-containing data of the IMU sensor to obtain measurement data after noise reduction processing and unified units, the resolving module corrects gyroscope data by means of linear complementary filtering combined with PI integral compensation, processed sensor data are led into extended Kalman filtering to carry out data fusion and attitude resolving, the calibrating module carries out online training by means of an Elman neural network, the IMU two-sensor information and attitude information before calibration output by the resolving module are used as input, attitude angle measurement values are used as neural network prediction values, three parallel Elman neural network structures are used for online training, weight values and threshold values of the neural network are updated in real time, three-axis attitude angles after calibration are output, resolving accuracy and system redundancy are improved.
Description
Technical Field
The invention belongs to the technical field of attitude calculation of fixed-wing unmanned aerial vehicles, and particularly relates to an attitude calculation system and a calculation method based on extended Kalman filtering.
Background
The flight control system of the fixed wing unmanned aerial vehicle comprises a plurality of modules such as attitude control, integrated navigation and fault diagnosis, is a core component of the unmanned aerial vehicle, determines the reliability and stability of the unmanned aerial vehicle, performs fusion processing on acquired data and performs attitude calculation to obtain a relatively accurate attitude angle, and is the first step of the whole flight control link. At present, a method for resolving the attitude adopted on a fixed-wing unmanned aerial vehicle generally uses a mahony complementary filter and an extended Kalman filter.
The Complementary Filter (CF) obtains low-frequency data from an accelerometer and a magnetic compass and fuses high-frequency data obtained from a gyroscope for attitude calculation, and the filter has the obvious advantages of small calculation amount and is the most widely used filter in commercial use at present, but the dynamic performance is lower than that of a Kalman filter, so that a flight control system has a hysteresis phenomenon, the attenuation near a cut-off frequency is slow, errors exist in the processed data, and the accuracy is low.
An Extended Kalman Filter (EKF) carries out linearization processing on a nonlinear system by Taylor series expansion, the dynamic performance of the EKF is good, only a parameter matrix is used for resolving, the estimation precision is not influenced, but the EKF also depends on a priori knowledge, and a certain linearization error exists in the linearization process.
The Neural Network (NN) is essentially a nonlinear mapping from an input space to a target space, error correlation between a carrier maneuvering device and an inertial navigation device can be accurately fitted, the Elman NN has a special bearing layer relative to the common NN, the Elman NN can enable the network to have a unique dynamic memory function, therefore, the Elman NN well accords with the inertial characteristic of the inertial navigation device, and can be used in an auxiliary mode by combining with an inertial measurement unit of a fixed wing unmanned aerial vehicle.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention aims to provide an attitude calculation system and a calculation method based on extended Kalman filtering, which utilize a neural network to calibrate the extended Kalman filtering and solve the limitations of the traditional attitude calculation method and the defects of excessive dependence on the performance of a gyroscope and an accelerometer sensor. The method can improve the influence of temperature drift, vibration and other errors on the sensor, and the Elman network is added, so that the dependency of attitude calculation on an inertial navigation system is reduced, the adaptability of attitude calculation is improved, the response speed and the convergence speed are higher, and the three-axis attitude angle of the unmanned aerial vehicle can be quickly and accurately calculated.
In order to achieve the purpose, the invention adopts the technical scheme that:
the attitude calculation system based on the extended Kalman filtering comprises a data processing module, a calculation module and a calibration module;
the data processing module: the device is used for processing data of two sensors, namely an IMU gyroscope and an accelerometer;
the resolving module is as follows: the system comprises a data fusion module, an attitude calculation module, an IMU gyroscope module, an accelerometer module, a data processing module and a data processing module, wherein the data fusion module is used for performing data fusion and attitude calculation on data of two sensors of the IMU gyroscope and the accelerometer through linear complementary filtering and extended Kalman filtering to obtain attitude information before calibration;
the calibration module is used for: the method is used for training and predicting the data of the IMU gyroscope and the accelerometer and the posture information before calibration as the input of an Elman neural network, so that the posture information after calibration is obtained.
The method for solving by using the attitude solution system based on the extended Kalman filtering comprises the following steps:
step 1, a data processing module processes data, and the specific method comprises the following steps:
leading in sensor noise-containing data, wherein the sensor noise-containing data comprises attitude angular velocity measured by a gyroscope and attitude angular gravity acceleration measured by an accelerometer; the sensor noisy data contains noise, and smooth filtering and normalization processing are carried out on the imported data to obtain processed data for the step 2;
step 2, a resolving module performs attitude resolving, and the specific method comprises the following steps:
firstly, filtering the data processed in the step 1 by a neural network through a linear complementary filtering algorithm, performing corresponding conversion and vector cross multiplication on acceleration g under different coordinate systems by using a rotation matrix, performing PI integral compensation on gyroscope data, and obtaining data closer to a real measurement result by fusing accelerometer high-pass filtering and gyroscope low-pass filtering;
thirdly, taking gyroscope data after linear complementary filtering as an initial state variable of an extended Kalman filtering algorithm, taking accelerometer measurement data as a measurement variable, solving a Jacobian coefficient matrix, a process noise covariance matrix and a measurement noise covariance matrix, substituting the obtained values into an extended Kalman filtering equation set for iterative calculation to obtain a quaternion representation of the attitude angle at the current moment, and repeating the steps to obtain a group of linear quaternion form attitude angles so as to achieve the purpose of accurately tracking the attitude information in real time;
step 3, calibrating the three-axis attitude angle by a calibration module, specifically:
inputting two sensor data and uncalibrated three-axis attitude angles to Elman NN, and adopting on-line training learning; when the work starts, setting the initial weight and the threshold of the Elman NN as random values, taking the attitude angle measurement value as a predicted value of the Elman NN, adopting a gradient descent learning method, updating the weight and the threshold of the neural network in real time, and outputting the calibrated triaxial attitude angle after learning.
Step 2, the neural network takes the three-axis attitude angle after linear complementary filtering and extended Kalman filtering resolving as input;
in order to avoid cross coupling in the training process, three parallel Elman neural network structures are adopted.
The three-axis attitude angle is obtained by resolving and calibrating data of two sensors, namely a gyroscope and an accelerometer, on an inertial measurement unit of the unmanned aerial vehicle;
the linear complementary filtering is combined with PI integral compensation to correct gyroscope data, the processed two sensor data are guided into an extended Kalman filtering to perform data fusion and attitude calculation, the Elman neural network takes the two sensor data and the calculated three-axis attitude angle information as input to perform online training in a neural network structure, the corrected three-axis attitude angle is output, and the calculation precision and the redundancy of the system are improved.
The extended Kalman filtering algorithm takes a quaternion representation of an attitude angle at a previous moment as an initial variable, takes gyroscope measurement data after linear complementary filtering as a state variable, takes accelerometer measurement data as a measurement variable, gives a coefficient matrix, obtains the quaternion representation of the attitude angle at the current moment through iterative calculation of an extended Kalman filtering equation set, and obtains a group of linear quaternion form attitude angles by analogy, thereby achieving the purpose of accurately tracking attitude information in real time.
Compared with the prior art, the invention has the beneficial effects that:
the attitude calculation method based on the improved extended Kalman filtering integrates the advantages of two algorithms, reduces the limitations of respective use, improves the calculation performance, and has higher real-time performance and stability.
The improved extended Kalman filtering module and the Elman neural network module not only perform algorithm processing on data of a single sensor and then perform attitude calculation, but also can calculate a triaxial attitude angle estimation value with higher precision in real time by fusing data of two sensors, namely a gyroscope and an accelerometer and performing online training through a neural network.
The invention only uses two sensors of the gyroscope and the accelerometer on the UAV-IMU, saves data space, saves time for data analysis and processing, improves the calculation speed and simplifies the calculation process.
The method comprises the steps of utilizing a neural network to calibrate the attitude calculation method of the extended Kalman filter fixed-wing unmanned aerial vehicle, and calculating and calibrating data of two sensors, namely a gyroscope and an accelerometer on a UAV-IMU (unmanned aerial vehicle-inertial measurement Unit), so as to obtain a three-axis attitude angle with higher precision; the data processing module carries out smooth filtering, normalization and other processing on IMU sensor measurement data, the resolving module firstly corrects gyroscope data by utilizing linear complementary filtering and PI integral compensation, then leads the processed two sensor data into extended Kalman filtering for data fusion and attitude resolving, the calibrating module carries out online training by utilizing an Elman neural network, IMU sensor information and attitude information before calibration output by the resolving module are used as input, an attitude angle measured value is used as a neural network predicted value, in order to avoid cross coupling in the training process, three parallel Elman neural network structures are adopted, weight values and threshold values of the neural network are updated in real time, three-axis attitude angles after calibration are output, resolving precision and system redundancy are improved.
Drawings
FIG. 1 is a method flow diagram of the system of the present invention.
Fig. 2 is a schematic block diagram of the system of the present invention.
Fig. 3 is a structural diagram of the Elman neural network according to the embodiment of the present invention.
Detailed Description
The invention is described in detail below with reference to the figures and the detailed description.
The attitude calculation system based on the extended Kalman filtering comprises a data processing module, a calculation module and a calibration module;
the data processing module: for processing IMU sensor data;
the resolving module is as follows: the system comprises a data fusion module, an attitude calculation module, an IMU gyroscope module, an accelerometer module, a data processing module and a data processing module, wherein the data fusion module is used for performing data fusion and attitude calculation on data of two sensors of the IMU gyroscope and the accelerometer through linear complementary filtering and extended Kalman filtering to obtain attitude information before calibration;
the calibration module is used for: the method is used for training and predicting the data of the IMU gyroscope and the accelerometer and the posture information before calibration as the input of an Elman neural network, so that the posture information after calibration is obtained.
The method for solving by using the attitude solution system based on the extended Kalman filtering comprises the following steps:
step 1, a data processing module processes data, and the specific method comprises the following steps:
leading in sensor noise-containing data, wherein the sensor noise-containing data comprises attitude angular velocity measured by a gyroscope and attitude angular gravity acceleration measured by an accelerometer; the sensor noisy data contains noise, and smooth filtering and normalization processing are carried out on the imported data to obtain processed data for the step 2;
step 2, a resolving module performs attitude resolving, and the specific method comprises the following steps:
firstly, filtering the data processed in the step 1 by a neural network through a linear complementary filtering algorithm, performing corresponding conversion and vector cross multiplication on acceleration g under different coordinate systems by using a rotation matrix, performing PI integral compensation on gyroscope data, and obtaining data closer to a real measurement result by fusing accelerometer high-pass filtering and gyroscope low-pass filtering;
thirdly, taking gyroscope data after linear complementary filtering as an initial state variable of an extended Kalman filtering algorithm, taking accelerometer measurement data as a measurement variable, solving a Jacobian coefficient matrix, a process noise covariance matrix and a measurement noise covariance matrix, substituting the obtained values into an extended Kalman filtering equation set for iterative calculation to obtain a quaternion representation of the attitude angle at the current moment, and repeating the steps to obtain a group of linear quaternion form attitude angles so as to achieve the purpose of accurately tracking the attitude information in real time;
step 3, calibrating the three-axis attitude angle by a calibration module, specifically:
inputting two sensor data and uncalibrated three-axis attitude angles to Elman NN, and adopting on-line training learning; when the work starts, setting the initial weight and the threshold of the Elman NN as random values, taking the attitude angle measurement value as a predicted value of the Elman NN, adopting a gradient descent learning method, updating the weight and the threshold of the neural network in real time, and outputting the calibrated triaxial attitude angle after learning.
Step 2, the neural network takes the three-axis attitude angle after linear complementary filtering and extended Kalman filtering resolving as input;
in order to avoid cross coupling in the training process, three parallel Elman neural network structures are adopted.
The three-axis attitude angle is obtained by resolving and calibrating data of two sensors, namely a gyroscope and an accelerometer, on an inertial measurement unit of the unmanned aerial vehicle;
the linear complementary filtering is combined with PI integral compensation to correct gyroscope data, the processed two sensor data are guided into an extended Kalman filtering to perform data fusion and attitude calculation, the Elman neural network takes the two sensor data and the calculated three-axis attitude angle information as input to perform online training in a neural network structure, the corrected three-axis attitude angle is output, and the calculation precision and the redundancy of the system are improved.
The extended Kalman filtering algorithm takes a quaternion representation of an attitude angle at a previous moment as an initial variable, takes gyroscope measurement data after linear complementary filtering as a state variable, takes accelerometer measurement data as a measurement variable, gives a coefficient matrix, obtains the quaternion representation of the attitude angle at the current moment through iterative calculation of an extended Kalman filtering equation set, and obtains a group of linear quaternion form attitude angles by analogy, thereby achieving the purpose of accurately tracking attitude information in real time.
The specific implementation method comprises the following steps: firstly, importing collected sensor noise-containing data, and carrying out smooth filtering, normalization and other processing on the data; then, the processed data are led into a resolving module for attitude resolving, and a resolved triaxial attitude angle is obtained by continuous updating; and then inputting the sensor information and the attitude angle information into ElmanNN for on-line training, and outputting the calibrated three-axis attitude angle in real time after updating a proper weight and a proper threshold value. Results show that compared with standard extended Kalman filtering solution and Mohony complementary filtering solution, the three-axis attitude angle mean square error calculated by the method is smaller, and particularly at a time point with larger attitude angle change, the calculation process is more stable and accurate.
Examples
The attitude resolving method of the fixed-wing unmanned aerial vehicle based on the assistance of the improved extended Kalman filtering algorithm and the Elman neural network comprises the following steps:
step 1, importing and processing data, specifically comprising:
and importing sensor noise-containing data which respectively comprise attitude angular velocity measured by a gyroscope and attitude angular gravity acceleration measured by an accelerometer. Because the data contains certain noise, the imported data is subjected to smooth filtering and normalization processing for the use of the step 2;
step 2, a resolving module performs attitude resolving, and the specific method comprises the following steps:
firstly, filtering the data processed in the step 1 by a linear complementary filtering algorithm, fusing accelerometer high-pass filtering and gyroscope low-pass filtering, and carrying out PI integral compensation on the gyroscope data to obtain data closer to a real measurement result;
thirdly, taking gyroscope data after linear complementary filtering as an initial state variable of an extended Kalman filtering algorithm, taking accelerometer measurement data as a measurement variable, giving a coefficient matrix, obtaining a quaternion representation method of the attitude angle at the current moment through iterative calculation of an extended Kalman filtering equation set, and so on to obtain a group of linear quaternion form attitude angles, thereby achieving the purpose of accurately tracking attitude information in real time;
step 3, calibrating the three-axis attitude angle by a calibration module, specifically:
inputting two sensor data and uncalibrated triaxial attitude angles to Elman NN, adopting on-line training learning, setting initial weight and threshold of Elman NN as random values when work starts, adopting a gradient descent learning method, updating the weight and threshold of a neural network in real time, and finally outputting the calibrated triaxial attitude angles.
In the above technical solution, the specific process of using the linear complementary filtering algorithm in step 2 is as follows:
firstly, determining a standard gravity acceleration g in a geographic coordinate system n, and defining the standard gravity acceleration g as gn＝[0 0 1]TThen, the standard gravitational acceleration can be obtained according to the rotation matrix in the carrier coordinate system b
The rotation matrix in the form of a known quaternion is
The rotation matrix in the above formula is the rotation sequence of the three-axis attitude angle according to ZYX, and the quaternion matrix Q is [ Q ═ Q0 q1 q2q3]Expressed, the three-axis gravity acceleration [ v ] can be obtainedx vy vz]Is composed of
Let the values of the three axes of the accelerometer be ax,ay,azThe modulo and normalization processes are performed separately as shown in the following equation.
Modulus calculation:
normalization:
then, for gb、abAnd (4) performing vector cross multiplication to obtain a correction compensation value e for the gyroscope, which is shown as the following formula.
Then, the PI controller is used for filtering, specifically, eliminating the drift error, as long as there is an error, the controller will continue to act until the error is 0, the control effect depends on the P and I parameters, which respectively correspond to the parameters of proportional control and integral control, and the filtering formula of the PI controller is as follows:
and finally, substituting the compensated angular velocity value w of the gyroscope into an extended Kalman filtering algorithm to update the attitude quaternion.
In the above technical solution, the step 2 using the extended kalman filter algorithm is mainly divided into two processes, a prediction process and a measurement process:
in the prediction process, firstly, the angular velocity vector [ w ] measured by the three-axis gyroscope after linear complementary filtering is utilizedgxwgy wgz]The initial estimate of the attitude is computed in quaternion form. The angular velocity of the rigid body is subject to a vector differential equation describing the rate of change of direction as a quaternion derivative, as shown in the following equation:
in the above equation
Is a quaternion form of the three-axis attitude angles,
corresponding to its derivative, coefficient matrix thereinbwq,tIt can be rewritten in a matrix form as follows:
wherein
Then, discretizing the above formula to obtain a system state equation shown in formula (8). Since the orientation of the onboard coordinate system with respect to time t of the geographic coordinate system can be calculated by numerically integrating the quaternion derivative using the sampling period Δ t, the discrete state transition vector equation describing the prediction process can be derived from (7), as follows:
xk+1＝Φ(bw,Δt)xk+wk (8)
wherein wkIs a state noise of approximately white Gaussian noise, the state transition matrix phi (bw, Δ t) are calculated by zeroth-order integration
And finally, updating the quaternion according to a formula (9), calculating the quaternion at the current moment and carrying out normalization processing on the quaternion.
In the measuring process, the characteristic that the accelerometer can measure gravity in a static state is utilized, and when the accelerometer inclines, the acceleration of gravity on three axes changes, so that the attitude angle is calculated.
Firstly, for a triaxial accelerometer, an acceleration vector measured at any moment is taken as an observation variable and recorded as an observation variable
z(k)＝[abx(k) aby(k) abz(k)]T+vk (10)
In the formula abx(k)、aby(k)、abz(k) Is the direction component of the gravity acceleration corresponding to the x, y and z axes in the carrier coordinate system, vkIs a measurement noise that is approximately white gaussian noise;
then, coordinate conversion is performed according to the rotation matrix. The rotation matrix in the form of a known quaternion is
The rotation matrix in the above formula is the rotation sequence of the three-axis attitude angle according to ZYX, and the quaternion matrix Q is [ Q ═ Q0 q1 q2q3]Expressed. Determining a standard gravity acceleration g in a geographic coordinate system n, and defining the acceleration as gn＝[0 0 1]TDue to the rotation of the matrix
Always maintain orthogonality, so
Then the standard gravitational acceleration can be obtained according to the rotation matrix in the carrier coordinate system b
Obtaining the compound shown in the formula (12):
the system's observation equation can be expressed as shown in the following equation:
then, h (x (k), k) is a nonlinear function according to equation (13), and the nonlinear part needs to be linearized by Taylor expansion to obtain a Jacobian matrix of
And finally, in order to keep the property of the quaternion unit norm, after the measurement updating stage, the estimation is normalized by adopting the final normalization step.
And finally solving a quaternion form of the three-axis attitude angles of the fusion gyroscope and the accelerometer through iteration and calculation of a prediction process and a measurement process, and solving three attitude angles through an Euler angle inverse solution formula: roll angle, pitch angle, course angle.
In the above technical solution, the Elman neural network main body adopted in step 3 is a 4-layer network structure, and has a multi-input and single-output structure, the input layer is parallel input, 9 pieces of information including information from a three-axis (x, y, z) gyroscope, information about three-axis (x, y, z) acceleration, and 3 pieces of posture information before calibration are used as input layer neurons of the Elman neural network, parameters of the network structure can be trained online through network learning, and a structure diagram of the Elman neural network is shown in fig. 3.
The calculation formula of the Elman neural network is as follows:
wherein, w11The weights of the hidden layer and the receiving layer are set; w is a12The weights of the input layer and the hidden layer are obtained; w is a13The connection weight of the hidden layer and the output layer; x is the number ofc(k) Inputting for an input layer; x (k) is the hidden layer input; y (k) is the output of the output layer; a is a feedback gain factor, f (-) and g (-) are activation functions, k is 2,3, …, n, n is the number of samples,
the learning method adopts a gradient descent method, the excitation function f (x) is sigmoid, and the formula is as follows:
finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art will understand that various changes, modifications and substitutions can be made without departing from the spirit and scope of the invention as defined by the appended claims. Any modification, equivalent replacement, or improvement made without departing from the spirit and principle of the present invention shall fall within the protection scope of the present invention.
Claims (5)
1. The attitude calculation system based on the extended Kalman filtering comprises a data processing module, a calculation module and a calibration module;
the data processing module: the device is used for processing data of two sensors, namely an IMU gyroscope and an accelerometer;
the resolving module is as follows: the system comprises a data fusion module, an attitude calculation module, an IMU gyroscope module, an accelerometer module, a data processing module and a data processing module, wherein the data fusion module is used for performing data fusion and attitude calculation on data of two sensors of the IMU gyroscope and the accelerometer through linear complementary filtering and extended Kalman filtering to obtain attitude information before calibration;
the calibration module is used for: the method is used for training and predicting the data of the IMU gyroscope and the accelerometer and the posture information before calibration as the input of an Elman neural network, so that the posture information after calibration is obtained.
2. The method for solving by using the attitude solution system based on the extended Kalman filtering is characterized by comprising the following steps of:
step 1, a data processing module processes data, and the specific method comprises the following steps:
leading in sensor noise-containing data, wherein the sensor noise-containing data comprises attitude angular velocity measured by a gyroscope and attitude angular gravity acceleration measured by an accelerometer; the sensor noisy data contains noise, and smooth filtering and normalization processing are carried out on the imported data to obtain processed data for the step 2;
step 2, a resolving module performs attitude resolving, and the specific method comprises the following steps:
firstly, filtering the data processed in the step 1 by a neural network through a linear complementary filtering algorithm, performing corresponding conversion and vector cross multiplication on acceleration g under different coordinate systems by using a rotation matrix, performing PI integral compensation on gyroscope data, and obtaining data closer to a real measurement result by fusing accelerometer high-pass filtering and gyroscope low-pass filtering;
thirdly, taking gyroscope data after linear complementary filtering as an initial state variable of an extended Kalman filtering algorithm, taking accelerometer measurement data as a measurement variable, solving a Jacobian coefficient matrix, a process noise covariance matrix and a measurement noise covariance matrix, substituting the obtained values into an extended Kalman filtering equation set for iterative calculation to obtain a quaternion representation of the attitude angle at the current moment, and repeating the steps to obtain a group of linear quaternion form attitude angles so as to achieve the purpose of accurately tracking the attitude information in real time;
step 3, calibrating the three-axis attitude angle by a calibration module, specifically:
inputting two sensor data and uncalibrated three-axis attitude angles to Elman NN, and adopting on-line training learning; when the work starts, setting the initial weight and the threshold of the Elman NN as random values, taking the attitude angle measurement value as the predicted value of the Elman NN, adopting a gradient descent learning method, updating the weight and the threshold of the neural network in real time, and outputting the calibrated triaxial attitude angle through training and learning.
3. The method for solving by using an extended kalman filter-based attitude solution system according to claim 2, wherein in the step 2, the neural network takes three-axis attitude angles after the linear complementary filtering and the extended kalman filter solution as input;
to avoid cross-coupling of the training process, three parallel Elman neural network structures are used.
4. The method for solving by using an attitude solution system based on extended kalman filter according to claim 3, wherein the three-axis attitude angle is obtained by solving and calibrating data of two sensors, namely a gyroscope and an accelerometer, on an inertial measurement unit of the unmanned aerial vehicle;
the linear complementary filtering is combined with PI integral compensation to correct gyroscope data, the processed two sensor data are guided into an extended Kalman filtering to perform data fusion and attitude calculation, the Elman neural network takes the two sensor data and the calculated three-axis attitude angle information as input to perform online training in a neural network structure, the corrected three-axis attitude angle is output, and the calculation precision and the redundancy of the system are improved.
5. The method for solving the problem by using the attitude solution system based on the extended kalman filter according to claim 3, wherein the extended kalman filter algorithm uses the quaternion representation of the attitude angle at the previous moment as an initial variable, uses the gyroscope measurement data after the linear complementary filtering as a state variable, uses the accelerometer measurement data as a measurement variable, gives a coefficient matrix, obtains the quaternion representation of the attitude angle at the current moment through iterative computation of an extended kalman filter equation set, and obtains a group of linear quaternion form attitude angles by analogy, thereby achieving the purpose of accurately tracking the attitude information in real time.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110069679.XA CN112945225A (en) | 2021-01-19 | 2021-01-19 | Attitude calculation system and method based on extended Kalman filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110069679.XA CN112945225A (en) | 2021-01-19 | 2021-01-19 | Attitude calculation system and method based on extended Kalman filtering |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112945225A true CN112945225A (en) | 2021-06-11 |
Family
ID=76235654
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110069679.XA Pending CN112945225A (en) | 2021-01-19 | 2021-01-19 | Attitude calculation system and method based on extended Kalman filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112945225A (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113663312A (en) * | 2021-08-16 | 2021-11-19 | 东南大学 | Micro-inertia-based non-apparatus body-building action quality evaluation method |
| CN113777645A (en) * | 2021-09-10 | 2021-12-10 | 北京航空航天大学 | High-precision pose estimation algorithm in GPS rejection environment |
| CN114061571A (en) * | 2021-11-12 | 2022-02-18 | 同济大学 | Attitude calculation method and system of adaptive gradient descent inertial measurement unit |
| CN114162344A (en) * | 2021-11-08 | 2022-03-11 | 陕西千山航空电子有限责任公司 | Data calibration device for airplane control system |
| CN114485877A (en) * | 2022-01-25 | 2022-05-13 | 常州纺织服装职业技术学院 | Weighing system and method for weighing compensation by combining inertia measurement module |
| CN114718546A (en) * | 2022-04-01 | 2022-07-08 | 中国矿业大学 | Novel anti-impact drilling robot pose adjusting method for spatial distribution IMU |
| CN115063945A (en) * | 2022-06-20 | 2022-09-16 | 浙江科技学院 | Fall detection alarm method and system based on attitude fusion calculation |
| CN115326063A (en) * | 2022-07-07 | 2022-11-11 | 江苏大块头智驾科技有限公司 | Inertial device signal filtering method based on deep learning |
| CN116588261A (en) * | 2023-07-03 | 2023-08-15 | 上海新纪元机器人有限公司 | Active compensation control method and system for seat |
| CN116625407A (en) * | 2023-06-05 | 2023-08-22 | 泉州职业技术大学 | Intelligent micro-attitude measurement method and system |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110208473A1 (en) * | 2008-07-18 | 2011-08-25 | Cindy Bassompiere | Method for an improved estimation of an object orientation and attitude control system implementing said method |
| CN106500695A (en) * | 2017-01-05 | 2017-03-15 | 大连理工大学 | A kind of human posture recognition method based on adaptive extended kalman filtering |
| CN108981694A (en) * | 2018-07-18 | 2018-12-11 | 兰州交通大学 | Attitude algorithm method and system based on wavelet neural network and EKF |
| CN109001787A (en) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | A kind of method and its merge sensor of solving of attitude and positioning |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
| CN111060097A (en) * | 2020-01-15 | 2020-04-24 | 东南大学 | Inertia/astronomical combined navigation method for improving position error estimation precision |
| CN111426318A (en) * | 2020-04-22 | 2020-07-17 | 中北大学 | Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering |
-
2021
- 2021-01-19 CN CN202110069679.XA patent/CN112945225A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110208473A1 (en) * | 2008-07-18 | 2011-08-25 | Cindy Bassompiere | Method for an improved estimation of an object orientation and attitude control system implementing said method |
| CN106500695A (en) * | 2017-01-05 | 2017-03-15 | 大连理工大学 | A kind of human posture recognition method based on adaptive extended kalman filtering |
| CN109001787A (en) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | A kind of method and its merge sensor of solving of attitude and positioning |
| CN108981694A (en) * | 2018-07-18 | 2018-12-11 | 兰州交通大学 | Attitude algorithm method and system based on wavelet neural network and EKF |
| CN110146077A (en) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | Pose of mobile robot angle calculation method |
| WO2020253854A1 (en) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | Mobile robot posture angle calculation method |
| CN111060097A (en) * | 2020-01-15 | 2020-04-24 | 东南大学 | Inertia/astronomical combined navigation method for improving position error estimation precision |
| CN111426318A (en) * | 2020-04-22 | 2020-07-17 | 中北大学 | Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering |
Non-Patent Citations (3)
| Title |
|---|
| 严甲汉;郭承军;: "基于Elman神经网络的SINS姿态解算算法研究", 电子测量与仪器学报, no. 06 * |
| 张栋;焦嵩鸣;刘延泉;: "互补滤波和卡尔曼滤波的融合姿态解算方法", 传感器与微***, no. 03 * |
| 王见等: "EKF与互补融合滤波在姿态解算中的研究", 传感技术学报, vol. 31, no. 8, pages 1187 - 1191 * |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113663312B (en) * | 2021-08-16 | 2022-05-13 | 东南大学 | Micro-inertia-based non-apparatus body-building action quality evaluation method |
| CN113663312A (en) * | 2021-08-16 | 2021-11-19 | 东南大学 | Micro-inertia-based non-apparatus body-building action quality evaluation method |
| CN113777645A (en) * | 2021-09-10 | 2021-12-10 | 北京航空航天大学 | High-precision pose estimation algorithm in GPS rejection environment |
| CN113777645B (en) * | 2021-09-10 | 2024-01-09 | 北京航空航天大学 | High-precision pose estimation algorithm under GPS refused environment |
| CN114162344A (en) * | 2021-11-08 | 2022-03-11 | 陕西千山航空电子有限责任公司 | Data calibration device for airplane control system |
| CN114061571B (en) * | 2021-11-12 | 2023-08-04 | 同济大学 | Gesture resolving method and system of self-adaptive gradient descent inertia measurement unit |
| CN114061571A (en) * | 2021-11-12 | 2022-02-18 | 同济大学 | Attitude calculation method and system of adaptive gradient descent inertial measurement unit |
| CN114485877B (en) * | 2022-01-25 | 2023-09-05 | 常州纺织服装职业技术学院 | Weighing system and method for weighing compensation by combining inertial measurement module |
| CN114485877A (en) * | 2022-01-25 | 2022-05-13 | 常州纺织服装职业技术学院 | Weighing system and method for weighing compensation by combining inertia measurement module |
| CN114718546A (en) * | 2022-04-01 | 2022-07-08 | 中国矿业大学 | Novel anti-impact drilling robot pose adjusting method for spatial distribution IMU |
| CN115063945A (en) * | 2022-06-20 | 2022-09-16 | 浙江科技学院 | Fall detection alarm method and system based on attitude fusion calculation |
| CN115063945B (en) * | 2022-06-20 | 2023-12-29 | 浙江科技学院 | Fall detection alarm method and system based on attitude fusion calculation |
| CN115326063A (en) * | 2022-07-07 | 2022-11-11 | 江苏大块头智驾科技有限公司 | Inertial device signal filtering method based on deep learning |
| CN116625407A (en) * | 2023-06-05 | 2023-08-22 | 泉州职业技术大学 | Intelligent micro-attitude measurement method and system |
| CN116625407B (en) * | 2023-06-05 | 2024-02-20 | 泉州职业技术大学 | Intelligent micro-attitude measurement method and system |
| CN116588261A (en) * | 2023-07-03 | 2023-08-15 | 上海新纪元机器人有限公司 | Active compensation control method and system for seat |
| CN116588261B (en) * | 2023-07-03 | 2024-02-09 | 上海新纪元机器人有限公司 | Active compensation control method and system for seat |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112945225A (en) | Attitude calculation system and method based on extended Kalman filtering | |
| CN108827299B (en) | Aircraft attitude calculation method based on improved quaternion second-order complementary filtering | |
| CN109001787B (en) | Attitude angle resolving and positioning method and fusion sensor thereof | |
| CN110017837B (en) | Attitude anti-magnetic interference combined navigation method | |
| US9417091B2 (en) | System and method for determining and correcting field sensors errors | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN109945859B (en) | Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering | |
| CN108731676B (en) | Attitude fusion enhanced measurement method and system based on inertial navigation technology | |
| US20180120127A1 (en) | Attitude sensor system with automatic accelerometer bias correction | |
| CN110426032B (en) | Analytical redundant aircraft fault-tolerant navigation estimation method | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN108981694A (en) | Attitude algorithm method and system based on wavelet neural network and EKF | |
| Sushchenko et al. | Processing of redundant information in airborne electronic systems by means of neural networks | |
| CN112683269B (en) | MARG attitude calculation method with motion acceleration compensation | |
| EP3527948B1 (en) | Air data aided inertial measurement unit | |
| CN109086250B (en) | Data fusion method suitable for MEMS inertial measurement unit with inclined fiber-optic gyroscope | |
| CN111964688B (en) | Attitude estimation method combining unmanned aerial vehicle dynamic model and MEMS sensor | |
| CN110702113B (en) | Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor | |
| CN109708663B (en) | Star sensor online calibration method based on aerospace plane SINS assistance | |
| JP2021179438A (en) | System and method for compensating for absence of sensor measurement in heading measuring system | |
| CN116147624B (en) | Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| CN110553642A (en) | Method for improving inertial guidance precision | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN110595434B (en) | Quaternion fusion attitude estimation method based on MEMS sensor |
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 |