Method for calculating aircraft attitude

Abstract

The present invention discloses a method for calculating the attitude of an aircraft, which mainly solves the defects of low precision and angle jump of the existing attitude fusion method. The implementation scheme is as follows: the output data of the inertial measurement unit is obtained and pre-processed by filtering and correction to obtain the angular velocity quaternion, the acceleration quaternion and the geomagnetic field quaternion; the angular velocity quaternion is substituted into the quaternion differential equation to obtain the predicted attitude quaternion; the error function is calculated by using the acceleration quaternion and the geomagnetic field quaternion, and its gradient direction is solved; an adaptive step length is set, and the attitude quaternion is calculated by combining the step length and the gradient direction; the predicted attitude quaternion and the calculated attitude quaternion are complementary and fused to obtain the fused attitude quaternion, and the fused attitude quaternion is converted into an attitude angle; the dynamic limit threshold is set by using the angular velocity quaternion, and the attitude angle is dynamically limited and filtered to obtain the final attitude angle. The present invention reduces the angle jump, improves the accuracy of the attitude angle, and can be used in an inertial navigation system.

Description

Technical Field

[0001] The present invention belongs to the technical field of data processing, and particularly relates to a method for solving the attitude of an aircraft, which can be used in an inertial navigation system. Background Art

[0002] Precisely obtaining the attitude of an object is a prerequisite for aircraft inertial navigation. The inertial measurement unit has the advantages of small size and high cost performance, and is a commonly used sensor for obtaining attitude data. Among them, the nine-axis inertial measurement unit consists of an accelerometer, a gyroscope, and a magnetometer. The gyroscope has a relatively accurate measurement result in the scenario of rapid angle change, but there will be obvious cumulative errors after integration for a period of time. The accelerometer and magnetometer have large measurement noises, and the measurement results of the accelerometer are easily interfered by the motion acceleration when the object is accelerating or decelerating rapidly, and the magnetometer is easily interfered by the magnetic materials in the nearby environment. Therefore, the key to attitude solution lies in how to fuse the data measured by the two to obtain a more accurate result.

[0003] Existing attitude solution methods based on inertial measurement units include complementary filtering method, extended Kalman filtering method, and gradient descent method. Among them:

[0004] The main idea of the complementary filtering method is to instantaneously correct the result of integrating the angular velocity using acceleration and geomagnetic field data. In view of the poor dynamic response characteristics of the gyroscope in the low-frequency band, a high-pass filter is used to suppress noise; the accelerometer and magnetometer have poor dynamic response characteristics in the high-frequency band, and a low-pass filter is used to suppress noise, so as to realize the complementarity of the dynamic response characteristics of the sensors. This method has strong practicability, but the PI correction parameters are difficult to adjust and are not applicable to systems with high-precision requirements.

[0005] The extended Kalman filtering method mainly follows two steps: prediction and update. In the prediction stage, the attitude quaternion and drift deviation of the gyroscope are selected to construct a state equation to generate a predicted attitude; in the update stage, the orthogonalization method is used to obtain the attitude quaternion from the acceleration and geomagnetic field data as a measurement vector, and the predicted value and the measurement value are fused through the extended Kalman filter to accurately represent the attitude of the aircraft. However, since the matrix operation involves a large amount of calculation and has high requirements for the operation speed and accuracy of the processor, it is not applicable to the application of low-cost aircraft.

[0006] The main idea of the gradient descent method is to obtain the optimal estimated value of the attitude by continuously iterating the error function along the negative gradient direction of the attitude error. Among them, the error function is generally calculated from acceleration and geomagnetic field data. The gradient descent method can eliminate noise interference to a certain extent, but it is necessary to design a suitable step size parameter. If the step size is too large, the solved attitude angle will diverge, and if the step size is too small, the convergence effect will be poor.

[0007] The attitude fusion method based on gradient descent was proposed by Gao Yi, Li Donghang, and Guo Piao in the Journal of Electronic Design Engineering in 2021. This method uses gradient descent to fuse acceleration and geomagnetic field data to solve a set of attitude quaternions. At the same time, another set of attitude quaternions is estimated by integrating the angular velocity output by the gyroscope. Finally, the two sets of attitude quaternions are complementarily fused and converted into attitude angles. This method solves the gimbal lock problem and has the advantages of not requiring the acquisition of the local geomagnetic inclination and fast convergence speed. However, due to its fixed step size, it is difficult to ensure the accuracy of attitude angles in both low-speed and high-speed motion states at the same time, resulting in a large average positioning error in the inertial navigation system for calculating the trajectory of the aircraft. Summary of the Invention

