CN116380054A - Aircraft attitude calculation method - Google Patents

Aircraft attitude calculation method Download PDF

Info

Publication number
CN116380054A
CN116380054A CN202310259439.5A CN202310259439A CN116380054A CN 116380054 A CN116380054 A CN 116380054A CN 202310259439 A CN202310259439 A CN 202310259439A CN 116380054 A CN116380054 A CN 116380054A
Authority
CN
China
Prior art keywords
quaternion
attitude
liquid crystal
display device
crystal display
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
Application number
CN202310259439.5A
Other languages
Chinese (zh)
Inventor
付少忠
周易
李靖
高明
刘刚
葛建华
田静
吴寅鹏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xidian University
Original Assignee
Xidian University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xidian University filed Critical Xidian University
Priority to CN202310259439.5A priority Critical patent/CN116380054A/en
Publication of CN116380054A publication Critical patent/CN116380054A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

The invention discloses a resolving method of an aircraft gesture, which mainly solves the defects of low precision and angle jump of the existing gesture fusion method. The implementation scheme is as follows: acquiring output data of an inertial measurement unit, and performing preprocessing of filtering correction to obtain an angular velocity quaternion, an acceleration quaternion and a geomagnetic field quaternion; substituting the angular velocity quaternion into a quaternion differential equation to obtain a predicted attitude quaternion; calculating an error function by using the acceleration quaternion and the geomagnetic field quaternion, and solving the gradient direction; setting a self-adaptive step length, and calculating a gesture quaternion by combining the step length and the gradient direction; complementarily fusing the predicted attitude quaternion and the calculated attitude quaternion to obtain a fused attitude quaternion, and converting the fused attitude quaternion into an attitude angle; and setting a dynamic limiting threshold value by using the angular velocity quaternion, and carrying out dynamic limiting filtering on the attitude angle to obtain a final attitude angle. The invention reduces the jump of the angle, improves the precision of the attitude angle, and can be used for an inertial navigation system.

Description

