Unmanned aerial vehicle fusion height estimation method, unmanned aerial vehicle and medium

Abstract

The present application relates to the technical field of unmanned aerial vehicle, provide a kind of unmanned aerial vehicle fusion height estimation method, unmanned aerial vehicle and medium, to solve the problem that RTK and barometer are all invalid when the stable, reliable height estimation value caused by IMU measurement error cannot be provided.The method comprises: high-frequency noise filtering is carried out on the original acceleration of inertial measurement unit to obtain a first acceleration value;The acceleration compensation value is obtained by first-order difference of global positioning system speed;A state space model containing vertical velocity, acceleration and accelerometer zero offset drift error is constructed;The first acceleration and acceleration compensation value are fused by Kalman filtering, and the drift is estimated and stripped in real time;Smooth height is output by trapezoidal integration.The output drift amount is less than 0.3m / min, the bandwidth is greater than or equal to 2Hz, and the height estimation value is continuous throughout the process, which is suitable for stable flight and uninterrupted operation when RTK and barometer are all invalid in complex scenes such as agricultural plant protection.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) technology, and more particularly to a UAV fusion altitude estimation method, a UAV, and a medium. Background Technology

[0002] Altitude information is one of the key state variables for UAVs to achieve stable flight and precise operations. Currently, UAV altitude information mainly relies on three sources: absolute altitude provided by the Global Navigation Satellite System (GNSS), static pressure altitude calculated by the barometer, and relative altitude change obtained by the Inertial Measurement Unit (IMU) through acceleration integration. However, in complex operational scenarios such as agricultural plant protection, UAVs often face various interferences such as terrain obstruction, weather changes, and ionospheric activity, leading to satellite signal loss, loss of fixed solutions in real-time dynamic positioning, and a sharp decline in altitude measurement accuracy. Simultaneously, due to the overall structure of the UAV and its high-speed flight attitude, the static pressure measured by the barometer is prone to severe disturbances and low-frequency drift, resulting in seriously inaccurate altitude output. Therefore, in situations where both real-time dynamic positioning and the barometer fail or their performance is severely degraded, providing UAVs with continuous, stable, and bandwidth-available fused altitude becomes a critical issue for ensuring flight safety and operational continuity.

[0003] Existing technologies often employ multi-sensor information fusion and redundancy backup strategies to address the aforementioned problems. For example, Chinese invention patent CN201710061088.1 discloses a UAV altitude measurement method and system based on multi-sensor information fusion. This method prioritizes sensors (e.g., radar / ultrasonic sensors, GPS, and barometers) and switches between them. When high-priority sensors are available, the zero bias of low-priority sensors is estimated and corrected online. The resulting data is then fused with the integrated altitude from the IMU through complementary filtering to maintain the consistency of altitude information. Another example is Chinese invention patent CN201610740929.7, which proposes an altitude information fusion method resistant to atmospheric parameter drift. This method utilizes a base station and an onboard barometer to construct a differential barometric altitude observation to suppress atmospheric drift and fuses barometric altitude and IMU acceleration information through a combination of complementary filtering and Kalman filtering.

[0004] However, the above solutions do not fully consider the inherent error characteristics of the IMU, especially the modal noise caused by body vibration and the time-varying drift of the sensor itself. If the system directly relies on the uncompensated raw IMU data for integration after the RTK and barometer fail, the altitude error will diverge cubically over time, making it impossible to maintain effective closed-loop control, and stable and reliable altitude information will be lost within seconds. Summary of the Invention

[0005] The purpose of this invention is to provide a method for fusion altitude estimation of unmanned aerial vehicles (UAVs), the UAV, and the medium, in order to solve the problem that stable and reliable altitude estimates cannot be provided due to IMU measurement errors when both RTK and barometer fail.

[0006] To achieve the above objectives, this application proposes the following technical solution:

[0007] In a first aspect, the present invention proposes a method for fusion altitude estimation of unmanned aerial vehicles (UAVs), comprising:

[0008] S1: Perform high-frequency noise filtering on the raw acceleration measurement value output by the inertial measurement unit to obtain the first acceleration value; and perform differentiation processing on the velocity measurement value output by the global positioning system to obtain the acceleration compensation value;

[0009] S2: Construct a state-space model with the vertical velocity, vertical acceleration, and accelerometer zero-bias drift error of the UAV as state variables; the state-space model includes a dynamic model of the accelerometer zero-bias drift error and a reduced-order model of the vertical motion of the UAV.

[0010] S3: Using a Kalman filter, the first acceleration value and the acceleration compensation value are used as observations and fused with the state-space model. Joint estimation of the state variables is then performed, outputting a joint optimal state estimation vector including the optimal vertical velocity estimate. The joint estimation includes:

[0011] Based on the first acceleration value, the estimate of the vertical acceleration and the zero-bias drift error of the accelerometer is updated through the first observation equation;

[0012] The velocity measurements are time-aligned, and the estimate of the vertical velocity is updated based on the aligned velocity measurements. The estimate of the vertical acceleration is also updated based on the acceleration compensation value using a second observation equation.

[0013] S4: Based on the optimal vertical velocity estimate, the fusion height estimate is calculated using the trapezoidal integral method.

[0014] Specifically, the time alignment process involves obtaining the corresponding historical state variable estimates from the state history buffer based on the timestamp of the GPS velocity measurement value, in order to construct the observation equation.

[0015] Furthermore, the state history buffer is used to store state estimates at historical moments. When GPS data arrives, the state estimate matching the historical moment is retrieved from the buffer based on its timestamp, denoted as... , where v p a p δap These are the estimated values ​​of vertical velocity, vertical acceleration, and accelerometer zero-bias drift error at the corresponding time points, in m / s² and m / s², respectively. 2 m / s 2 Then, GPS data is used to update these historical states, thereby achieving temporal alignment and effective data fusion.

[0016] Specifically, S1 performs high-frequency noise filtering on the raw acceleration measurement value output by the inertial measurement unit (IMU), which suppresses high-frequency vibration noise of the aircraft and obtains the first acceleration value. It also performs first-order difference on the velocity measurement value output by the global positioning system (GPS) to obtain an auxiliary acceleration observation independent of IMU drift. This step constructs a complementary dual-channel system at the data source level, consisting of the first acceleration value and the acceleration compensation value. The aim is to suppress high-frequency modal noise caused by aircraft vibration while obtaining a clean acceleration observation independent of IMU drift, providing a highly reliable raw input for subsequent fusion.

[0017] The accelerometer zero-bias drift error described in S2 is a component of the accelerometer's measurement zero bias in the inertial measurement unit that changes slowly over time. Its dynamic characteristics can be described by a first-order autoregressive model and it is the main error source causing the simple integral altitude to diverge over time. This step establishes a state-space model that integrates the dynamic model of the accelerometer zero-bias drift error and the reduced-order model of the UAV's vertical motion. The integrated accelerometer drift dynamic model tracks the time-varying error of the sensor in real time, while the reduced-order model of the UAV's vertical motion provides prior knowledge of the motion trend, laying the foundation for estimating the true state from noisy data in the event of sensor failure.

[0018] S3 utilizes a Kalman filter to fuse the first acceleration value and acceleration compensation value from S1 into a complementary dual-channel system with the state-space model of S2. The aim is to achieve a single observation correcting three states: vertical velocity, acceleration, and accelerometer drift. This process effectively decouples real motion from sensor drift and suppresses altitude integral divergence. Even under extreme conditions of RTK and barometer failure, altitude integral divergence remains within a controllable range.

[0019] S4 numerically integrates the optimal vertical velocity estimate output by S3 using the trapezoidal integral method to obtain the fused altitude estimate. This step processes data continuously at 100Hz, synchronized with the IMU, resulting in a fused altitude estimate with an effective signal bandwidth of at least 2Hz. This meets the dynamic response requirements of UAV vertical control and can be directly fed into the flight control vertical closed loop. Furthermore, compared to simple rectangular integration, this method calculates the accumulation of speed over time with higher accuracy, resulting in a smoother and more accurate fused altitude estimate with a drift of <0.3m / min, supporting continuous and stable flight and control of the UAV.

[0020] Specifically, the method is based on dual-channel observations with different update frequencies:

[0021] The inertial measurement unit continuously outputs the raw acceleration measurement value, which is then filtered and provided at a first frequency as the first acceleration value.

[0022] The global positioning system periodically outputs velocity measurements, which, after differentiation, provide acceleration compensation values ​​at a second frequency lower than the first frequency.

[0023] Furthermore, the first frequency is 100Hz and the second frequency is 10Hz; the Kalman filter is configured to fuse the dual-channel observation information to achieve a joint optimal estimate of the state variables.

[0024] Specifically, the high-frequency noise filtering of the acceleration measurement values ​​of the inertial measurement unit includes:

[0025] Based on the analysis of the vibration modes of the UAV body, a low-pass filter is designed;

[0026] The low-pass filter is used to filter out the high-frequency modal noise caused by the vibration of the body in the acceleration measurement to obtain the first acceleration value.

[0027] Furthermore, the low-pass filter is a second-order Butterworth filter with a cutoff frequency of 4Hz. This step designs the filter based on the measured analysis of the UAV's vibration modes, enabling the solution to target the high-frequency vibration bands that have the greatest impact on measurement accuracy, specifically tailored to the vibration spectrum characteristics of the UAV model. Setting the low-pass filter cutoff frequency to 4Hz is based on the engineering judgment that the vertical motion frequency of the UAV is usually lower than this value. This allows for the filtering out vibration noise above 5Hz while preserving the low-frequency acceleration signal of the actual motion to the greatest extent possible, avoiding phase lag or signal distortion introduced by over-filtering.

