A heading fusion method based on GNSS velocity observation

Abstract

The application relates to a heading fusion method based on GNSS speed observation, which comprises the following steps: acquiring gyro angular velocity measurement values and accelerometer measurement values, preprocessing the gyro angular velocity measurement values, preprocessing the accelerometer measurement values, obtaining navigation system speed increments based on the preprocessed acceleration computer system, constructing a state transition equation according to the navigation system speed increments and calculating a prediction covariance matrix, calculating a heading angle, and switching a heading source in the operation of a UAV. The heading fusion method based on GNSS speed observation can solve the problems that the heading observation is invalid when the RTK is affected, the heading output is discontinuous in the invalidation and recovery process, the mutation response of the flight control closed loop is easily induced, the risk actions such as the rapid swing of the aircraft head are caused, and the problems that the deviation accumulation or the unstable updating is caused due to the differences between the time stamps and the updating frequencies of the GNSS and the IMU.

Description

Technical Field

[0001] This invention relates to the field of aircraft integrated navigation and attitude and heading calculation technology, specifically to a heading fusion method based on GNSS velocity observation. Background Technology

[0002] In the navigation and control systems of aircraft such as UAVs, the heading angle is a crucial state variable for attitude calculation and trajectory tracking. In engineering, RTK orientation or magnetic compasses are commonly used to provide heading observations, which are then combined with inertial measurement unit (IMU) outputs to complete attitude and heading calculations. However, the reliability of heading observations is affected by environmental factors, electromagnetic fields, and satellite signal quality, and is more prone to observation distortion, interruptions, or delays in complex operational scenarios.

[0003] When the RTK is affected by multipath effects, ionospheric activity, etc., and falls out of the fixed solution or becomes inaccurate in orientation, or when the magnetic compass is subjected to strong electromagnetic interference, heading observation may fail. If the heading output is discontinuous during the failure and recovery process, it can easily induce abrupt responses in the flight control closed loop, leading to risky maneuvers such as rapid nose sway. In addition, there are differences in the timestamps and update frequencies between GNSS and IMU. If measurement lag and time alignment are not properly handled, the fusion estimation may suffer from accumulated bias or unstable updates.

[0004] Therefore, existing technologies have shortcomings and need to be improved and developed. Summary of the Invention

[0005] This invention provides a heading fusion method based on GNSS velocity observations to address the problem in existing technologies where heading observations may fail when the RTK is affected by multipath effects, ionospheric activity, etc., causing it to fall out of a fixed solution or become inaccurate, or when the magnetic compass is subjected to strong electromagnetic interference. Furthermore, if the heading output is discontinuous during the failure and recovery process, it can easily induce abrupt changes in the flight control closed-loop response, leading to risky maneuvers such as rapid nose sway. In addition, the differences in timestamps and update frequencies between GNSS and IMU mean that if measurement lag and time alignment are not properly handled, the fusion estimation may suffer from accumulated bias or unstable updates.

[0006] This invention provides a heading fusion method based on GNSS velocity observations, comprising:

[0007] Acquire the gyroscope angular velocity measurement value and accelerometer measurement value output by the inertial measurement unit (IMU);

[0008] Based on the gyroscope measurement error model, vibration mode error filtering and zero bias estimation are performed on the gyroscope angular velocity measurement values, and zero bias compensation is performed on the gyroscope angular velocity measurement values ​​to obtain the preprocessed angular velocity;

[0009] The accelerometer measurements are preprocessed, and the preprocessed angular velocity and preprocessed acceleration are input together through a complementary filtering algorithm to generate a reference attitude angle and obtain an attitude cosine matrix.

[0010] Based on the velocity increment in the preprocessed acceleration computer system, and using the attitude cosine matrix, the velocity increment in the machine system is transformed to the NED navigation coordinate system to obtain the navigation system velocity increment;

[0011] The northbound and eastbound velocities in the navigation coordinate system output by real-time dynamic differential RTK are obtained and state variables are constructed based on the heading angle. A state transition equation is constructed based on the velocity increment of the navigation system and the prediction covariance matrix is ​​calculated.

[0012] Time alignment is performed on the time lag between the RTK output velocity and the IMU predicted state. After time alignment, Kalman measurement update is performed, the fused state estimation result at the current moment is output, and the heading angle estimation value is obtained from the fused state estimation result and connected to the closed loop.

[0013] In the event of RTK heading data failure and recovery, the heading source is switched between the RTK-based heading angle estimate and the GNSS velocity observation fusion-based heading angle estimate to keep the heading angle output continuous.

[0014] Furthermore, the method for filtering out vibration mode errors includes:

[0015] Based on the aforementioned gyroscope measurement error model: Continuous wavelet transform analysis was performed on the gyroscope measurement data, and combined with aircraft hammer impact tests, the vibration mode distribution at 5Hz, 7Hz, 9Hz, and 20Hz was determined. Based on the measurement error model, a second-order Butterworth low-pass filter with a cutoff frequency of 3Hz and an attenuation gain of 3dB was designed to filter out [the vibration]. ;in, These are gyroscope measurements. This is the true value of angular velocity. Errors caused by modality, Zero bias error It is white Gaussian noise.