Aircraft attitude calculation method
Technical Field
The invention belongs to the technical field of data processing, and particularly relates to an aircraft attitude resolving method which can be used for an inertial navigation system.
Background
The accurate acquisition of the object attitude is a precondition for inertial navigation of the aircraft. The inertial measurement unit has the advantages of small volume and high cost performance, and is a common sensor for acquiring attitude data. The nine-axis inertial measurement unit consists of an accelerometer, a gyroscope and a magnetometer, the gyroscope has more accurate measurement results under the scene of rapid angle change, but has obvious accumulated errors after integration for a period of time, the accelerometer and the magnetometer have larger measurement noise, the measurement results of the accelerometer during rapid acceleration and deceleration of an object are easily interfered by motion acceleration, and the magnetometer is easily interfered by magnetic materials in the nearby environment. Thus, the key to pose resolution is how to fuse the two measured data to obtain more accurate results.
The existing attitude resolving method based on the inertial measurement unit comprises a complementary filtering method, an extended Kalman filtering method and a gradient descent method, wherein:
the main idea of the complementary filtering method is that: and (5) correcting the result of integrating the angular velocity by using the acceleration and geomagnetic field data in real time. Aiming at the poor dynamic response characteristic of the gyroscope in a low frequency band, a high-pass filter is used for suppressing noise; the dynamic response characteristics of the accelerometer and the magnetometer in a high frequency band are poor, and the low-pass filter is used for suppressing noise, so that the complementation of the dynamic response characteristics of the sensor is realized. The method is high in practicability, but PI correction parameters are difficult to adjust and not suitable for a high-precision requirement system.
The extended kalman filtering method mainly follows two steps: prediction and updating. In the prediction stage, a state equation is constructed by selecting the attitude quaternion and drift deviation of a gyroscope, and a predicted attitude is generated; in the updating stage, an orthogonalization method is utilized to obtain attitude quaternion from acceleration and geomagnetic field data to serve as a measurement vector, and the attitude of the aircraft can be accurately represented by expanding Kalman filtering and fusing a predicted value and a measured value. However, since matrix operation involves a large amount of calculation, the operation speed and precision of the processor are high, and the method is not suitable for low-cost aircraft application.
The main idea of the gradient descent method is that: along the negative gradient direction of the attitude error, the optimal estimated value of the attitude is obtained by continuously iterating the error function. Wherein the error function is generally calculated from acceleration, geomagnetic field data. The gradient descent method can eliminate noise interference to a certain extent, but proper step parameters are required to be designed, too large step length can cause divergence of the solved attitude angle, and too small step length can cause poor convergence effect.
The posture fusion method of the gradient descent method is proposed by the journal of electronic design engineering in 2021, gao Yi, li Donghang and Guo Piao, and the method utilizes the gradient descent method to fuse acceleration and geomagnetic field data and solves a group of posture quaternions; simultaneously estimating another group of attitude quaternions by angular velocity integration output by the gyroscope; and finally complementarily fusing the two groups of gesture quaternions and converting the quaternions into gesture angles. The method solves the problem of deadlock of the universal joint, has the advantages of no need of acquiring local geomagnetic inclination angle and high convergence speed, but is difficult to ensure the attitude angle precision in a low-speed motion state and a high-speed motion state simultaneously due to the adoption of a fixed step length, so that the average positioning error of the inertial navigation system for calculating the aircraft track is larger.
Disclosure of Invention
The invention aims at overcoming the defects of the gradient descent gesture fusion method, and provides a method for resolving the gesture of an aircraft, so that the average positioning error of the gesture of the aircraft calculated by a navigation system is reduced, and the positioning precision of the inertial navigation system of the aircraft is improved.
In order to achieve the above purpose, the technical scheme of the invention comprises the following steps:
s1) obtaining output data of an inertial measurement unit, and performing preprocessing of filtering correction to obtain an angular velocity quaternion under a carrier coordinate system b
Figure BDA0004130696980000021
Acceleration quaternion->
Figure BDA0004130696980000022
Geomagnetic field quaternion->
Figure BDA0004130696980000023
S2) quaterning the angular velocity
Figure BDA0004130696980000024
Substituting the quaternion differential equation to predict the attitude quaternion of the carrier coordinate system b relative to the geographic coordinate system n>
Figure BDA0004130696980000025
S3) utilizing predicted gesture quaternions
Figure BDA0004130696980000026
Calculating theoretical gravitational acceleration->
Figure BDA0004130696980000027
Simultaneously, the theoretical geomagnetic field +.A. under the geographic coordinate system is calculated by utilizing the characteristic that the geomagnetic field has no component in the east-west direction>
Figure BDA0004130696980000028
Obtaining an error function f through cross product operation of a theoretical vector and an actual measurement vector a,m And solve f a,m Is a gradient direction of (2);
s4) setting an adaptive step size mu based on the acceleration and angular velocity factors output by the inertial measurement unit in the step S1), and combining the adaptive step size mu with an error function f a,m Is used for calculating attitude quaternion in negative gradient direction
Figure BDA0004130696980000029
S5) predicting the attitude quaternion of the carrier coordinate system b relative to the geographic coordinate system n in the step S2)
Figure BDA00041306969800000210
Gesture quaternion calculated with step S4)>
Figure BDA00041306969800000211
Complementary fusion is carried out to obtain a fused gesture quaternion +.>
Figure BDA00041306969800000212
And converts it into an attitude angle Euler, which includes a roll angle +.>
Figure BDA00041306969800000213
Pitch angle θ, heading angle ψ;
s6) quaterning the angular velocity under the carrier coordinate system b in the step S1)
Figure BDA00041306969800000214
Conversion to an angular velocity quaternion in the geographical coordinate system n>
Figure BDA00041306969800000215
By->
Figure BDA00041306969800000216
Setting a dynamic clipping threshold value, and carrying out dynamic clipping filtering on the attitude Angle Euler obtained in the step S5) to obtain a resolved attitude Angle.
Drawings
Fig. 1 is a flowchart for implementing the technical scheme of the present invention.
Detailed description of the preferred embodiments
Embodiments of the present invention are described in further detail below with reference to the accompanying drawings.
Referring to fig. 1, the method for resolving an aircraft attitude of this example is implemented as follows:
and step 1, acquiring output data of the inertial measurement unit and preprocessing the output data.
The inertial measurement unit is a device for measuring inertial information of an object and comprises a gyroscope, an accelerometer and a magnetometer sensor, wherein the gyroscope, the accelerometer and the magnetometer sensor are respectively used for measuring the 3-axis angular speed, the 3-axis acceleration and the 3-axis geomagnetic field in the environment of the object.
The specific implementation of the steps is as follows:
1.1 Zero offset correction is carried out on the 3-axis angular velocity output by the gyroscope, the angular velocity unit is converted into rad/s, and then the corrected angular velocity measured value is substituted into the quaternion imaginary part to obtain an angular velocity quaternion
Figure BDA0004130696980000031
1.2 Ellipsoid fitting is carried out on the original data output by the accelerometer and the magnetometer, zero offset and scale error parameters of the accelerometer and the magnetometer are calibrated, and the original data are corrected according to the calibrated error parameters;
1.3 The corrected acceleration and geomagnetic field data are sequentially filtered and normalized by a moving average filter, and the measured 3-axis acceleration and 3-axis geomagnetic field data are respectively converted into acceleration quaternions
Figure BDA0004130696980000032
And geomagnetic field quaternion->
Figure BDA0004130696980000033
Step 2, utilizing angular velocity quaternion
Figure BDA0004130696980000034
Predicting the current gesture to obtain a gesture quaternion +.>
Figure BDA0004130696980000035
The quaternion, which is defined as:
Figure BDA0004130696980000036
wherein (1)>
Figure BDA0004130696980000037
Is an imaginary unit which is orthogonal to each other in pairs and can be used for respectively representing the rotation of a rectangular coordinate system around a 3-axis, q 0 、q 1 、q 2 、q 3 Are all real numbers, q 0 As real component, q 1 、q 2 、q 3 Respectively 3 imaginary components, can be written in a matrix form: .
The rotational relationship of the carrier coordinate system b with respect to the geographic coordinate system n is typically in the form of a gesture quaternion
Figure BDA0004130696980000038
In the form of a representation of (a),
Figure BDA0004130696980000039
is to use the angular velocity quaternion +.>
Figure BDA00041306969800000310
For gesture quaternion->
Figure BDA00041306969800000311
The predicted results are specifically implemented as follows:
2.1 According to the quaternion of angular velocity
Figure BDA00041306969800000312
Calculating the posture change speed +.>
Figure BDA00041306969800000313
Figure BDA00041306969800000314
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA00041306969800000315
for the exact gesture quaternion calculated at the last moment, < >>
Figure BDA00041306969800000316
Representing a quaternion product;
2.2 Speed of change according to the posture
Figure BDA00041306969800000317
Predicted gesture quaternion->
Figure BDA00041306969800000318
Figure BDA0004130696980000041
Wherein T is s Sampling intervals for the sensor.
Step 3, calculating an error function f by using the acceleration and geomagnetic field data a,m And solving an error function f a,m Is a gradient direction of (c).
3.1 Using predicted gesture quaternions
Figure BDA0004130696980000042
Calculating theoretical gravitational acceleration->
Figure BDA0004130696980000043
Let the quaternion of the gravitational acceleration conversion under the geographic coordinate system be
Figure BDA0004130696980000044
Based on the vector rotation property of quaternion, the method uses the vector rotation property of the quaternion to determine the vector rotation property in the geographic coordinate system
Figure BDA0004130696980000045
Conversion to the gravitational acceleration quaternion in the carrier coordinate system b>
Figure BDA0004130696980000046
Figure BDA0004130696980000047
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0004130696980000048
for predicted gesture quaternion->
Figure BDA0004130696980000049
Conjugation of (2);
3.2 To the preprocessed geomagnetic field quaternion
Figure BDA00041306969800000410
Conversion to geomagnetic field quaternion +.>
Figure BDA00041306969800000411
Figure BDA00041306969800000412
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA00041306969800000413
for predicted gesture quaternion +.>
Figure BDA00041306969800000414
Conjugation of (2);
3.3 For geomagnetic field quaternion
Figure BDA00041306969800000415
Wherein m is x 、m y 、m z Respectively->
Figure BDA00041306969800000416
An imaginary component;
3.4 According to the characteristic that the component of geomagnetic field vector in east-west direction is 0 under ideal condition, converting the ideal geomagnetic field in geographic coordinate system into quaternion
Figure BDA00041306969800000417
Figure BDA00041306969800000418
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA00041306969800000419
the imaginary y-axis component and the real component of (2) are both 0;
3.5 According to the acceleration quaternion after pretreatment
Figure BDA00041306969800000420
And calculating to obtain the gravitational acceleration quaternion under the carrier coordinate system>
Figure BDA00041306969800000421
Constructing an acceleration error function: />
Figure BDA00041306969800000422
3.6 Geomagnetic field quaternion according to a calculated geographic coordinate system
Figure BDA00041306969800000423
Theoretical geomagnetic field in geographic coordinate System>
Figure BDA00041306969800000424
Constructing a geomagnetic field error function: />
Figure BDA00041306969800000425
3.7 According to the acceleration error function f a And geomagnetic field error function f m Constructing an error function:
f a,m ＝[f a f m ]
3.8 Calculating an error function f a,m Jacobian matrix of (a)
Figure BDA0004130696980000051
Wherein (1)>
Figure BDA0004130696980000052
Is a predicted gesture quaternion;
3.9 Solving an error function f a,m Is the gradient direction of (2)
Figure BDA0004130696980000053
Figure BDA00041306969800000518
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0004130696980000054
is the predicted pose quaternion.
Step 4, setting an adaptive step mu, and combining the adaptive step mu with an error function f a,m Is used for calculating the attitude quaternion by using a gradient descent method
Figure BDA0004130696980000055
4.1 Setting an adaptive step size mu) based on the acceleration and the angular velocity output by the inertial measurement unit in the step 1:
Figure BDA0004130696980000056
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0004130696980000057
is the calculated gesture change speed norm, alpha > 0 is the increasing ratio, ||f a I is the norm of the acceleration error function, T s Is the sensor sampling interval, mu 0 > 0 is the step initial value;
4.2 A) negative gradient direction according to the adaptive step size mu and the error function
Figure BDA0004130696980000058
Calculating attitude quaternion->
Figure BDA0004130696980000059
Figure BDA00041306969800000510
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA00041306969800000511
gradient of error function->
Figure BDA00041306969800000512
Is (are) norms of->
Figure BDA00041306969800000513
And calculating the attitude quaternion for the last moment.
Step 5, for predicted gesture quaternion
Figure BDA00041306969800000514
And the estimated gesture quaternion->
Figure BDA00041306969800000515
Fusion is performed and converted into an attitude angle.
The attitude angles are also called Euler angles, can intuitively describe the rotation relation of a carrier coordinate system relative to a geographic coordinate system, and comprise a heading angle phi and a roll angle
Figure BDA00041306969800000516
And a pitch angle theta, the specific implementation of this step is as follows:
5.1 Calculating a complementary fusion coefficient λ):
Figure BDA00041306969800000517
where β is the gyroscope device noise variance, T s Is the sensor sampling interval, μ is the calculated adaptive step size;
5.2 Predicted gesture quaternion
Figure BDA0004130696980000061
And calculated gesture quaternion->
Figure BDA0004130696980000062
Adding to obtain the gesture quaternion after complementary fusion>
Figure BDA0004130696980000063
Figure BDA0004130696980000064
5.2 Enabling the fused gesture quaternion
Figure BDA0004130696980000065
Wherein q 0 Is->
Figure BDA0004130696980000066
Real part, q of 1 、q 2 、q 3 Are respectively->
Figure BDA0004130696980000067
3 imaginary components of (a);
5.3 Will) be
Figure BDA0004130696980000068
The attitude angle Euler is converted according to the following formula:
Figure BDA0004130696980000069
and 6, performing dynamic limiting filtering on the calculated attitude angle Euler.
6.1 Quaternion of angular velocity in carrier coordinate system b)
Figure BDA00041306969800000610
Conversion to angular velocity quaternions in a geographic coordinate system n
Figure BDA00041306969800000611
Figure BDA00041306969800000612
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA00041306969800000613
is the gesture quaternion after complementation and fusion, +.>
Figure BDA00041306969800000614
Is->
Figure BDA00041306969800000615
Conjugation of (2);
6.2 Calculating absolute value |delta|= |euler-Angle of the difference between the attitude angles calculated at the last two times last I, wherein Angle last Is the attitude angle after amplitude limiting and filtering at the last moment;
6.3 Setting a dynamic clipping threshold of the attitude angle:
maximum threshold value thr max ＝K× n ωT s Wherein K > 1 is the maximum proportionality coefficient, T s Sampling intervals for the sensor;
minimum threshold value thr min ＝J× n ωT s Wherein J < 1 is the minimum scaling factor;
6.4 Comparing the absolute value |delta| of the difference between the attitude angles calculated at the last two times with a set threshold value:
if thr min ≤|Δ|≤thr max The final attitude angle is obtained: angle = Euler;
if |delta| > thr max Updating the attitude angle Euler converted in 5.3) to obtain a final attitude angle:
Figure BDA0004130696980000071
wherein Angle is last Is the attitude angle after amplitude limiting and filtering at the last moment;
if |delta| < thr min Updating the attitude Angle to obtain a final attitude angle=angle last 。
The above description is only one specific example of the invention and does not constitute any limitation of the invention, and it will be apparent to those skilled in the art that various modifications and changes in form and details may be made without departing from the principles, construction of the invention, but these modifications and changes based on the idea of the invention are still within the scope of the claims of the invention.