[0028] Specifically, the differential processing of the velocity measurement value output by the Global Positioning System is achieved through first-order difference:

[0029] ,

[0030] Where k represents the current time and k-1 represents the previous time, both of which are dimensionless; v gps (k) and v gps (k-1) represent the velocity measurements corresponding to system processing times k and k-1, respectively, in m / s; Δt gps Based on the two observations v gps (k) and v gps The time interval determined by the time information corresponding to (k-1) is in seconds; a diff(k) represents the acceleration compensation value at time k, in m / s². 2 The purpose of this approach is to leverage the high accuracy and stable error characteristics of the Global Positioning System (GPS), which can be considered constant in the short term, to extract an auxiliary acceleration observation from the velocity information using first-order difference, unaffected by IMU time-varying drift. Its advantages lie in providing the system with an independent and reliable second observation channel, effectively complementing the filtered IMU acceleration data. This allows subsequent fusion algorithms to distinguish and accurately estimate both the actual body motion acceleration and the IMU's own drift error, significantly improving the robustness and accuracy of estimation even under extreme conditions such as GNSS positioning failure.

[0031] Specifically, the state-space model is constructed in the following way:

[0032] The dynamic model for the zero-bias drift error of the accelerometer is established as follows:

[0033] ,

[0034] Where, δ a (k) and δ a (k-1) represents the state estimates of the accelerometer zero-bias drift error at times k and k-1, in m / s². 2 ; The state transition coefficient with zero offset drift error is dimensionless. Process noise, in m / s 2 ;

[0035] Furthermore, a dynamic model for the accelerometer zero-bias drift error is used to track and predict the time-varying drift error of the accelerometer as an internal system state in real time. Based on wavelet spectrum analysis and ARMA modeling system identification of historical flight data, the dynamic characteristics of the accelerometer zero-bias drift error can be simplified into an effective description using a first-order Markov process. This allows the system to actively identify and compensate for the key slowly varying errors of the inertial measurement unit, rather than treating them as fixed biases or simple noise, thereby significantly improving the processing accuracy of drift-containing sensor data.

[0036] The reduced-order model of the vertical motion of the UAV is established as follows:

[0037] ,

[0038] Where a(k) and a(k-1) are the estimated vertical acceleration states at time k and time k-1, respectively, in m / s². 2 ;c a a is the transfer coefficient, dimensionless; cmd (k-1) represents the vertical acceleration control command at time k-1, in m / s².2 ;

[0039] Where k is the current time and k-1 is the previous time, both of which are dimensionless.

[0040] Furthermore, a reduced-order model of the UAV's vertical motion is used to incorporate the aircraft's basic dynamic characteristics as prior knowledge into the state estimation process. This model uses the acceleration state and control commands from the previous moment to predict the acceleration at the current moment, providing the filter with physically consistent motion trend constraints. This enhances the rationality and robustness of the state estimation when external observation information is insufficient or unreliable.

[0041] Specifically, the joint estimation of the state variables includes:

[0042] Based on the state transition equation in the state-space model, and the state estimate from the previous moment, predict the vertical velocity, vertical acceleration, and accelerometer zero-bias drift error at the current moment.

[0043] Using a Kalman filter, the first acceleration value and the acceleration compensation value are processed synchronously to update the estimate of the state variable, including:

[0044] Based on the first acceleration value, the estimate of the vertical acceleration and the zero-bias drift error of the accelerometer is updated using the first observation equation.

[0045] Based on the acceleration compensation value, the estimate of the vertical acceleration is updated using the second observation equation;

[0046] The formula for the first observation equation is:

[0047] ,

[0048] Among them, a mes (k) represents the first acceleration value at time k, in m / s². 2 a(k) is the vertical acceleration state value to be estimated at time k, in m / s². 2 δ a (k) represents the accelerometer zero-bias drift error state value to be estimated at time k, in m / s². 2 ω a (k) represents the first acceleration value a mes The observation noise of (k), in m / s 2 ;

[0049] The formula for the second observation equation is:

[0050] ,

[0051] Among them, adiff (k) represents the acceleration compensation value at time k, in m / s². 2 a(k) is the vertical acceleration state value to be estimated at time k, in m / s². 2 , For acceleration compensation value a diff The observation noise of (k), in m / s 2 .