[0008] The purpose of the present invention is to provide a method for calculating the attitude of an aircraft to reduce the average positioning error of the navigation system in calculating the trajectory of the aircraft and improve the positioning accuracy of the inertial navigation system of the aircraft in view of the defects of the above gradient descent attitude fusion method.

[0009] To achieve the above purpose, the technical solution of the present invention includes the following steps:

[0010] S1) Obtain the output data of the inertial measurement unit and perform preprocessing of filtering and correction to obtain the angular velocity quaternion in the body coordinate system b Acceleration quaternion Geomagnetic field quaternion

[0011] S2) Substitute the angular velocity quaternion into the quaternion differential equation to predict the attitude quaternion of the body coordinate system b relative to the geographic coordinate system n

[0012] S3) Use the predicted attitude quaternion to calculate the theoretical gravitational acceleration At the same time, use the characteristic that there is no component of the geomagnetic field in the east-west direction to calculate the theoretical geomagnetic field in the geographic coordinate system Obtain the error function f through the cross product operation of the theoretical vector and the measured vector a,m , and solve the gradient direction of f a,m ;

[0013] S4) Based on the acceleration and angular velocity factors output by the inertial measurement unit in step S1), set the adaptive step size μ, and combine the adaptive step size μ with the negative gradient direction of the error function f a,m to calculate the attitude quaternion

[0014] S5) Compare the attitude quaternion predicted in step S2) for the body coordinate system b relative to the geographic coordinate system n with the attitude quaternion Complementary fusion is performed to obtain the attitude quaternion after fusion. And convert it to the attitude angle Euler, which includes the roll angle pitch angle θ, and yaw angle ψ;

[0015] S6) Convert the angular velocity quaternion in the vehicle coordinate system b in step S1) to the angular velocity quaternion in the geographical coordinate system n Using Set the dynamic limit threshold, and perform dynamic limit filtering on the attitude angle Euler obtained in step S5) to obtain the calculated attitude angle Angle. Description of the Drawings

[0016] Figure 1 is the flowchart for implementing the technical solution of the present invention. Specific Embodiment

[0017] The following further elaborates on the embodiments of the present invention with reference to the accompanying drawings.

[0018] Refer to Figure 1 , the method for solving the attitude of the aircraft in this embodiment is implemented as follows:

[0019] Step 1: Obtain the output data of the inertial measurement unit and perform preprocessing.

[0020] The inertial measurement unit is a device for measuring the inertial information of an object, which includes a gyroscope, an accelerometer, and a magnetometer sensor, and is respectively used to measure the 3-axis angular velocity, 3-axis acceleration of the object, and the 3-axis geomagnetic field in the environment.

[0021] The specific implementation of this step is as follows:

[0022] 1.1) Perform zero-bias correction on the 3-axis angular velocity output by the gyroscope, convert the angular velocity unit to rad / s, and then substitute the corrected angular velocity measurement value into the imaginary part of the quaternion to obtain the angular velocity quaternion

[0023] 1.2), perform ellipsoid fitting on the original data output by the accelerometer and magnetometer, calibrate their zero-bias and scale error parameters, and correct their original data according to the calibrated error parameters;

[0024] 1.3), filter and normalize the corrected acceleration and geomagnetic field data in sequence using a moving average filter, and convert the measured 3-axis acceleration and 3-axis geomagnetic field data into the acceleration quaternion and the geomagnetic field quaternion

[0025] Step 2: Use the angular velocity quaternion to predict the current attitude and obtain the attitude quaternion

[0026] The quaternion is defined as: where are imaginary unit vectors that are pairwise orthogonal to each other and can be used to represent the rotation of a rectangular coordinate system about the 3-axis respectively. q0, q1, q2, and q3 are all real numbers, q0 is the real part component, and q1, q2, and q3 are the three imaginary part components respectively, and it can be written in matrix form.