Claims (11)

1. A method of resolving an aircraft attitude, comprising the steps of:
s1) obtaining output data of an inertial measurement unit, and performing preprocessing of filtering correction to obtain an angular velocity quaternion under a carrier coordinate system b
Figure FDA0004130696970000011
Acceleration quaternion->
Figure FDA0004130696970000012
Geomagnetic field quaternion->
Figure FDA0004130696970000013
S2) quaterning the angular velocity
Figure FDA0004130696970000014
Substituting the quaternion differential equation to predict the attitude quaternion of the carrier coordinate system b relative to the geographic coordinate system n>
Figure FDA0004130696970000015
S3) utilizing predicted gesture quaternions
Figure FDA0004130696970000016
Calculating theoretical gravitational acceleration->
Figure FDA0004130696970000017
Simultaneously, the theoretical geomagnetic field +.A. under the geographic coordinate system is calculated by utilizing the characteristic that the geomagnetic field has no component in the east-west direction>
Figure FDA0004130696970000018
Obtaining an error function f through cross product operation of a theoretical vector and an actual measurement vector a,m And solve f a,m Is a gradient direction of (2);
s4) setting an adaptive step size mu based on the acceleration and angular velocity factors output by the inertial measurement unit in the step S1), and combining the adaptive step size mu with an error function f a,m Is used for calculating attitude quaternion in negative gradient direction
Figure FDA0004130696970000019
S5) quaterning the gesture of the carrier coordinate system b predicted in the step S2) relative to the geographic coordinate system n
Figure FDA00041306969700000110
Gesture quaternion calculated with step S4)>
Figure FDA00041306969700000111
Complementary fusion is carried out to obtain a fused gesture quaternion +.>
Figure FDA00041306969700000112
And converts it into an attitude angle Euler, which includes a roll angle +.>
Figure FDA00041306969700000113
Pitch angle θ, heading angle ψ;
s6) quaterning the angular velocity under the carrier coordinate system b in the step S1)
Figure FDA00041306969700000114
Conversion to an angular velocity quaternion in the geographical coordinate system n>
Figure FDA00041306969700000115
By->
Figure FDA00041306969700000116
Setting a dynamic clipping threshold value, and carrying out dynamic clipping filtering on the attitude Angle Euler obtained in the step S5) to obtain a resolved attitude Angle.
2. The method according to claim 1, wherein the preprocessing of the filter correction of the acquired inertial measurement unit output data in step S1) is implemented as follows:
zero offset correction is carried out on the 3-axis angular velocity output by the gyroscope, an angular velocity unit is converted into rad/s, and then the corrected angular velocity measured value is substituted into the quaternion imaginary part to obtain an angular velocity quaternion
Figure FDA00041306969700000117
Performing ellipsoidal fitting on the original data output by the accelerometer and the magnetometer, calibrating zero offset and scale error parameters of the original data, and correcting the original data according to the calibrated error parameters; sequentially filtering and normalizing the corrected data by using a moving average filter, and respectively converting the measured 3-axis acceleration and 3-axis geomagnetic field data into acceleration quaternions
Figure FDA0004130696970000021
And geomagnetic field quaternion->
Figure FDA0004130696970000022
3. The method according to claim 1, wherein in step S2) the quaternion is based on angular velocity
Figure FDA00041306969700000225
Predicting the carrier coordinate system b relative toGesture quaternion of geographic coordinate system n>
Figure FDA0004130696970000023
The formula is as follows:
Figure FDA0004130696970000024
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA0004130696970000025
for the speed of posture change, +.>
Figure FDA0004130696970000026
For the accurate attitude quaternion calculated at the previous moment, T s For gyroscope sampling interval, +.>
Figure FDA0004130696970000027
Representing a quaternion product.
4. The method of claim 1, wherein the predicted pose quaternion is utilized in step S3)
Figure FDA0004130696970000028
Calculating theoretical gravitational acceleration->
Figure FDA0004130696970000029
The realization is as follows:
let gravity acceleration quaternion under geographic coordinate system be
Figure FDA00041306969700000210
From the vector rotation property of quaternion, the system of geographic coordinates
Figure FDA00041306969700000211
Conversion to weights in the carrier coordinate systemForce acceleration quaternion->
Figure FDA00041306969700000212
Figure FDA00041306969700000213
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA00041306969700000214
for predicted gesture quaternion->
Figure FDA00041306969700000215
Is a conjugate of (c).
5. The method according to claim 1, wherein the theoretical geomagnetic field in the geographic coordinate system is calculated in step S3) using a characteristic that the geomagnetic field has no component in an east-west direction
Figure FDA00041306969700000216
The realization is as follows:
s31) the geomagnetic field quaternion to be preprocessed
Figure FDA00041306969700000217
Geomagnetic field quaternion converted into geographic coordinate system>
Figure FDA00041306969700000218
Figure FDA00041306969700000219
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA00041306969700000220
for predicted gesture quaternion +.>
Figure FDA00041306969700000221
Is->
Figure FDA00041306969700000222
Conjugation of (2);
s32) order
Figure FDA00041306969700000223
Wherein m is x 、m y 、m z Respectively->
Figure FDA00041306969700000224
Triaxial components of the imaginary part;
s33) converting the ideal geomagnetic field in the geographic coordinate system into quaternion according to the characteristic that the component of the geomagnetic field vector in the east-west direction is 0 under the ideal condition
Figure FDA0004130696970000031
Figure FDA0004130696970000032
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA0004130696970000033
the imaginary y-axis and the real-axis of (2) are both 0.
6. The method according to claim 1, characterized in that the error function f is calculated in step S3) a,m The implementation is as follows:
s34) according to the preprocessed acceleration quaternion
Figure FDA0004130696970000034
And calculating to obtain a gravitational acceleration quaternion under the carrier coordinate system
Figure FDA0004130696970000035
Constructing an acceleration error function: />
Figure FDA0004130696970000036
S35) geomagnetic field quaternion according to the calculated geographic coordinate system
Figure FDA0004130696970000037
Theoretical geomagnetic field in geographic coordinate System>
Figure FDA0004130696970000038
Constructing a geomagnetic field error function: />
Figure FDA0004130696970000039
S36) constructing an error function from the acceleration error function and the geomagnetic field error function:
f a,m ＝[f a f m ]
wherein f a As a function of acceleration error, f m As a function of geomagnetic field errors.
7. The method according to claim 1, characterized in that the error function f is solved in step S3) a,m Is the gradient direction of (2)
Figure FDA00041306969700000310
The formula is as follows:
Figure FDA00041306969700000311
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA00041306969700000312
is an error function f a,m Jacobian matrix, ">
Figure FDA00041306969700000313
Is the predicted pose quaternion.
8. The method according to claim 1, characterized in that in step S4) an adaptation step μ is set, the formula being as follows:
Figure FDA00041306969700000314
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA00041306969700000315
is the norm of the calculated attitude change speed, alpha > 0 is the increasing ratio, ||f a I is the norm of the acceleration error function, T s Is the sensor sampling interval, mu 0 > 0 is the step initial value.
9. The method according to claim 1, characterized in that in step S4) the adaptive step μ is based on an error function f a,m Is used for calculating attitude quaternion in negative gradient direction
Figure FDA0004130696970000041
The formula is as follows:
Figure FDA0004130696970000042
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA0004130696970000043
gradient of error function->
Figure FDA0004130696970000044
Is (are) norms of->
Figure FDA0004130696970000045
And calculating the attitude quaternion for the last moment.
10. The method of claim 1, wherein step S5) is to quaternion the predicted pose
Figure FDA0004130696970000046
And the calculated gesture quaternion->
Figure FDA0004130696970000047
Complementary fusion is carried out, and the complementary fusion is converted into an attitude angle, so that the following is realized:
s51) the predicted gesture quaternion
Figure FDA0004130696970000048
And the calculated gesture quaternion->
Figure FDA0004130696970000049
Adding to obtain the gesture quaternion after complementary fusion>
Figure FDA00041306969700000410
Figure FDA00041306969700000411
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA00041306969700000412
is the complementary fusion coefficient, beta is the gyroscope device noise variance, T s Is the sensor sampling interval, μ is the calculated adaptive step size;
s52) enabling the fused gesture quaternion
Figure FDA00041306969700000413
Wherein q 0 Is->
Figure FDA00041306969700000414
The real part of (2)，q 1 、q 2 、q 3 Are respectively->
Figure FDA00041306969700000415
Is a virtual component of (a);
s53) will
Figure FDA00041306969700000416
The attitude angle Euler is converted according to the following formula:
Figure FDA00041306969700000417
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA00041306969700000418
is the roll angle, θ is the pitch angle, and ψ is the heading angle.
11. The method of claim 1, wherein step S6) performs dynamic clipping filtering on the calculated attitude angle, as follows:
s61) quaterning the angular velocity under the carrier coordinate system b
Figure FDA0004130696970000051
Conversion to an angular velocity quaternion in the geographical coordinate system n>
Figure FDA0004130696970000052
Figure FDA0004130696970000053
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure FDA0004130696970000054
is the gesture quaternion after complementation and fusion, +.>
Figure FDA0004130696970000055
Is->
Figure FDA0004130696970000056
Conjugation of (2);
s62) setting a gesture angle dynamic clipping maximum threshold thr max ＝K× n ωT and minimum threshold thr min ＝J× n ωT s
Wherein, parameters K & gt1 and J & lt 1 are respectively set maximum proportionality coefficient and minimum proportionality coefficient;
s63) dynamic clipping maximum threshold thr by using set attitude angle max And a minimum threshold thr min And carrying out dynamic limiting filtering on the obtained attitude angle Euler, wherein the formula is as follows:
Figure FDA0004130696970000057
wherein Angle is last Is the attitude Angle calculated at the previous time, delta=euler-Angle last The difference between the attitude angles calculated for the last two moments.
CN202310259439.5A 2023-03-16 2023-03-16 Aircraft attitude calculation method Pending CN116380054A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310259439.5A CN116380054A (en) 2023-03-16 2023-03-16 Aircraft attitude calculation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310259439.5A CN116380054A (en) 2023-03-16 2023-03-16 Aircraft attitude calculation method