[0052] The purpose of this approach is to provide the Kalman filter with sufficient information to decouple key state variables using the first and second observation equations. The first observation equation correlates the IMU's acceleration observations with both the actual acceleration and sensor drift, while the second observation equation correlates the GPS acceleration compensation value with only the actual acceleration. The advantage is that the filter, based on these two complementary observation channels, can synchronously and accurately separate and estimate the UAV's actual vertical acceleration from the accelerometer's drift error using optimal estimation algorithms. This avoids altitude estimation divergence caused by accumulated sensor errors at the source, achieving a pure estimation of the motion state.

[0053] Specifically, the joint estimation also includes:

[0054] The Kalman filter calculates the Kalman gain and dynamically allocates the observation residuals from the first acceleration value and the acceleration compensation value to the estimation corrections of the vertical velocity, the vertical acceleration, and the accelerometer zero-bias drift error, so as to synchronously output the joint optimal estimate of the vertical velocity, vertical acceleration, and accelerometer zero-bias drift error at the current moment. The purpose of this is to achieve intelligent and accurate correction of multiple related state variables based on multi-source observation information. Its core advantage is that it allows the system to automatically determine which sensor is more reliable and intelligently and accurately allocate the correction amount to the three interrelated states of velocity, acceleration, and drift error, achieving synchronous optimal updates.

[0055] Specifically, the formula for calculating the trapezoidal integral is as follows:

[0056] ,

[0057] Where k is the current time and k-1 is the previous time, both of which are dimensionless; This is the estimated fusion height at time k, in meters. This is the estimated fusion height of the previous time step k-1, in meters. This is the optimal vertical velocity estimate at time k, in m / s; dt is the optimal vertical velocity estimate of the previous time k-1, in m / s; dt is the time interval between the current time k and the previous time k-1, in seconds.

[0058] The process of calculating the fused height estimate using the trapezoidal integral method is executed continuously at a frequency no lower than the data output frequency of the inertial measurement unit, with the frequency no lower than 50Hz. The purpose of this is to transform the optimal vertical velocity estimate obtained after fusion and error compensation into the final required height information through time integration, while ensuring that the bandwidth of the fused height estimate is ≥2Hz.

[0059] In a second aspect, the present invention provides an unmanned aerial vehicle (UAV) including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the method described in any of the above-mentioned embodiments.

[0060] Thirdly, the present invention also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the method described in any one of the above-mentioned methods.

[0061] Beneficial effects:

[0062] As can be seen from the above technical solutions, the technical solution of the present invention provides a method for fusion altitude estimation of UAVs, UAVs and media, which has the following advantages compared with the prior art: 1. In terms of the stability of altitude estimation values, under the condition that both RTK and barometer fail, the traditional method for integrating altitude by IMU is prone to rapid divergence due to sensor drift. However, this technology obtains the first acceleration value by filtering the original acceleration of the inertial measurement unit through high-frequency noise and obtains acceleration compensation by differentiating the velocity of the global positioning system, constructing a complementary dual channel, and using Kalman filtering to synchronously estimate and remove accelerometer drift, and then using trapezoidal integration to restore altitude, thus constructing a closed-loop correction loop that includes drift estimation, achieving drift <0.3m / min and a smooth altitude curve without divergence.

[0063] 2. Regarding the availability of altitude estimates, in situations where both the RTK and barometer fail, traditional techniques integrate the uncompensated IMU acceleration twice, causing the error to diverge cubically over time, resulting in the loss of usable altitude within seconds. This technology, however, uses Kalman filtering to fuse dual-channel observations and updates with trapezoidal integrals for high-precision time-domain accumulation, thereby suppressing drift in real time. The fused altitude is continuously output with a bandwidth of ≥2Hz, which can be directly connected to the vertical closed loop of the flight control system. This ensures that the UAV can continue spraying, obstacle avoidance, and precise landing even after both the RTK and barometer fail, without interrupting operations.

[0064] 3. Regarding the anti-interference and robustness of the altitude estimation, this invention effectively suppresses body vibration noise by performing targeted high-frequency noise filtering on the IMU acceleration, such as using a 4Hz Butterworth low-pass filter based on body vibration mode analysis; at the same time, it utilizes GPS velocity differential to provide acceleration observations independent of IMU drift, constructing a dual-channel complementary observation system, which can still maintain the reliability of altitude estimation when GNSS signals are temporarily lost or disturbed.

[0065] 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.

[0066] 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

[0067] 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:

[0068] Figure 1 This is a flowchart of a drone fusion altitude estimation method disclosed in this invention.

[0069] Figure 2 This is a schematic diagram of a Kalman filter data fusion and state estimation framework based on the method disclosed in this invention.

[0070] Figure 3 This is a comparison diagram of the overall height channels based on the method disclosed in this invention and the RTK fixed solution method.

[0071] Figure 4 This is a partial comparison diagram of the height channel based on the method disclosed in this invention and the RTK fixed solution method. Detailed Implementation

[0072] 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.