[0016] Furthermore, the method for zero-bias estimation includes:

[0017] Based on the zero-biased estimation formula: Fast Fourier Transform (FFT) analysis was performed on the static, unexcited sampling data of the gyroscope, combined with analysis using the autocorrelation function (ACF) and partial autocorrelation function (PACF), to model the zero-bias error as a random constant; among which, For the current time-biased estimate, This is the zero-bias estimate from the previous time step. This corresponds to the gyroscope measurement value at the current moment. These are filter coefficients with a value of 0.001. For discrete time indices;

[0018] Preprocessing of gyroscope measurements based on the aforementioned bias estimation: ,in, These are the pre-processed gyroscope measurements. The zero-biased estimate This is a low-pass filter operator.

[0019] Furthermore, the method for preprocessing the accelerometer measurements includes: using a second-order Butterworth filter with a cutoff frequency of 4Hz to filter out high-frequency modal noise caused by body vibration in the measured acceleration, wherein high-frequency modal noise refers to modal noise with a frequency greater than 5Hz.

[0020] Furthermore, the step of transforming the velocity increment in the machine system to the NED navigation coordinate system using the attitude cosine matrix based on the velocity increment in the preprocessed acceleration computer system to obtain the navigation system velocity increment includes:

[0021] Based on formula Calculate the speed increment; among which, Preprocessed acceleration sampling data for measuring acceleration For speed increments in the machine system, For time intervals;

[0022] Based on formula The velocity increment in the machine system will be transformed to the NED navigation coordinate system to obtain the navigation system velocity increment; wherein, For the northbound velocity increment under the NED navigation system, Let be the attitude cosine matrix.

[0023] Furthermore, the step of acquiring the northward and eastward velocities in the navigation coordinate system output by real-time dynamic differential RTK and constructing state variables based on the heading angle, so as to construct a state transition equation based on the velocity increment of the navigation system and perform state prediction and prediction covariance update, includes:

[0024] Based on the relationship between aircraft velocity and attitude angle, design state variables. ;in, The current northbound velocity. The current eastward velocity is... The heading angle at the current moment;

[0025] Construct the state transition matrix and driving noise matrix ;in, For eastward velocity increments under the NED navigation system;

[0026] Based on the state transition matrix and driving noise matrix Construct the state transition equation: ;in, , , All are white noise. The northbound velocity at the previous moment. The eastward velocity at the previous moment. The heading angle at the previous moment;

[0027] Calculate the prediction covariance matrix : ;in, The updated covariance matrix, Let be the variance matrix of the process noise.

[0028] Furthermore, the step of time-aligning the RTK output velocity with the time lag of the IMU predicted state, performing Kalman measurement updates after time alignment, outputting the fused state estimation result at the current moment, and obtaining the heading angle estimate from the fused state estimation result to enter the closed loop includes:

[0029] The state variables predicted based on IMU data are pushed into a buffer. When the RTK output speed is updated, the predicted state variables corresponding to the speed timestamp of the RTK output are retrieved from the buffer for measurement update; wherein, the measurement update measurement matrix... ;

[0030] Calculate the Kalman gain matrix at the current time step. ;in, The measurement noise covariance matrix;

[0031] Update fusion status: and from The heading angle estimate is obtained and connected to the closed loop; whereby... The measurement vector is composed of the northward and eastward velocities output by the RTK. This is the fused state estimation vector.

[0032] Furthermore, after updating the fusion state, it also includes updating the covariance matrix. : ;in, It is a third-order identity matrix.

[0033] Furthermore, the heading source switching includes:

[0034] When the UAV is on the ground, it performs a flight status pre-check. When the RTK is working properly, the heading output adopts the heading angle estimate based on RTK fusion and allows the aircraft power system to be unlocked. At the same time, the heading observer based on GNSS velocity observation is aligned with the heading output.

[0035] When the UAV is in flight, the heading output uses the heading angle estimate based on RTK fusion; when RTK orientation fails, the RTK-based heading observation input is cut off, and the heading output is switched to the heading observer output value based on GNSS velocity observation, and the gain is made constant after the covariance of the heading observer has been iteratively stabilized.

[0036] When RTK heading measurement data is recovered, the heading observer input based on GNSS velocity observation is cut off and the heading output is switched to the RTK-based heading observer estimate so that the heading angle estimate is continuously output.

[0037] Beneficial effects:

[0038] As can be seen from the above technical solutions, the present invention provides a heading fusion method based on GNSS velocity observation, which has the following beneficial effects:

[0039] 1. Improve heading availability coverage: When orientation methods such as RTK and magnetic compass fail due to interference in complex scenarios, the system can still maintain heading estimation output by utilizing the relationship between GNSS velocity observation and inertial prediction, thus avoiding operation interruption caused by heading unavailability.

[0040] 2. Improve the continuity and closed-loop friendliness of heading output: By organizing the heading source switching under failure and recovery conditions, the heading angle of the closed-loop access remains continuous when the data source changes, reducing the risk of sudden response in the attitude heading control loop.