[0027] The rotation relationship of the body coordinate system b relative to the geographic coordinate system n is usually represented in the form of the attitude quaternion and is the result predicted by using the angular velocity quaternion for the attitude quaternion The specific implementation is as follows:

[0028] 2.1) Calculate the attitude change rate based on the angular velocity quaternion

[0029]

[0030] where is the exact attitude quaternion calculated at the previous moment, represents the quaternion product;

[0031] 2.2) Predict the attitude quaternion based on the attitude change rate

[0032]

[0033] where T s is the sensor sampling interval.

[0034] Step 3, calculate the error function f a,m using the acceleration and geomagnetic field data, and solve for the gradient direction of the error function f a,m

[0035] 3.1) Deduce the theoretical gravitational acceleration using the predicted attitude quaternion

[0036] Let the quaternion obtained by transforming the gravitational acceleration in the geographic coordinate system be

[0037] According to the vector rotation property of the quaternion, transform in the geographic coordinate system into the gravitational acceleration quaternion

[0038] ​

[0039] Among them, is the predicted attitude quaternion conjugate;

[0040] 3.2) Convert the preprocessed geomagnetic quaternion into the geomagnetic quaternion

[0041]

[0042] Among them, is the predicted attitude quaternion, conjugate;

[0043] 3.3) Let the geomagnetic quaternion where m x 、m y 、m z are respectively imaginary components;

[0044] 3.4) According to the characteristic that the component of the geomagnetic vector in the east-west direction is 0 under ideal conditions, convert the ideal geomagnetic field in the geographic coordinate system into a quaternion

[0045]

[0046] Among them, the imaginary part y-axis component and the real part are both 0;

[0047] 3.5) According to the preprocessed acceleration quaternion and the calculated gravity acceleration quaternion in the carrier coordinate system, construct the acceleration error function:

[0048] 3.6) According to the calculated geomagnetic quaternion of the geographic coordinate system the theoretical geomagnetic field in the geographic coordinate system, construct the geomagnetic error function:

[0049] a and the geomagnetic error function f m , construct the error function:

[0050] f a,m =[f a f m

[0051] 3.8) Calculate the error function f a,m ​Jacobian matrix wherein, is the predicted attitude quaternion;

[0052] 3.9) Solve the gradient direction of the error function f a,m

[0053]

[0054] wherein, is the predicted attitude quaternion.

[0055] Step 4, set the adaptive step size μ, combine the adaptive step size μ with the negative gradient direction of the error function f a,m and use the gradient descent method to deduce the attitude quaternion

[0056] 4.1) Based on the acceleration and angular velocity output by the inertial measurement unit in Step 1, set the adaptive step size μ:

[0057]

[0058] wherein, is the norm of the calculated attitude change speed, α > 0 is the increase ratio, ||f a || is the norm of the acceleration error function, T s is the sensor sampling interval, μ0 > 0 is the initial value of the step size;

[0059] 4.2) Deduce the attitude quaternion according to the adaptive step size μ and the negative gradient direction of the error function

[0060]

[0061] wherein, is the gradient of the error function is the attitude quaternion calculated at the previous moment.

[0062] Step 5, fuse the predicted attitude quaternion and the deduced attitude quaternion and convert them into the attitude angle angle.

[0063] The attitude angle, also known as the Euler angle, can intuitively describe the rotational relationship between the carrier coordinate system and the geographic coordinate system. A set of attitude angles includes the heading angle ψ, the roll angle and the pitch angle θ. The specific implementation of this step is as follows:

[0064] 5.1) Calculate the complementary fusion coefficient λ: ​​​

[0065]

[0066] where β is the gyroscope device noise variance, T s is the sensor sampling interval, and μ is the calculated adaptive step size;

[0067] 5.2) Add the predicted attitude quaternion to the calculated attitude quaternion to obtain the complementary fused attitude quaternion

[0068]

[0069] 5.2) Let the fused attitude quaternion where q0 is the real part of and q1, q2, q3 are the 3 imaginary components of

[0070] 5.3) Convert to the attitude angle Euler according to the following formula:

[0071]

[0072] Step 6, perform dynamic limit filtering on the calculated attitude angle Euler.