[0073] 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.

[0074] Before introducing this application, the relevant technologies and concepts will be introduced first.

[0075] An inertial measurement unit (IMU) is a combination of an accelerometer and a gyroscope mounted on a drone, used to output raw acceleration and angular velocity.

[0076] The Global Positioning System, or GPS, outputs the absolute speed of the UAV by receiving satellite signals; this application performs a first-order difference on its speed to obtain an acceleration compensation value independent of IMU drift.

[0077] The Global Navigation Satellite System (GNSS) is a satellite navigation system composed of multiple constellations, including GPS, BeiDou, and GLONASS, which provides absolute position and speed information for drones.

[0078] RTK, or Real-Time Dynamic Differential, uses carrier phase correction transmitted from ground-based reference stations to instantly reduce GNSS positioning accuracy from meters to centimeters. It should be noted that in the operating conditions addressed in this application, RTK failure primarily refers to the loss of the real-time dynamic differential positioning function, resulting in the inability to obtain centimeter-level position information. However, GPS's single-point velocity measurement function, due to its relatively high accuracy and good short-term stability, can generally still serve as a reliable auxiliary information source in such scenarios to solve altitude estimation problems.

[0079] A barometer, a barometric sensor that converts altitude by measuring static pressure, is susceptible to disturbances to the body and changes in weather, resulting in low-frequency drift. In this application, it is used as a replacement source in barometer failure scenarios.

[0080] High-frequency noise filtering is a process of low-pass processing of the raw IMU signal to remove high-frequency modal noise caused by body vibration and retain true motion information.

[0081] The first acceleration value is the vertical acceleration of the IMU after low-pass filtering, which serves as the first observation for subsequent Kalman filtering.

[0082] The acceleration compensation value, which is the first-order difference result of GPS velocity, constitutes a second path observation that complements the first acceleration value, and is used to decouple real motion from sensor drift.

[0083] The Kalman filter is an algorithm that uses complementary dual-channel observations to make optimal estimates of a state-space model, and is used to simultaneously output optimal estimates of vertical velocity, acceleration and drift.

[0084] The joint optimal state estimation vector, which is the result of the Kalman filter update per cycle, contains the optimal vertical velocity estimate and is used for subsequent height integration.

[0085] Trapezoidal integration, a numerical algorithm that accumulates the optimal vertical velocity in the time domain, is more accurate and has less phase lag than rectangular integration.

[0086] The core of this invention lies in the fact that, under extreme conditions where both RTK and barometer fail, complementary dual-channel acceleration observations are obtained by performing high-frequency noise filtering on IMU acceleration and first-order difference on GPS velocity. A state-space model containing vertical velocity, vertical acceleration, and accelerometer drift error is constructed. Joint estimation and error stripping are performed through Kalman filtering. Finally, a continuous, stable, and bandwidth-available fused altitude estimate is output through trapezoidal integration to meet the flight control requirements of UAVs in complex operational scenarios.

[0087] The following detailed description, in conjunction with the specific structure of the embodiments shown in the accompanying drawings, provides a further detailed account of the UAV fusion altitude estimation method, the UAV, and the medium disclosed in this invention.

[0088] Example 1: As Figure 1 As shown, this embodiment proposes a UAV fusion altitude estimation method, which includes the following steps:

[0089] S1: Perform high-frequency noise filtering on the raw acceleration measurement value output by the inertial measurement unit to obtain the first acceleration value; and perform differentiation processing on the velocity measurement value output by the global positioning system to obtain the acceleration compensation value;

[0090] S2: Construct a state-space model with the vertical velocity, vertical acceleration, and accelerometer zero-bias drift error of the UAV as state variables; the state-space model includes a dynamic model of the accelerometer zero-bias drift error and a reduced-order model of the vertical motion of the UAV.

[0091] S3: Using a Kalman filter, the first acceleration value and the acceleration compensation value are used as observations and fused with the state-space model. Joint estimation of the state variables is then performed, outputting a joint optimal state estimation vector including the optimal vertical velocity estimate. The joint estimation includes:

[0092] Based on the first acceleration value, the estimate of the vertical acceleration and the zero-bias drift error of the accelerometer is updated through the first observation equation;

[0093] The velocity measurements are time-aligned, and the estimate of the vertical velocity is updated based on the aligned velocity measurements. The estimate of the vertical acceleration is also updated based on the acceleration compensation value using a second observation equation.

[0094] S4: Based on the optimal vertical velocity estimate, the fusion height estimate is calculated using the trapezoidal integral method.

[0095] Specifically, S1 includes the following steps:

[0096] First, high-frequency noise filtering is performed on the raw IMU acceleration. During UAV flight, body vibration generates high-frequency modal noise, directly affecting the accelerometer's measurement accuracy. Through analysis of a large amount of flight data and modal testing, the main vibration modes were identified at frequencies of 5Hz, 7Hz, 14Hz, and 20Hz, with the 7Hz mode exhibiting a relatively high energy peak. To suppress this high-frequency noise, this invention designs a second-order Butterworth low-pass filter with a cutoff frequency of 4Hz. This filter exhibits almost no attenuation below 2Hz and attenuates by approximately 5dB at 5Hz, effectively filtering out high-frequency noise caused by modes above 5Hz while retaining the main low-frequency components of the UAV's vertical motion, typically below 4Hz. The filtered acceleration is referred to as the first acceleration value, serving as one observation for subsequent Kalman filtering.

[0097] Then, a first-order difference is performed on the GPS velocity. Because GPS speed measurement is highly accurate and its short-term error can be considered constant, a first-order difference can be performed on the GPS velocity to obtain an acceleration compensation value 'a' independent of IMU drift. diff (k), the calculation formula is as follows:

[0098] ,

[0099] Where k represents the current time and k-1 represents the previous time, both of which are dimensionless; v gps (k) and v gps (k-1) represent the velocity measurements corresponding to system processing times k and k-1, respectively, in m / s; Δt gps Based on the two observations v gps (k) and v gps The time interval determined by the time information corresponding to (k-1) is in seconds; a diff (k) represents the acceleration compensation value at time k, in m / s². 2 This acceleration compensation value constitutes a second way-view measurement that complements the first acceleration value, used to decouple real motion from sensor drift.

[0100] Specifically, S2 includes the following steps:

[0101] The state-space model uses the UAV's vertical velocity v, vertical acceleration a, and accelerometer zero-bias drift error δ. a For state variables, i.e., state vectors .

[0102] The dynamic model of the accelerometer zero-bias drift error is derived by performing wavelet analysis and ARMA modeling on historical flight data. It was found that the accelerometer drift error can be approximately described by a first-order autoregressive model AR(1), i.e., a first-order Markov process:

[0103] ,

[0104] Where, δ a (k) and δ a (k-1) represents the state estimates of the accelerometer zero-bias drift error at times k and k-1, in m / s². 2 ; The state transition coefficient with zero offset drift error is dimensionless. Process noise, in m / s 2 ;

[0105] The reduced-order model of the vertical motion of the UAV is established as follows:

[0106] ,

[0107] Where a(k) and a(k-1) are the estimated vertical acceleration states at time k and time k-1, respectively, in m / s². 2 ;c a a is the transfer coefficient, dimensionless; cmd (k-1) represents the vertical acceleration control command at time k-1, in m / s². 2 ;

[0108] Where k is the current time and k-1 is the previous time, both of which are dimensionless.

[0109] Based on the above model, the state transition equation is:

[0110] ,

[0111] Where F is the state transition matrix and G is the noise driving matrix, as follows:

[0112] , ,

[0113] in Let be the process noise vector, and let Q be its covariance matrix.

[0114] Specifically, S3 includes the following steps:

[0115] The Kalman filter is used to fuse the dual-channel observations in S1 with the state-space model in S2 to achieve joint optimal estimation of the state variables.

[0116] Based on the state estimate from the previous moment The covariance matrix P k−1 The formula for predicting the current state and covariance is as follows:

[0117] ,

[0118] ,

[0119] in Let be the prior state estimate at time k, and F be the state transition matrix; Compared with prior state estimation The corresponding prior estimation error covariance matrix represents the uncertainty of the prediction; G is the process noise driving matrix; Q is the process noise covariance matrix, representing the uncertainty of the system model itself.

[0120] Based on the currently available observation data, select the appropriate observation matrix H for updating.

[0121] When the IMU data is updated, the first observation equation is used:

[0122] , ,

[0123] Among them, a mes (k) represents the first acceleration value at time k, in m / s². 2 H1 is the observation matrix for the IMU, which is dimensionless; the state vector is... The units are m / s and m / s. 2 m / s 2 ;ω a (k) represents the first acceleration value a mes The observation noise of (k), in m / s 2 .

[0124] Equation expansion: ,

[0125] Right now: ;

[0126] When GPS data is updated, the second observation equation is used:

[0127] , ,

[0128] Among them, v gps (k) represents the vertical velocity measured by GPS, in m / s; a diff (k) represents the acceleration compensation value at time k, in m / s². 2 H2 is the GPS observation matrix, which is dimensionless; x p (k) represents the time-delayed state variable retrieved from the buffer at the corresponding moment. The units are m / s and m / s. 2 m / s 2 ; This is GPS velocity observation noise, in m / s; The observed noise for GPS acceleration compensation values ​​is expressed in m / s². 2 ;

[0129] Equation expansion:

[0130] ,

[0131] Right now: ;