[0041] 3. Reduce systematic errors introduced by vibration and zero bias: Targeted filtering of gyroscope modal errors and completion of zero bias estimation and compensation before solution, providing more reliable input for attitude cosine matrix and velocity increment calculation, and suppressing error propagation from the source.

[0042] 4. Improve the timing consistency of fusion updates: Time alignment is performed on the lag between RTK velocity measurements and IMU predictions, and measurement updates are completed by matching timestamps to reduce estimation bias caused by asynchronous updates and improve filtering stability.

[0043] 5. Balancing engineering feasibility with computational resource constraints: While drawing on the modeling approach of speed and heading relationship, the computational complexity is reduced by simplifying the observer organization and switching logic, making it more suitable for platforms with limited computing power such as microcontrollers.

[0044] It should be understood that all combinations of the foregoing concepts and the additional concepts described in more detail below can be considered part of the inventive subject matter of this disclosure, provided that such concepts do not contradict each other.

[0045] The foregoing and other aspects, embodiments, and features of the teachings of the present invention will be more fully understood from the following description in conjunction with the accompanying drawings. Other additional aspects of the invention, such as features and / or beneficial effects of exemplary embodiments, will become apparent from the following description or may be learned through practice of specific embodiments according to the teachings of the present invention. Attached Figure Description

[0046] The accompanying drawings are not drawn to scale. In the drawings, each identical or nearly identical component shown in the various figures may be denoted by the same reference numeral. For clarity, not every component is labeled in each figure. Embodiments of various aspects of the invention will now be described by way of example and with reference to the accompanying drawings, wherein:

[0047] Figure 1 This is a general flowchart of a heading fusion method based on GNSS velocity observation in an embodiment of this application.

[0048] Figure 2 This is a flowchart of step S108 of a heading fusion method based on GNSS velocity observation in an embodiment of this application.

[0049] Figure 3 This is a flowchart of step S110 of a heading fusion method based on GNSS velocity observation in an embodiment of this application.

[0050] Figure 4 This is a flowchart of step S112 of a heading fusion method based on GNSS velocity observation in an embodiment of this application.

[0051] Figure 5 This is a schematic diagram of an electronic device according to an embodiment of this application.

[0052] Figure 6 This is a frequency response diagram of the gyroscope low-pass filter in an embodiment of this application.

[0053] Figure 7 This is a spectrum diagram of gyroscope measurement data in an embodiment of this application.

[0054] Figure 8 This is a graph showing the ACF and PACF coefficients of the gyroscope measurement data in the embodiments of this application.

[0055] Figure 9 This is a comparison diagram of flight path channels in the embodiments of this application.

[0056] Figure 10Examples of embodiments in this application Figure 9 A magnified view at 907 seconds.

[0057] Figure 11 For the embodiments of this application Figure 9 A magnified view at 797.6 seconds. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the described embodiments of the present invention without creative effort are within the scope of protection of the present invention. Unless otherwise defined, the technical or scientific terms used herein should have the ordinary meaning understood by those skilled in the art to which this invention pertains.

[0059] The terms "first," "second," and similar words used in the specification and claims of this patent application do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Similarly, unless the context clearly indicates otherwise, the singular forms of "an," "a," or "the," etc., do not indicate a quantity limitation, but rather indicate the presence of at least one. Terms such as "comprising" or "including" mean that the element or object preceding "comprising" encompasses the features, integrals, steps, operations, elements, and / or components listed following "comprising" or "including," and do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components, and / or collections thereof. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships; these relative positional relationships may change accordingly when the absolute position of the described object changes.

[0060] When the RTK is affected by multipath effects, ionospheric activity, etc., and falls out of the fixed solution or becomes inaccurate in orientation, or when the magnetic compass is subjected to strong electromagnetic interference, heading observation may fail. If the heading output is discontinuous during the failure and recovery process, it can easily induce abrupt responses in the flight control closed loop, leading to risky maneuvers such as rapid nose sway. In addition, there are differences in the timestamps and update frequencies between GNSS and IMU. If measurement lag and time alignment are not properly handled, the fusion estimation may suffer from accumulated bias or unstable updates.

[0061] Therefore, embodiments of the present invention provide a heading fusion method based on GNSS velocity observation, referring to... Figure 1 ,include:

[0062] Step S102: Obtain the gyroscope angular velocity measurement value and accelerometer measurement value output by the inertial measurement unit (IMU).

[0063] Step S104: Based on the gyroscope measurement error model, the vibration mode error of the gyroscope angular velocity measurement value is filtered out and zero bias is estimated, and the zero bias is compensated to obtain the preprocessed angular velocity.

[0064] Step S106: Preprocess the accelerometer measurements, and use a complementary filtering algorithm to input the preprocessed angular velocity and preprocessed acceleration together to generate a reference attitude angle and obtain the attitude cosine matrix.

[0065] Step S108: Based on the velocity increment in the preprocessed acceleration computer system, and using the attitude cosine matrix, transform the velocity increment in the computer system to the NED navigation coordinate system to obtain the navigation system velocity increment.

[0066] Step S110: Obtain the northward and eastward velocities in the navigation coordinate system output by the real-time dynamic differential RTK and construct state variables based on the heading angle. Construct state transition equations based on the navigation system velocity increments and calculate the prediction covariance matrix.