[0073] 6.1) Convert the angular velocity quaternion in the body coordinate system b to the angular velocity quaternion

[0074]

[0075] where, is the complementary fused attitude quaternion, is the conjugate of;

[0076] 6.2) Calculate the absolute value of the difference between the attitude angles calculated at the two most recent moments |Δ| = |Euler - Angle last |, where Angle last is the attitude angle after limit filtering at the previous moment;

[0077] 6.3) Set the attitude angle dynamic limit threshold:

[0078] Maximum threshold: thr max = K × n ωT s , where K > 1 is the maximum proportionality coefficient, and T s is the sensor sampling interval;

[0079] Minimum threshold: thr min = J × n ωT s , where J < 1 is the minimum proportionality coefficient;

[0080] 6.4) Compare the absolute value |Δ| of the difference between the attitude angles calculated at the two most recent moments with the set threshold:

[0081] If thr min ≤ |Δ| ≤ thr max , then obtain the final attitude angle: Angle = Euler;

[0082] If |Δ| > thr max , then update the attitude angle Euler converted in 5.3) to obtain the final attitude angle: where Angle last is the attitude angle after amplitude limiting filtering at the previous moment;

[0083] If |Δ| < thr min , then update the attitude angle to obtain the final attitude angle Angle = Angle last .

[0084] The above description is only a specific example of the present invention and does not constitute any limitation to the present invention. Obviously, for professionals in the field, after understanding the content and principle of the present invention, various modifications and changes in form and details may be made without departing from the principle and structure of the present invention. However, these corrections and changes based on the idea of the present invention are still within the protection scope of the claims of the present invention.

Claims

1. A method for calculating the attitude of an aircraft, characterized in that, It includes the following steps: S1) Obtain the output data of the inertial measurement unit and perform preprocessing of filtering and correction to obtain the angular velocity quaternion in the carrier coordinate system b Acceleration quaternion Geomagnetic field quaternion S2) Substitute the angular velocity quaternion into the quaternion differential equation to predict the attitude quaternion of the body coordinate system b relative to the geographical coordinate system n S3) Use the predicted attitude quaternion to calculate the theoretical gravitational acceleration At the same time, utilize the characteristic that there is no component of the geomagnetic field in the east-west direction to calculate the theoretical geomagnetic field in the geographical coordinate system Obtain the error function f through the cross product operation of the theoretical vector and the measured vector a,m , and solve for f a,m of the gradient direction; S4) Set an adaptive step size μ based on the acceleration and angular velocity factors output by the inertial measurement unit in step S1), and deduce the attitude quaternion by combining the adaptive step size μ with the negative gradient direction of the error function f a,m ​ S5) Quaternion of the attitude of the vehicle coordinate system b relative to the geographic coordinate system n predicted in step S2 is complementarily fused with the quaternion of the attitude calculated in step S4 to obtain the fused quaternion of the attitude and convert it into Euler attitude angles, which include roll angle pitch angle θ, and heading angle ψ; S6) Convert the angular velocity quaternion in the vehicle coordinate system b in step S1) to the angular velocity quaternion in the geographic coordinate system n Use Set the dynamic limit threshold, perform dynamic limit filtering on the attitude angle Euler obtained in step S5), and obtain the calculated attitude angle Angle.

2. The method according to claim 1, wherein In step S1), preprocessing of filtering and correcting the output data of the inertial measurement unit is implemented as follows: Perform zero-bias correction on the three-axis angular velocities output by the gyroscope, convert the angular velocity unit to rad / s, and then substitute the corrected angular velocity measurement value into the imaginary part of the quaternion to obtain the angular velocity quaternion Perform ellipsoidal fitting on the original data output by the accelerometer and magnetometer, calibrate their zero bias and scale error parameters, and correct their original data according to the calibrated error parameters; then use a moving average filter to perform filtering and normalization on the corrected data in sequence, and convert the measured 3-axis acceleration and 3-axis geomagnetic field data into acceleration quaternions and geomagnetic field quaternions 3. The method according to claim 1, wherein In step S2), according to the angular velocity quaternion predict the attitude quaternion of the vehicle coordinate system b relative to the geographic coordinate system n The formula is as follows: Among them, is the attitude change speed, is the precise attitude quaternion calculated at the previous moment, T s is the gyroscope sampling interval, represents the quaternion product.