[0132] Then calculate the Kalman gain and state update:

[0133]

[0134] Where K is the Kalman gain matrix; H is the observation matrix; and I is the identity matrix. R is the observation noise covariance matrix. Let k be the prior state estimate at time k; To estimate prior state The corresponding prior estimation error covariance matrix; z(k) represents the observation vector at time k, that is, the actual sensor measurement data available at the current time. The observation residual represents the difference between the actual and expected observations; depending on the sensor type, z(k) is composed as follows:

[0135] When only IMU data is updated: z(k) = a mes (k) represents a scalar observation in m / s. 2 ;

[0136] When GPS data is updated: z(k) = [v gps (k)a diff (k)] T , are two-dimensional observation vectors, with units of m / s and m / s respectively. 2 .

[0137] Specifically, S4 includes the following steps:

[0138] Use the optimal vertical velocity estimate output by S3 The estimated fusion height is calculated using the trapezoidal integral method:

[0139] ,

[0140] Where dt is the time interval. Compared with rectangular integral, this method has higher accuracy and smaller phase lag, and can output a smooth and continuous height curve.

[0141] Figure 2 This is a schematic diagram of a Kalman filter data fusion and state estimation framework based on the method disclosed in this invention.

[0142] This framework mainly includes a state prediction module, a measurement update module, a state estimation buffer, and a multi-source sensor data input channel. Its workflow is as follows:

[0143] The multi-source sensor data input channels include: the IMU channel, which collects the raw acceleration data of the UAV in real time and obtains the first acceleration value after low-pass filtering; and the GPS channel, which collects the speed data of the UAV in real time and obtains the acceleration compensation value after first-order differential processing.

[0144] The state prediction module, based on the optimal state estimate and its covariance matrix from the previous time step, predicts the prior state value and its prior covariance matrix for the current time step using the state transition matrix. The measurement update module, based on currently available sensor data (IMU or GPS), selects the appropriate observation matrix to construct the observation equation.

[0145] Calculate the Kalman gain, use the observation residuals to correct the state prediction, and obtain the optimal posterior state estimate and its posterior covariance matrix at the current time.

[0146] The state estimation buffer stores the state estimates at historical moments. It is used for time alignment and data synchronization when there is a time delay in GPS data, ensuring the consistency of the fusion time.

[0147] Through the above prediction update loop, the joint optimal estimation of vertical velocity, vertical acceleration and accelerometer drift error is achieved. Finally, based on the optimal vertical velocity estimate, a continuous, stable and bandwidth-available fused height estimate is obtained by trapezoidal integration.

[0148] Example 2: This example provides a drone, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of the method described in any of the above examples. This drone can be used in complex scenarios such as agricultural plant protection, surveying, and inspection, and can still maintain stable flight and continuous operation even when RTK and barometer failure occur.

[0149] Example 3: This example also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the method described in any of the above examples. This medium can be an embedded memory in a flight control system or an external storage device, used to persist and execute the altitude estimation algorithm.

[0150] Furthermore, the performance indicators of the method described in this invention were verified through a real-world flight test process: During the test, the RTK fixed solution was artificially shielded in an open area, and the barometer data was considered invalid; the method was run solely based on IMU and GPS speed data. During the hovering phase, the fused altitude output was recorded continuously for 10 minutes, and its linear trend slope was calculated, yielding an integral drift rate of <0.3 m / min. By controlling the UAV to perform a vertical frequency sweep motion, the frequency response of the fused altitude and the high-precision RTK altitude was compared, confirming that its -3dB cutoff frequency was not lower than 2Hz. Throughout the entire flight's takeoff, landing, and maneuvering, the fused altitude curve remained continuous without any jumps.

[0151] Figure 3 and Figure 4 This is a comparison diagram of the overall and local height channels based on the method disclosed in this invention and the RTK fixed solution method. For example... Figure 3As shown, throughout the entire process of the drone's takeoff, flight, and landing, the fused altitude and RTK altitude closely match, exhibiting a smooth and continuous trend without jumps or divergences. Figure 4 As shown, the fused height curve and the RTK curve are consistent in terms of detailed fluctuations, indicating that the output can truly reflect the instantaneous vertical motion. During high-dynamic maneuvers such as rapid ascent or descent, the fused height can still closely follow the RTK reference, indicating that its effective bandwidth meets the control requirements. The deviation between the fused height and the RTK remains at the centimeter level throughout the entire process, and there is no cumulative drift trend, which verifies the effectiveness of modeling to compensate for accelerometer drift error.

[0152] In scenarios where both RTK and barometer fail, this method can not only output continuous and stable altitude information with a drift rate of <0.3m / min and a bandwidth of ≥2Hz, but also has high dynamic tracking capability and can be directly connected to the vertical closed loop of the flight control system to ensure continuous and stable operation of the UAV.