[0067] Step S112: Time-align the RTK output velocity with the time lag of the IMU predicted state. After time alignment, perform Kalman measurement update, output the fused state estimation result at the current moment, and obtain the heading angle estimate from the fused state estimation result to enter the closed loop.

[0068] Step S114: In the event of RTK heading data failure and recovery, switch the heading source between the RTK-based heading angle estimate and the GNSS velocity observation fusion-based heading angle estimate to keep the heading angle output continuous.

[0069] The data from the IMU gyroscope and accelerometer are preprocessed separately, and a reference attitude is formed by complementary filtering to obtain the attitude cosine matrix. The velocity increment is calculated based on the preprocessed acceleration and transformed to the NED navigation coordinate system. The state is then constructed using the northward velocity, eastward velocity and heading angle, and prediction and measurement updates are performed in combination with the RTK output velocity. The fused heading angle is output and connected to the closed loop. When the RTK heading fails or recovers, the heading source is switched to maintain continuous output.

[0070] This approach employs a continuous output strategy during heading estimation and failure recovery supported by GNSS velocity observations. Firstly, it links short-term IMU attitude calculations and velocity increment estimations to heading state prediction, updating the data using RTK velocity as the measurement. Secondly, for engineering scenarios involving heading data failure and recovery, it introduces heading source switching, ensuring continuity of the heading angle when switching between different data sources. Even in scenarios where the heading sensor is unreliable, this process maintains the availability of the heading angle and reduces the impact of sudden heading changes on the closed loop through the switching mechanism, thereby improving flight operation stability.

[0071] In some embodiments, the method for filtering vibration mode errors includes: based on a gyroscope measurement error model. The gyroscope measurement error model is a measurement model for this model of gyroscope, summarized after analyzing a large amount of measurement data of BMI088 model gyroscopes under dynamic and static conditions using continuous wavelet transform technology.

[0072] Continuous wavelet transform analysis was performed on gyroscope measurement data, and combined with aircraft hammer impact tests, the vibration mode distribution at 5Hz, 7Hz, 9Hz, and 20Hz was determined. Based on the measurement error model, a second-order Butterworth low-pass filter with a cutoff frequency of 3Hz and an attenuation gain of 3dB was designed to filter out [the vibration]. ;in, These are gyroscope measurements. This is the true value of angular velocity. Errors caused by modality, Zero bias error The noise is white Gaussian noise. The modal energy peak at 7Hz is relatively high, severely affecting measurement accuracy and requiring careful elimination, while the modal noise energy peak near 5Hz is relatively low. Based on the gyroscope's measurement model, a second-order Butterworth low-pass filter is designed to filter out errors caused by vibration modes. The frequency response of this filter is as follows: Figure 6 As shown.

[0073] After analyzing a large amount of static, unexcited sampling data from the BMI088 gyroscope using FFT technology, and referring to... Figure 7 and Figure 8 The conclusions are as follows: 1. The angular velocity measurement of this model of gyroscope exhibits zero bias; 2. The angular velocity measurement accuracy of this gyroscope is high, the amplitude spectrum distribution is relatively flat, there are no obvious bandpass or bandstop characteristics, only weak harmonic peaks in amplitude, and it approximately follows a normal distribution or a short-correlation-time random process. Furthermore, by calculating the ACF and PACF coefficients of the gyroscope measurements, it was found that within the 20th order, all coefficients except the 0th order coefficient are essentially close to 0. Looking at the 100th order, except for the 0th order, the peak coefficients are mainly distributed at the 65th order, which is too high. Therefore, the zero bias error... The model is a random constant.

[0074] By correlating the vibration mode identification results with the gyroscope error model, modal errors are specifically separated from angular velocity measurements, providing a cleaner angular velocity input for subsequent attitude and heading estimation. This reduces the impact of airframe structural vibration on angular velocity measurements, decreases noise injection in attitude calculations, and provides a more stable prediction basis for the heading fusion chain.

[0075] In some embodiments, the method for zero-bias estimation includes: based on the zero-bias estimation formula: Fast Fourier Transform (FFT) analysis was performed on the static, unexcited sampling data of the gyroscope, combined with analysis using the autocorrelation function (ACF) and partial autocorrelation function (PACF), to model the zero-bias error as a random constant; among which, For the current time-biased estimate, This is the zero-bias estimate from the previous time step. This corresponds to the gyroscope measurement value at the current moment. These are filter coefficients with a value of 0.001. This represents the discrete time sequence number.

[0076] Preprocessing of gyroscope measurements based on zero-bias estimation: ,in, These are the pre-processed gyroscope measurements. The zero-biased estimate This is a low-pass filter operator.

[0077] When the aircraft is idling on the ground, the rotor blades rotate, causing vibrations throughout the aircraft. Therefore, the error measured by the gyroscope at this time includes modal noise and cannot reflect the true zero-bias error. Thus, zero-bias estimation needs to be completed before the calculation. Considering the actual operational scenario of the aircraft, which takes off and operates shortly after power-on, the entire estimation time needs to be controlled within 3-5 seconds. Furthermore, since the gyroscope zero-bias is an extremely low-frequency quantity, and taking the above factors into account, a first-order low-pass filter with a cutoff frequency of 0.1 rad is designed, and the zero-bias estimation formula is used to estimate the gyroscope's zero-bias.