4. The method according to claim 1, characterized in that, In step S3), the predicted attitude quaternion is used to calculate the theoretical gravitational acceleration The implementation is as follows: Let the quaternion of gravitational acceleration in the geodetic coordinate system be According to the vector rotation property of quaternions, the is converted into the quaternion of gravitational acceleration in the body coordinate system wherein, is the predicted attitude quaternion 's conjugate.

5. The method according to claim 1, characterized in that, In step S3), the theoretical geomagnetic field in the geographical coordinate system is calculated by using the characteristic that there is no component of the geomagnetic field in the east-west direction. The implementation is as follows: S31) Convert the preprocessed geomagnetic quaternion into the geomagnetic quaternion in the geographic coordinate system wherein, is the predicted attitude quaternion, is 's conjugate; S32) Let where m x , m y , m z are respectively the three-axis components of the imaginary part; S33) According to the characteristic that the component of the geomagnetic field vector in the east-west direction is 0 under ideal conditions, the ideal geomagnetic field in the geographic coordinate system is converted into a quaternion Among them, the imaginary part's y-axis and the real part are both 0.

6. The method according to claim 1, characterized in that In step S3), the error function f is calculated a,m , and the implementation is as follows: S34) According to the preprocessed acceleration quaternion and the calculated gravity acceleration quaternion in the vehicle coordinate system construct an acceleration error function: S35) Geomagnetic quaternion of the calculated geographic coordinate system Theoretical geomagnetic field in the geographic coordinate system Construct the geomagnetic field error function: S36) Construct an error function according to the acceleration error function and the geomagnetic field error function: f a,m = [f a f m ​ Among them, f a is the acceleration error function, and f m is the geomagnetic field error function.

7. The method according to claim 1, characterized in that, Solving the gradient direction of the error function f in step S3 a,m is as follows: The formula is as follows: Among them, is the Jacobian matrix of the error function f a,m , and is the predicted attitude quaternion.

8. The method according to claim 1, characterized in that In step S4), an adaptive step size μ is set, and the formula is as follows: where, is the norm of the calculated attitude change rate, α > 0 is the increase ratio, ||f a || is the norm of the acceleration error function, T s is the sensor sampling interval, and μ0 > 0 is the initial value of the step size.

9. The method according to claim 1, wherein In step S4), the attitude quaternion is calculated based on the adaptive step size μ and the negative gradient direction of the error function f a,m as follows: The formula is as follows: wherein, is the norm of the error function gradient , is the attitude quaternion calculated at the previous moment.

10. The method according to claim 1, characterized in that, Step S5) Complementary fusion of the predicted attitude quaternion and the calculated attitude quaternion is performed and converted into attitude angles as follows: S51) Add the predicted attitude quaternion and the calculated attitude quaternion to obtain the attitude quaternion after complementary fusion Among them, is the complementary fusion coefficient, β is the gyroscope device noise variance, T s is the sensor sampling interval, and μ is the calculated adaptive step size; S52) Let the fused attitude quaternion where q0 is the real part of, and q1, q2, q3 are respectively the imaginary components of; S53) Convert to the attitude angle Euler according to the following formula: where, is the roll angle, θ is the pitch angle, and ψ is the heading angle.

11. The method according to claim 1, wherein In step S6), dynamic amplitude limiting filtering is performed on the calculated attitude angles, and the implementation is as follows: S61) Convert the angular velocity quaternion in the body coordinate system b to the angular velocity quaternion in the navigation coordinate system n Among them, is the attitude quaternion after complementary fusion, is 's conjugate; S62) Set the maximum threshold thr for dynamic limit of the attitude angle max = K × n ωT and the minimum threshold thr min = J × n ωT s Among them, the parameters K > 1 and J < 1 are the set maximum proportionality coefficient and minimum proportionality coefficient respectively; S63) Utilize the dynamically set maximum threshold thr max and minimum threshold thr min to perform dynamic limit filtering on the obtained Euler attitude angle. The formula is as follows: Among them, Angle last is the attitude angle calculated at the previous moment, and Δ = Euler - Angle last is the difference between the attitude angles calculated at the two most recent moments.

Images