A sequential filtering method for data fusion of attitude reference system of rotary-wing UAV

Abstract

This invention relates to a sequential filtering method for data fusion in a rotorcraft unmanned aerial vehicle (UAV) attitude reference system, belonging to the field of UAV attitude estimation. In this invention, a low-pass filter is used to process the acquired sensor data to obtain the initialized horizontal attitude angle and heading angle, thus initializing the attitude reference system. During the calculation process, Euler angle differential equations are used to construct the system's state transition equations, defining relevant state variables and obtaining the state transition matrix. Measurement equations are constructed based on the acceleration output and magnetometer output of the carrier's observations, yielding the corresponding observation matrix. Following the steps required for Kalman filtering, the state variables required during the Kalman filter's operation are mapped to the system, and calculations are performed using the Kalman filter data update method to complete the calculation derivation of the corresponding quantities, thus realizing the sequential processing method of Kalman filtering. Compared to traditional Kalman filtering algorithms, this algorithm implements Kalman filtering sequentially, requiring only partial data calculations during attitude updates, and does not involve matrix operations throughout the process, thereby improving the algorithm's computational efficiency.

Description

Technical Field

[0001] This invention relates to the field of UAV attitude estimation, and in particular to a Kalman filter data fusion method implemented in a sequential manner for an attitude reference system. Background Technology

[0002] With the rapid development of UAV technology, rotary-wing UAVs have been widely used in many fields, thus placing stricter requirements on their flight stability and attitude measurement accuracy. As the core of flight control and navigation attitude calculation for rotary-wing UAVs, the attitude reference system (ARS) plays a crucial role, and its data fusion method is of paramount importance. In the field of rotary-wing UAV attitude estimation, the ARS typically consists of a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer. Data fusion methods can provide accurate attitude angles for the UAV in real time.

[0003] In an attitude reference system, when a UAV is stationary or in uniform motion, the vehicle's acceleration is primarily driven by gravitational acceleration. In this state, the outputs of a three-axis accelerometer and a three-axis magnetometer can be used to estimate the horizontal attitude and heading angle. Accelerometer and magnetometer calculations do not involve integration, thus avoiding accumulated errors and achieving high accuracy in static scenarios. However, when the UAV maneuvers, the three-axis accelerometer measurements simultaneously include gravitational acceleration and external acceleration, rendering gravity-vector-based horizontal attitude calculations ineffective. Therefore, a three-axis gyroscope is needed as the core data source for dynamic attitude calculation. The gyroscope measures the vehicle's angular velocity in real time, providing high-frequency, continuous attitude change information within a short period, meeting the real-time requirements of dynamic scenarios. However, gyroscope measurement noise accumulates during integration, causing attitude estimation drift. Therefore, relying solely on the gyroscope for extended periods can lead to attitude divergence. A fusion algorithm, combining accelerometer and magnetometer corrections, is needed to suppress drift and maintain long-term stability of the attitude estimation.

[0004] Existing data fusion technologies for attitude and bearing reference systems (AHRS) mainly include complementary filtering and Kalman filtering. Complementary filtering uses fixed parameters to fuse sensor data, which fails to effectively balance the weights of each sensor, making it difficult to guarantee attitude accuracy under complex conditions. The Kalman filtering algorithm, as the optimal recursive nonlinear minimum variance estimation system, is widely used in various commercially available AHRS products. However, the Kalman filtering algorithm involves a large number of matrix operations during operation, placing high demands on computational resources. Therefore, this invention proposes a data fusion algorithm that implements Kalman filtering in a sequential manner, improving computational efficiency to meet practical needs. Summary of the Invention

[0005] The purpose of this invention is to provide a data fusion algorithm that can be used in the attitude reference system of rotary-wing UAVs. This algorithm implements Kalman filtering in a sequential manner, and only needs to perform calculations on a portion of the data during attitude updates. The entire process does not involve matrix operations, which improves the computational efficiency of the algorithm.