[0078] By combining the statistical modeling conclusions of zero bias with a short-time online estimation strategy, zero bias compensation can be completed before the solution is applied, adapting to the rapid takeoff operation rhythm. This reduces the contribution of zero bias to yaw angle integral drift, improves the reliability of the attitude cosine matrix generated by complementary filtering, and provides a more consistent attitude basis for subsequent heading correction based on velocity observations.

[0079] In some embodiments, the method for preprocessing accelerometer measurements includes: filtering out high-frequency modal noise caused by body vibration in the measured acceleration using a second-order Butterworth filter with a cutoff frequency of 4 Hz, wherein high-frequency modal noise refers to modal noise with a frequency greater than 5 Hz.

[0080] In some embodiments, based on the velocity increment in the preprocessed acceleration computer system, the velocity increment in the machine system is transformed to the NED navigation coordinate system using the attitude cosine matrix to obtain the navigation system velocity increment, with reference to... Figure 2 ,include:

[0081] Step S1081: Based on the formula Calculate the speed increment; among which, Preprocessed acceleration sampling data for measuring acceleration For speed increments in the machine system, For time intervals.

[0082] Step S1082: Based on the formula The velocity increment in the machine system is transformed to the NED navigation coordinate system to obtain the navigation system velocity increment; among which, For the northbound velocity increment under the NED navigation system, Let be the attitude cosine matrix.

[0083] The attitude cosine matrix obtained from the attitude calculation is used as a coordinate transformation bridge, ensuring that the velocity increment obtained from acceleration integration is in the same navigation coordinate system as the GNSS velocity observation, thus fulfilling the prerequisite for fusion. This achieves coordinate consistency between the predicted and measured quantities, reduces systematic errors caused by coordinate inconsistencies, and improves the effectiveness of fusion updates.

[0084] In some embodiments, the northbound and eastbound velocities in the navigation coordinate system output by real-time dynamic differential RTK are obtained and used as state variables based on the heading angle. State transition equations are then constructed based on the navigation system velocity increments, and state prediction and prediction covariance updates are performed, referencing... Figure 3 ,include:

[0085] Step S1101: Based on the relationship between aircraft velocity and attitude angle, design state variables. ;in, The current northbound velocity. The current eastward velocity is... This represents the heading angle at the current moment.

[0086] Step S1102: Construct the state transition matrix and driving noise matrix ;in, This is the eastward velocity increment under the NED navigation system.

[0087] Step S1103: Based on the state transition matrix and driving noise matrix Construct the state transition equation: ;in, , , All are white noise. The northbound velocity at the previous moment. The eastward velocity at the previous moment. This is the heading angle at the previous moment.

[0088] Step S1104: Calculate the prediction covariance matrix : ;in, This is the updated covariance matrix; Let be the variance matrix of the process noise.

[0089] By employing state modeling that couples velocity and heading, the heading angle is incorporated into the prediction and update framework as a state. This allows heading estimation to rely not only on fixed-vector measurements but also on constraints provided by velocity observations under dynamic conditions. Even when the quality of orientation data deteriorates, the heading state remains estimable within the filtering framework, improving the system's robustness to sensor failure scenarios.

[0090] In some embodiments, the time lag between the RTK output velocity and the IMU predicted state is time-aligned. After time alignment, a Kalman measurement update is performed, outputting the fused state estimation result at the current moment. The heading angle estimate is then obtained from the fused state estimation result and incorporated into the closed loop. Figure 4 ,include:

[0091] Step S1121: Push the state variables predicted based on IMU data into the buffer. When the RTK output speed is updated, retrieve the predicted state variables corresponding to the speed timestamp of the RTK output from the buffer for measurement update; wherein, the measurement update measurement matrix... .

[0092] Step S1122: Calculate the Kalman gain matrix at the current time step. ;in, This is the measurement noise covariance matrix.

[0093] Step S1123: Update fusion status: and from The heading angle estimate is obtained and connected to the closed loop; whereby... The measurement vector is composed of the northward and eastward velocities output by the RTK. This is the fused state estimation vector.

[0094] To address the lag issue in RTK velocity measurements, a buffer is used to store the IMU prediction state. During RTK updates, the corresponding prediction state is retrieved according to the timestamp for measurement updates. The measurement matrix, Kalman gain, and state update formulas are provided. The heading angle is output from the fused state and integrated into the closed loop. The buffer enables cross-frequency and cross-delay data alignment, ensuring that RTK velocity measurements are updated to the correct time-referenced prediction state, thus avoiding update deviations caused by lag. This improves the timing consistency of fused updates, reduces the risk of filter divergence, and makes the heading output smoother, facilitating closed-loop use.

[0095] In some embodiments, after updating the fusion state, the method further includes: updating the covariance matrix. : ;in, It is a third-order identity matrix. The recursive management of uncertainty enables the system to reflect the stage-wise confidence changes of different sensors using covariance, providing a mathematical basis for heading source switching and gain fixation, maintaining filter stability, and ensuring that the heading estimate maintains a consistent confidence evolution during dynamic maneuvers and data source changes.