[0153] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any altitude estimation achieved by those skilled in the art using velocity observations from other global navigation satellite systems besides GPS, such as BeiDou and GLONASS, through the same or similar error modeling, filtering, and fusion methods, within the technical principles disclosed in the present invention, should be included within the scope of protection of the present invention.

Claims

1. A method for fusion altitude estimation of unmanned aerial vehicles (UAVs), characterized in that, include: S1: Perform high-frequency noise filtering on the raw acceleration measurement value output by the inertial measurement unit to obtain the first acceleration value; and perform differentiation processing on the velocity measurement value output by the global positioning system to obtain the acceleration compensation value; S2: Construct a state-space model with the UAV's vertical velocity, vertical acceleration, and accelerometer zero-bias drift error as state variables; the state-space model includes a dynamic model of the accelerometer zero-bias drift error and a reduced-order model of the UAV's vertical motion; wherein, the dynamic model of the accelerometer zero-bias drift error is established as follows: , where δ a (k) and δ a (k-1) represents the state estimates of the accelerometer zero-bias drift error at time k and time k-1; The state transition coefficient with zero offset drift error; To account for process noise, a reduced-order model of the vertical motion of the UAV is established as follows: Where a(k) and a(k-1) are the estimated vertical acceleration states at time k and time k-1, respectively; c a For the transfer coefficient; a cmd (k-1) represents the vertical acceleration control command at time k-1; where k is the current time and k-1 is the previous time. S3: Using a Kalman filter, the first acceleration value and the acceleration compensation value are used as observations and fused with the state-space model. Joint estimation of the state variables is then performed, outputting a joint optimal state estimation vector including the optimal vertical velocity estimate. The joint estimation includes: Based on the first acceleration value, the estimate of the vertical acceleration and the accelerometer zero-bias drift error is updated using a first observation equation; the first observation equation is: , where a mes (k) represents the first acceleration value at time k, a(k) represents the vertical acceleration state value to be estimated at time k, and δ a (k) represents the accelerometer zero-bias drift error state value to be estimated at time k, ω a (k) represents the first acceleration value a mes Observation noise of (k); The velocity measurements are time-aligned, and the estimate of the vertical velocity is updated based on the aligned velocity measurements. The estimate of the vertical acceleration is also updated based on the acceleration compensation value using a second observation equation. The second observation equation is: , where a diff (k) represents the acceleration compensation value at time k, and a(k) represents the vertical acceleration state value to be estimated at time k. For acceleration compensation value a diff Observation noise of (k); S4: Based on the optimal vertical velocity estimate, the fusion height estimate is calculated using the trapezoidal integral method.

2. The UAV fusion altitude estimation method according to claim 1, characterized in that, The high-frequency noise filtering of the acceleration measurement values ​​of the inertial measurement unit specifically includes: Based on the analysis of the vibration modes of the UAV body, a low-pass filter is designed; The low-pass filter is used to filter out the high-frequency modal noise caused by the vibration of the body in the acceleration measurement to obtain the first acceleration value.

3. The UAV fusion altitude estimation method according to claim 2, characterized in that, The low-pass filter is a second-order Butterworth filter with a cutoff frequency of 4Hz.

4. The UAV fusion altitude estimation method according to claim 1, characterized in that, The differential processing is a first-order difference processing, specifically including: ， Where k is the current time and k-1 is the previous time; a diff (k) represents the acceleration compensation value at time k; v gps (k) and v gps (k-1) represent the velocity measurements corresponding to system processing times k and k-1, respectively; Δt gps Based on two observations v gps (k) and v gps The time interval determined by the time information corresponding to (k-1).

5. The UAV fusion altitude estimation method according to claim 1, characterized in that, The joint estimation also includes: The Kalman filter calculates the Kalman gain and dynamically allocates the observation residuals from the first acceleration value and the acceleration compensation value to the estimation correction of the vertical velocity, the vertical acceleration, and the accelerometer zero-bias drift error, so as to synchronously output the joint optimal estimate of the vertical velocity, vertical acceleration, and accelerometer zero-bias drift error at the current moment.

6. The UAV fusion altitude estimation method according to claim 1, characterized in that, The formula for calculating the trapezoidal integral is as follows: ， Where k is the current time and k-1 is the previous time; This is the estimated fusion height at time k. This is the estimated fusion height value at the previous time step k-1; This is the optimal vertical velocity estimate at time k. dt is the optimal vertical velocity estimate of the previous time k-1; dt is the time interval between the current time k and the previous time k-1.

7. A drone, characterized in that, It includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the steps of a UAV fusion altitude estimation method as described in any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that, The device contains a computer program that, when executed by a processor, implements the steps of a UAV fusion altitude estimation method as described in any one of claims 1 to 6.

Images