[0006] This invention discloses a sequential data fusion method applicable to the attitude reference system of a rotary-wing unmanned aerial vehicle (UAV), comprising: Step 1: acquiring the outputs of a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer after processing with a low-pass filter; Step 2: initializing the attitude reference system, setting the roll angle γ and pitch angle θ to zero, and calculating the heading angle ψ from the magnetometer output and the magnetic declination of the local geographical location; Step 3: constructing the system's state transition equation from the differential equation of Euler angles, defining state variables, and obtaining the state transition matrix; Step 4: calculating the observables of the carrier based on the accelerometer output and the magnetometer output, obtaining... and Step 5: Construct the measurement equations to obtain the corresponding observation matrix; Step 6: According to the required steps of Kalman filtering, match the state variables required during the operation of the Kalman filter with the system; Step 7: Perform calculations and organization according to the Kalman filter data update method, complete the calculation derivation of the corresponding quantities, and realize the sequential processing of Kalman filtering.

[0007] In step one, a low-pass filter needs to be designed based on the actual rotor rotation to reduce the impact of vibration noise on the accelerometer output and to collect the filtered data from the three-axis accelerometer, three-axis gyroscope, and three-axis magnetometer in real time.

[0008] In step two, the horizontal attitude roll angle γ and pitch angle θ are initialized to zero, the magnetic declination Δψ of the local geographical location is calculated, and the heading angle can be output from the triaxial magnetometer. The calculation yielded the following:

[0009]

[0010] The differential equation for the Euler angles in step three is as follows:

[0011]

[0012] Given the limited horizontal maneuverability of rotary-wing UAVs, the above model is simplified as follows:

[0013]

[0014] Define the state variable as:

[0015] X=[γ θ ψ ε x ε y ε z] T

[0016] In the formula, ε x ε y and ε z These are the zero bias values ​​of the three-axis gyroscope.

[0017] Then the state equation can be It can be expanded as follows:

[0018]

[0019] In step four, an observation measurement needs to be constructed based on the outputs of the triaxial accelerometer and triaxial magnetometer. The horizontal attitude can be determined by the accelerometer measurement value f in the carrier coordinate system. b =[f x f y f z ] T Calculation yielded:

[0020]

[0021] The values ​​from the triaxial magnetometer can be used to obtain the heading angle of the carrier. First, the magnetometer output is leveled according to the horizontal attitude. The quaternion used to level the magnetometer output is:

[0022]

[0023] The magnetometer output m after leveling h for:

[0024]

[0025] The formula for calculating the heading angle is as follows:

[0026]

[0027] In step five, the measurement equation Z = HX + V can be expanded as follows:

[0028]

[0029] In the formula, V represents the measurement noise.

[0030] The Kalman filter steps in step six are as follows:

[0031] One-step prediction estimation:

[0032]

[0033] The discretized state transition matrix Φ is:

[0034] Φ=I+F△t

[0035] In the formula, I is the identity matrix and Δt is the sampling time.

[0036] The error covariance of the one-step prediction estimate is:

[0037] P k+1 / k =ΦP k Φ T +Q k

[0038] In the formula, Q k Let be the system noise variance matrix.

[0039] The filter gain matrix is:

[0040]

[0041] In the formula, R is the measurement noise variance matrix.

[0042] Error correction:

[0043]

[0044] Error covariance update:

[0045] P k+1 =(IK k H)P k+1 / k

[0046] In step seven, the Kalman filter is operated according to the method described in step six, and the data fusion method using sequential filtering is obtained. The process is as follows:

[0047] One-step prediction estimate is written as:

[0048]

[0049] The error covariance matrix of the one-step prediction estimate can be obtained by calculation. After each update iteration, only a portion of the elements are non-zero.

[0050]

[0051] The filter gain matrix is:

[0052]

[0053] Error correction is as follows:

[0054]

[0055]

[0056] Error covariance update:

[0057]

[0058] Based on the above processing steps, sensor data from the attitude reference system can be collected in real time, and a sequential filtering data processing method can be implemented to output the attitude angle of the UAV carrier in real time.

[0059] Compared with the prior art, the advantages of the present invention are:

[0060] 1. The sequential filtering method designed in this invention can reduce the amount of computation compared with the traditional Kalman filtering method, and avoids matrix calculation and inversion processing during the calculation process.