[0096] In some embodiments, when the heading angle output by the RTK fails and then recovers, the fused heading angle connected to the closed loop must be continuous; otherwise, dangerous maneuvers such as sudden nose-bouncing of the aircraft may occur. To address this issue, a fused heading source switching mechanism is designed as follows:

[0097] When the UAV is on the ground, it performs a flight status pre-check. When the RTK is working properly, the heading output adopts the heading angle estimate based on RTK fusion and allows the aircraft's power system to be unlocked. At the same time, the heading observer based on GNSS velocity observation is aligned with the heading output.

[0098] When the UAV is in flight, the heading output uses the heading angle estimate based on RTK fusion; when RTK orientation fails, the heading observation input based on RTK is cut off, and the heading output is switched to the heading observer output value based on GNSS velocity observation, and the gain is made constant after the covariance of the heading observer has been iteratively stabilized.

[0099] When RTK heading measurement data is recovered, the heading observer input based on GNSS velocity observation is cut off and the heading output is switched to the RTK-based heading observer estimate so that the heading angle estimate is continuously output.

[0100] During the ground pre-inspection phase, when RTK is normal, the RTK heading is used and aligned with the velocity fusion heading observer. During the flight phase, when RTK heading fails, the RTK heading input is cut off and the heading output based on GNSS velocity observation is switched off, with the gain fixed after the covariance iteratively stabilizes. When the RTK heading recovers, the RTK heading output is switched back to maintain continuity. By pre-aligning and managing gain after switching, the abrupt changes during source switching are reduced, meeting the engineering requirements for continuity in the closed loop. This avoids dangerous responses to the aircraft induced by sudden heading changes and improves flight stability and availability in complex operational scenarios. The experimental description in the handover document shows that the heading output remains continuous and converges during the RTK failure and recovery switching process. Specifically, "making the gain constant after covariance iteratively stabilizes" means that during the Kalman filter recursion process, the covariance matrix's elements converge after continuous iterative updates and remain unchanged within a preset threshold range. Since the Kalman gain matrix is ​​determined by the covariance matrix, the corresponding Kalman gain matrix value is fixed, making the gain constant. Among them, "RTK heading measurement data recovery" refers to the RTK heading measurement data being restored from a failed state to a valid state, and the re-output of heading measurement values ​​that can be used for heading estimation when the preset validity criteria are met.

[0101] The method provided in this application solves the problem of how to determine the heading when RTK and magnetic compass orientation fail. It can switch to a heading estimation algorithm based on GNSS velocity, outputting continuous, stable and accurate heading estimates, and can achieve strict consistency and asymptotic stability.

[0102] The aircraft was operated to fly along the planned route. After reaching the end of the route, the aircraft turned around and flew another route. During the flight, the RTK data source was switched multiple times to verify both valid and invalid cases. Since the RTK heading angle measurement value obtained from the fixed solution has high accuracy and precision, it can be considered as the true value and can be used for comparison with the fused heading angle.

[0103] Reference Figure 9 The red line represents the RTK measured heading angle; the blue line represents the fused heading angle; and the black line represents the current heading source. Before 797.6 seconds, RTK data is invalid, and the heading source switches to the speed-fused heading. Between 797.6 and 907 seconds, RTK data is valid, and the heading source switches to the RTK measured heading. After 907 seconds, RTK data is invalid, and the heading source switches to the speed-fused heading. It can be observed that the fused heading angle is very close to the RTK measured value. When the aircraft sharply turns its nose, the fused heading angle, after a brief adjustment, gradually converges to near the true heading angle.

[0104] Reference Figure 10 ,for Figure 9The magnified image at 907 seconds reveals that the RTK heading measurement failed. When the heading source is switched to the velocity-fused heading, the output fused heading angle is continuously stable, which keeps the aircraft's flight status stable.

[0105] Reference Figure 11 ,for Figure 9 The magnified image at 797.6 seconds shows that after switching the data source back to the RTK heading measurement, the fused heading angle continuously and smoothly converged to near the true heading angle, ensuring the stability of the aircraft's flight status and preventing any dangerous flight maneuvers.

[0106] Another embodiment of the present invention also provides a heading fusion device based on GNSS velocity observation, comprising:

[0107] The acquisition module is used to acquire the gyroscope angular velocity measurement value and accelerometer measurement value output by the inertial measurement unit (IMU).

[0108] The first processing module is used to filter out vibration mode errors and estimate zero bias in the gyroscope angular velocity measurement value based on the gyroscope measurement error model, and to perform zero bias compensation on the gyroscope angular velocity measurement value to obtain the preprocessed angular velocity.

[0109] The second processing module is used to preprocess the accelerometer measurements, and then use a complementary filtering algorithm to input the preprocessed angular velocity and preprocessed acceleration to generate a reference attitude angle and obtain the attitude cosine matrix.

[0110] The transformation module is used to transform the velocity increment in the machine system to the NED navigation coordinate system based on the velocity increment in the preprocessed acceleration computer system and using the attitude cosine matrix to obtain the navigation system velocity increment.