Publications (1)

Publication Number Publication Date
CN116380054A true CN116380054A (en) 2023-07-04

Family

ID=86972323

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310259439.5A Pending CN116380054A (en) 2023-03-16 2023-03-16 Aircraft attitude calculation method

Country Status (1)

Country Link
CN (1) CN116380054A (en)

Similar Documents

Publication Publication Date Title
CN106679649B (en) Hand motion tracking system and method
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
CN101726295B (en) Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation
CN109238262B (en) Anti-interference method for course attitude calculation and compass calibration
CN107270893B (en) Lever arm and time asynchronous error estimation and compensation method for real estate measurement
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
KR101922700B1 (en) Method and Apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor
CN109425339B (en) Ship heave error compensation method considering lever arm effect based on inertia technology
CN106370178B (en) Attitude measurement method and device of mobile terminal equipment
CN107063254B (en) Gesture resolving method for gyros and geomagnetic combination
CN108731676B (en) Attitude fusion enhanced measurement method and system based on inertial navigation technology
CN109682377A (en) A kind of Attitude estimation method based on the decline of dynamic step length gradient
CN106153069B (en) Attitude rectification device and method in autonomous navigation system
WO2020164206A1 (en) Calibration method for gravity gradiometer of rotating accelerometer
CN111189442B (en) CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method
CN106403952A (en) Method for measuring combined attitudes of Satcom on the move with low cost
CN116147624B (en) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
CN113188505A (en) Attitude angle measuring method and device, vehicle and intelligent arm support
CN115046539A (en) Dynamic calibration method for MEMS electronic compass
CN113175926B (en) Self-adaptive horizontal attitude measurement method based on motion state monitoring
CN111141285B (en) Aviation gravity measuring device
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN114994352B (en) High-speed rotation guided projectile rotation speed measuring method
CN116380054A (en) Aircraft attitude calculation method
CN115523919A (en) Nine-axis attitude calculation method based on gyro drift optimization

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