[0061] 2. Compared with the traditional complementary filtering algorithm, the present invention introduces the calculation of error covariance, which can improve the accuracy of attitude calculation.

[0062] 3. In this invention, the heading angle can be obtained not only by a magnetometer, but also by the heading measured by dual antennas in satellite navigation. The calculation process can be completed simply by replacing the observations without changing the subsequent calculation process. Attached Figure Description

[0063] Appendix Figure 1 This is a flowchart of the sequential filtering attitude calculation process of the attitude reference system for the rotary-wing UAV of the present invention. Detailed Implementation

[0064] To make the technical problems, technical solutions, and beneficial effects of this invention clearer, the invention will be described in detail below with reference to the accompanying drawings and embodiments. It should be noted that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0065] Figure 1 This is a flowchart illustrating the attitude calculation process for the rotary-wing UAV of the present invention. In this invention, the coordinate systems are first defined as follows: the carrier coordinate system is "front right-lower," and the navigation coordinate system is "northeast-ground." The calculation method includes:

[0066] Step 1: Since the rotor vibration frequency of the rotary-wing UAV is relatively high, the data from the three-axis accelerometer, three-axis gyroscope and three-axis magnetometer need to be low-pass filtered to reduce the impact of vibration on the sensor data.

[0067] Step 2: Initialize the system to obtain the initial attitude angles. To simplify processing, the roll angle γ and roll angle θ in the initial horizontal attitude are both set to 0. The data is then obtained from the triaxial magnetometer output. Calculate the heading angle. Before this, the magnetic declination Δψ of the local geographical location needs to be calculated based on the world geomagnetic model. The initial heading angle ψ is calculated as follows:

[0068]

[0069] Step 3: From the differential equation of Euler angles, we can obtain:

[0070]

[0071] In the formula ω b =[ω x ω y ω z ] T Let θ be the output value of the three-axis gyroscope in the carrier coordinate system. Due to the limited dynamic performance of rotary-wing UAVs, it is difficult to perform large-amplitude movements in the roll and pitch directions. Therefore, the above nonlinear Euler angle differential equation is simplified. When θ and γ are small, we have:

[0072]

[0073] Define the state variable as:

[0074] X=[γ θ ψ ε x ε y ε z ] T

[0075] In the formula, ε x ε y and ε z These are the zero bias values ​​of the three-axis gyroscope.

[0076] Then the state equation It can be expanded as follows:

[0077]

[0078] Step 4: Calculate the observations. Since the horizontal maneuverability of the rotary-wing UAV is poor, the horizontal attitude can be measured by the accelerometer value f in the carrier coordinate system. b =[f x f y f z ] T Calculation yielded:

[0079]

[0080] The values ​​from the triaxial magnetometer can be used to obtain the heading angle of the carrier. First, the magnetometer output is leveled according to the horizontal attitude. The quaternion used to level the magnetometer output is:

[0081]

[0082] The magnetometer output m after leveling h for:

[0083]

[0084] The formula for calculating the heading angle is as follows:

[0085]

[0086] Step 5: The measurement equation Z = HX + V can be expanded as follows:

[0087]

[0088] V represents the measurement noise.

[0089] Step 6: The Kalman filter update calculation equation is as follows:

[0090] One-step prediction estimation:

[0091]

[0092] The discretized state transition matrix Φ is:

[0093] Φ=I+F△t

[0094] In the formula, I is the identity matrix and Δt is the sampling time.

[0095] The error covariance of the one-step prediction estimate is:

[0096] P k+1 / k =ΦP k Φ T +Q k

[0097] In the formula, Q k Let be the system noise variance matrix.

[0098] The filter gain matrix is:

[0099]

[0100] In the formula, R is the measurement noise variance matrix.

[0101] Error correction:

[0102]

[0103] Error covariance update:

[0104] P k+1 =(IK k H)P k+1 / k

[0105] Step 7:

[0106] The Kalman filter calculation involves numerous matrix operations, including matrix inversion. It can be observed that both the F and H matrices in the system contain a large number of zero elements. Therefore, the Kalman filter calculation can be modified. First, the first-step prediction estimate can be written as:

[0107]

[0108] The error covariance matrix of the one-step prediction estimate can be obtained by calculation. After each update iteration, only a portion of the elements are non-zero.

[0109]

[0110] The filter gain matrix is:

[0111]

[0112] Error correction is as follows:

[0113]

[0114]

[0115] Error covariance update:

[0116]

[0117] The above processing method realizes the sequential processing of Kalman filtering. The attitude result of the rotorcraft UAV can be obtained by simply calculating the corresponding data. At the same time, the value of the noise variance matrix can be adjusted in real time according to the accelerometer output and magnetometer output. The matrix calculation is avoided in the whole calculation process, thus improving the calculation efficiency.

[0118] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the principles and spirit of the present invention are included within the protection scope of the present invention.

Claims

1. A sequential filtering method for data fusion in a rotary-wing unmanned aerial vehicle (UAV) attitude reference system, characterized in that, include: Step 101: Process the collected sensor data through a low-pass filter; Step 102: Initialize the attitude reference system, including the horizontal attitude angle and heading angle; Step 103: Construct the state transition equation of the system using Euler angle differential equations, define the state variables, and obtain the state transition matrix; Step 104: Calculate the observables of the carrier based on the acceleration output and magnetometer output; Step 105: Construct the measurement equations to obtain the corresponding observation matrix; Step 106: Based on the steps required for Kalman filtering, map the state variables required during the operation of the Kalman filter to the system. Step 107: Perform calculations and organization according to the Kalman filter data update method, complete the calculation derivation of the corresponding quantities, and realize the sequential processing of Kalman filtering.

2. The sequential filtering method for data fusion of a rotary-wing unmanned aerial vehicle (UAV) attitude reference system according to claim 1, characterized in that, The first step is to perform low-pass filtering on the data from the three-axis accelerometer and three-axis gyroscope based on the actual vibration frequency of the drone's rotor, and then output the data in real time.

3. The sequential filtering method for data fusion of a rotary-wing unmanned aerial vehicle (UAV) attitude reference system according to claim 1, characterized in that, The second step is to initialize the horizontal attitude, with roll and pitch angles both at zero, and calculate the magnetic declination at the local geographical location. The heading angle can be calculated using the output of a three-axis magnetometer.

4. The sequential filtering method for data fusion of a rotary-wing unmanned aerial vehicle (UAV) attitude reference system according to claim 1, characterized in that, The third step is to simplify the model based on the differential equation of Euler angles to obtain the system's state equation and state transition matrix.

5. The sequential filtering method for data fusion of a rotary-wing unmanned aerial vehicle (UAV) attitude reference system according to claim 1, characterized in that, The fourth step is to construct the observations based on the measured outputs of the triaxial accelerometer and triaxial magnetometer. The horizontal attitude can be calculated from the accelerometer measurements in the carrier coordinate system, and the heading angle can be calculated from the horizontal attitude, the triaxial magnetometer output, and the magnetic declination.

6. The sequential filtering method for data fusion of a rotary-wing unmanned aerial vehicle attitude reference system according to claim 1, characterized in that, The fifth step is to construct measurement equations based on the actual characteristics of the system.

7. The sequential filtering method for data fusion of a rotary-wing unmanned aerial vehicle attitude reference system according to claim 1, characterized in that, The sixth step is to calculate the corresponding parameter matrix according to the Kalman filter steps.

8. The sequential filtering method for data fusion of a rotary-wing unmanned aerial vehicle attitude reference system according to claim 1, characterized in that, The seventh step involves solving the problem based on the operation mode of the Kalman filter described in step six, which yields the data fusion method using sequential filtering. The one-step prediction estimate of the sequential filtering is: The error covariance of the one-step prediction estimate is: The filter gain matrix is: The sequential filtering error is corrected as follows: Error covariance update: Based on the above processing steps, sensor data from the attitude reference system can be collected in real time, and a sequential filtering data processing method can be implemented to output the attitude angle of the UAV carrier in real time.

Images