[0111] The calculation module is used to obtain the northward and eastward velocities in the navigation coordinate system output by real-time dynamic differential RTK and construct state variables based on the heading angle. It then constructs state transition equations based on the navigation system velocity increments and calculates the prediction covariance matrix.

[0112] The update module is used to time-align the velocity output by the RTK with the time lag of the IMU predicted state. After time alignment, it performs Kalman measurement update, outputs the fused state estimation result at the current moment, and obtains the heading angle estimate from the fused state estimation result to enter the closed loop.

[0113] The switching module is used to switch the heading source between the RTK-based heading angle estimate and the GNSS velocity observation fusion estimate in the event of RTK heading data failure and recovery, so as to keep the heading angle output continuous.

[0114] It should be noted that although several units or sub-units of the device have been mentioned in the detailed description above, this division is merely exemplary and not mandatory. In fact, according to embodiments of this application, the features and functions of two or more units described above can be embodied in one unit. Conversely, the features and functions of one unit described above can be further divided and embodied by multiple units.

[0115] Based on the same inventive concept as the above method embodiments, this application also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it enables the electronic device to implement the control method described in the above embodiments.

[0116] In one embodiment, the electronic device may be a server, and in this embodiment, the structure of the electronic device may be as follows: Figure 5 As shown, it includes a memory, a communication module, and one or more processors.

[0117] Memory is used to store computer programs executed by the processor. Memory can be mainly divided into a program storage area and a data storage area. The program storage area can store the operating system and programs required to run instant messaging functions, etc.; the data storage area can store various instant messaging information and operation instruction sets, etc.

[0118] Memory can be volatile memory, such as random access memory (RAM); memory can also be non-volatile memory, such as read-only memory, flash memory, hard disk drive (HDD), or solid-state drive (SSD); or memory can be any other medium capable of carrying or storing a desired computer program having the form of instructions or data structures and accessible by a computer, but is not limited thereto. Memory can be a combination of the above-mentioned types of memory.

[0119] A processor may include one or more central processing units (CPUs) or digital processing units, etc. The processor is used to implement the aforementioned audio data processing methods when it invokes computer programs stored in memory.

[0120] The communication module is used to communicate with terminal devices and other servers.

[0121] This application embodiment does not limit the specific connection medium between the above-described memory, communication module, and processor. This application embodiment... Figure 5 The memory and processor are connected via a bus, and the bus is in... Figure 5 The connections between other components are illustrated with arrows and are for illustrative purposes only, not as limiting information. Buses can be categorized as address buses, data buses, control buses, etc. For ease of description, Figure 5 The text uses only one arrow to describe it, but does not indicate that there is only one bus or one type of bus.

[0122] Based on the same inventive concept as the above-described method embodiments, embodiments of the present invention also provide a computer-readable storage medium for storing a computer program. When the computer program is run on a computer, it enables an electronic device to implement the control methods described in the above embodiments. The computer-readable storage medium can be a readable signal medium or a readable storage medium. A readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: an electrical connection having one or more wires, a portable disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof.

[0123] Based on the same inventive concept as the above-described method embodiments, embodiments of the present invention also provide a computer program product. The computer program product includes a computer program that, when run on an electronic device, causes the electronic device to perform the steps of the control methods described above according to various exemplary embodiments of this application. The program product may take the form of any combination of one or more readable media. These computer program commands can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing device to produce a machine, such that the commands executed by the processor of the computer or other programmable data processing device generate a process for implementing... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0124] While the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the invention. Those skilled in the art can make various modifications and refinements without departing from the spirit and scope of the invention. Therefore, the scope of protection of the present invention shall be determined by the claims.

Claims

1. A heading fusion method based on GNSS velocity observation, characterized in that, include: Acquire the gyroscope angular velocity measurement value and accelerometer measurement value output by the inertial measurement unit (IMU); Based on the gyroscope measurement error model, vibration mode error filtering and zero bias estimation are performed on the gyroscope angular velocity measurement values, and zero bias compensation is performed on the gyroscope angular velocity measurement values ​​to obtain the preprocessed angular velocity; The accelerometer measurements are preprocessed, and the preprocessed angular velocity and preprocessed acceleration are input together through a complementary filtering algorithm to generate a reference attitude angle and obtain an attitude cosine matrix. Based on the velocity increment in the preprocessed acceleration computer system, and using the attitude cosine matrix, the velocity increment in the machine system is transformed to the NED navigation coordinate system to obtain the navigation system velocity increment; The northbound and eastbound velocities in the navigation coordinate system output by real-time dynamic differential RTK are obtained and state variables are constructed based on the heading angle. A state transition equation is constructed based on the velocity increment of the navigation system and the prediction covariance matrix is ​​calculated. Time alignment is performed on the time lag between the RTK output velocity and the IMU predicted state. After time alignment, Kalman measurement update is performed, the fused state estimation result at the current moment is output, and the heading angle estimation value is obtained from the fused state estimation result and connected to the closed loop. In the event of RTK heading data failure and recovery, a heading source switch is performed between the RTK-based heading angle estimate and the GNSS velocity observation fusion-based heading angle estimate to maintain continuous heading angle output. The heading source switch includes: When the UAV is on the ground, it performs a flight status pre-check. When the RTK is working properly, the heading output adopts the heading angle estimate based on RTK fusion and allows the aircraft power system to be unlocked. At the same time, the heading observer based on GNSS velocity observation is aligned with the heading output. When the UAV is in flight, the heading output uses the heading angle estimate based on RTK fusion; when RTK orientation fails, the RTK-based heading observation input is cut off, and the heading output is switched to the heading observer output value based on GNSS velocity observation, and the gain is made constant after the covariance of the heading observer has been iteratively stabilized. When RTK heading measurement data is recovered, the heading observer input based on GNSS velocity observation is cut off and the heading output is switched to the RTK-based heading observer estimate so that the heading angle estimate is continuously output.

2. The heading fusion method based on GNSS velocity observation according to claim 1, characterized in that, The method for filtering out vibration mode errors includes: Based on the aforementioned gyroscope measurement error model: Continuous wavelet transform analysis was performed on the gyroscope measurement data, and combined with aircraft hammer impact tests, the vibration mode distribution at 5Hz, 7Hz, 9Hz, and 20Hz was determined. Based on the measurement error model, a second-order Butterworth low-pass filter with a cutoff frequency of 3Hz and an attenuation gain of 3dB was designed to filter out [the vibration]. ;in, These are gyroscope measurements. This is the true value of angular velocity. Errors caused by modality, Zero bias error It is white Gaussian noise.

3. The heading fusion method based on GNSS velocity observation according to claim 2, characterized in that, The method for zero bias estimation includes: Based on the zero-biased estimation formula: Fast Fourier Transform (FFT) analysis was performed on the static, unexcited sampling data of the gyroscope, combined with analysis using the autocorrelation function (ACF) and partial autocorrelation function (PACF), to model the zero-bias error as a random constant; among which, For the current time-biased estimate, This is the zero-bias estimate from the previous time step. This corresponds to the gyroscope measurement value at the current moment. These are filter coefficients with a value of 0.

001. For discrete time indices; Preprocessing of gyroscope measurements based on the aforementioned bias estimation: ,in, These are the pre-processed gyroscope measurements. The zero-biased estimate This is a low-pass filter operator.

4. The heading fusion method based on GNSS velocity observation according to claim 1, characterized in that, The method for preprocessing the accelerometer measurements includes: using a second-order Butterworth filter with a cutoff frequency of 4Hz to filter out high-frequency modal noise caused by body vibration in the measured acceleration, wherein high-frequency modal noise refers to modal noise with a frequency greater than 5Hz.

5. The heading fusion method based on GNSS velocity observation according to claim 1, characterized in that, The step of transforming the velocity increment in the machine system to the NED navigation coordinate system using the attitude cosine matrix based on the preprocessed acceleration computer system to obtain the navigation system velocity increment includes: Based on formula Calculate the speed increment; among which, Preprocessed acceleration sampling data for measuring acceleration For speed increments in the machine system, For time intervals; Based on formula The velocity increment in the machine system will be transformed to the NED navigation coordinate system to obtain the navigation system velocity increment; wherein, For the northbound velocity increment under the NED navigation system, Let be the attitude cosine matrix.

6. The heading fusion method based on GNSS velocity observation according to claim 5, characterized in that, The process of acquiring the northward and eastward velocities in the navigation coordinate system from the real-time dynamic differential RTK output and constructing state variables based on the heading angle, to build a state transition equation based on the navigation system velocity increment and perform state prediction and prediction covariance update, includes: Based on the relationship between aircraft velocity and attitude angle, design state variables. ;in, The current northbound velocity. The current eastward velocity is... The heading angle at the current moment; Construct the state transition matrix and driving noise matrix ;in, For eastward velocity increments under the NED navigation system; Based on the state transition matrix and driving noise matrix Construct the state transition equation: ;in, , , All are white noise. The northbound velocity at the previous moment. The eastward velocity at the previous moment. The heading angle at the previous moment; Calculate the prediction covariance matrix : ;in, The updated covariance matrix, Let be the variance matrix of the process noise.

7. The heading fusion method based on GNSS velocity observation according to claim 6, characterized in that, The process of time-aligning the RTK output velocity with the time lag of the IMU predicted state, performing Kalman measurement updates after time alignment, outputting the fused state estimation result at the current moment, and obtaining the heading angle estimate from the fused state estimation result to enter the closed loop includes: The state variables predicted based on IMU data are pushed into a buffer. When the RTK output speed is updated, the predicted state variables corresponding to the speed timestamp of the RTK output are retrieved from the buffer for measurement update; wherein, the measurement update measurement matrix... ; Calculate the Kalman gain matrix at the current time step. ;in, The measurement noise covariance matrix; Update fusion status: and from The heading angle estimate is obtained and connected to the closed loop; whereby... The measurement vector is composed of the northward and eastward velocities output by the RTK. This is the fused state estimation vector.

8. The heading fusion method based on GNSS velocity observation according to claim 7, characterized in that, After updating the fusion state, the process also includes updating the covariance matrix. : ;in, It is a third-order identity matrix.

Images