Unmanned aerial vehicle ground-hugging flight method and device

Abstract

The application provides a UAV ground-hugging flight method and device, and relates to the technical field of UAVs, and comprises the following steps: estimating a target flight state vector of a UAV at a current time according to current sensor data of the UAV and the target flight state vector at a previous time; switching or maintaining the current state of the UAV based on the target flight state vector at the previous time and a route height allowance, and an effective ground-hugging flight mode, to obtain a current effective ground-hugging flight mode; generating a ground-hugging flight control instruction at the current time according to the target flight state vector at the current time under the current effective ground-hugging flight mode; and performing a ground-hugging flight action in response to the ground-hugging flight control instruction. The application can effectively avoid the problem of frequent state oscillation during the ground-hugging flight of the UAV, thereby significantly improving the safety and stability of the ground-hugging flight.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) technology, and in particular to a method and apparatus for UAV ground-following flight. Background Technology

[0002] In existing UAV ground-following flight technology, the altitude protection strategy generally adopts a single threshold instantaneous response mechanism: when the real-time ground altitude estimate is lower than the preset minimum safe altitude threshold, the controller immediately triggers an ascent command, and once the altitude rises back above the minimum safe altitude threshold, the ascent command is canceled, thus causing frequent state oscillations. Summary of the Invention

[0003] In view of this, the purpose of the present invention is to provide a method and apparatus for UAV ground-following flight, which can effectively avoid the problem of frequent state oscillations during UAV ground-following flight, thereby significantly improving the safety and stability of ground-following flight.

[0004] In a first aspect, the present invention provides a method for unmanned aerial vehicle (UAV) ground-following flight, comprising: Based on the current sensor data of the UAV and the target flight state vector at the previous moment, the target flight state vector of the UAV at the current moment is estimated. Based on the target flight state vector and flight path altitude margin at the previous moment, as well as the effective terrain-following flight mode, the current state of the UAV is switched or maintained to obtain the currently effective terrain-following flight mode. In the currently active terrain-following flight mode, generate terrain-following flight control commands based on the target flight state vector at the current moment. It responds to terrain-following flight control commands and executes terrain-following flight maneuvers.

[0005] In one implementation, the current sensor data includes at least vertical velocity observation data and ground altitude observation data; navigation estimation is performed based on the current sensor data of the UAV and the target flight state vector at the previous moment to obtain the target flight state vector at the current moment, including: Based on the target flight state vector at the previous moment, predict the initial flight state vector at the current moment; wherein, the initial flight state vector includes the acceleration zero bias error estimate, the vertical velocity estimate, and the ground altitude estimate; If the ground altitude observation data is not invalid, determine whether to downgrade from dual observation mode to single observation mode based on the vertical velocity observation data, so as to determine the target observation mode and its corresponding target observation vector; wherein, dual observation mode is a mode that observes based on ground altitude observation data and vertical velocity observation data, and single observation mode is a mode that observes based only on ground altitude observation data. Based on the target observation vector and the target flight state vector at the previous moment, the initial flight state is updated to obtain the target flight state vector at the current moment.

[0006] In one implementation, the initial flight state is updated based on the target observation vector and the target flight state vector at the previous moment to obtain the target flight state vector at the current moment, including: Determine the residual matrix between the target observation vector and the initial flight state vector; Based on the target observation noise variance and residual matrix at the current moment, the initial flight state vector is updated to obtain the target flight state vector at the current moment; In the dual-observation mode, the residual matrix includes the vertical velocity residual and the ground altitude residual; in the single-observation mode, the residual matrix includes the ground altitude residual; the target observation noise variance is determined based on the target observation noise variance at the previous moment, the ground altitude residual at the current moment, and their corresponding statistics.

[0007] In one implementation, in the event that ground altitude observation data is unavailable, the method further includes: The estimated ground altitude is updated incrementally with the maximum ground altitude as a constraint. Based on the zero-biased acceleration error estimate, the vertical velocity estimate, and the incrementally updated ground altitude estimate, the target flight state vector at the current moment is determined.

[0008] In one implementation, based on the target flight state vector and flight path altitude margin of the previous moment, and the currently active terrain-following flight mode, the current state of the UAV is switched or maintained to obtain the currently active terrain-following flight mode, including: Extract the target altitude value from the target flight state vector at the previous moment; Determine whether the target altitude value above the ground and the altitude margin of the flight path at the previous moment meet the preset mode switching conditions for switching from the active terrain-following flight mode to other terrain-following flight modes. If yes, switch from the currently active terrain-following flight mode to another terrain-following flight mode; otherwise, keep the currently active terrain-following flight mode to obtain the currently active terrain-following flight mode. The terrain-following flight mode includes ascent mode, altitude hold mode, and descent mode. The preset mode switching conditions are determined based on one or more of the following: minimum safe ground altitude, maximum safe ground altitude, altitude hysteresis, and heading margin hysteresis. Altitude hysteresis is a tolerance range set in the altitude direction, and heading margin hysteresis is a tolerance range set in the heading altitude margin.

[0009] In one implementation, the preset mode switching condition for switching from altitude holding mode to descent mode includes: the target ground altitude value at the previous moment is less than the minimum safe ground altitude; The preset mode switching conditions for switching from altitude hold mode to ascent mode include: the target ground altitude value at the previous moment is greater than the maximum safe ground altitude, and the route altitude margin at the previous moment is greater than the heading margin hysteresis. The preset mode switching conditions for switching from descent mode to altitude hold mode include: the target ground altitude value at the previous moment is greater than the first altitude threshold, and the flight path altitude margin at the previous moment is positive. The first altitude threshold is determined based on the minimum safe ground altitude and altitude hysteresis. The preset mode switching conditions for switching from ascent mode to altitude hold mode include: the target ground altitude value at the previous moment is less than the second altitude threshold, which is determined based on the maximum ground altitude and altitude hysteresis.

[0010] In one implementation, under the currently active terrain-following flight mode, terrain-following flight control commands are generated based on the target flight state vector at the current moment, including: Extract the target altitude value from the target flight state vector at the current moment; In the currently active ground-following flight mode, the flight altitude target is determined based on the ground altitude target value at the current moment; The dynamic integral limit value is determined based on the altitude error between the ground target altitude value and the flight target altitude value at the current moment; Based on the dynamic integral limit value, the terrain-following flight control command at the current moment is determined.

[0011] In one implementation, determining the terrain-following flight control command at the current moment based on the dynamic integral limit value includes: The vertical velocity adjustment at the current moment is determined based on the altitude error and the dynamic integral limit value. Determine the horizontal speed scaling factor corresponding to the currently active terrain-following flight mode; By using the vertical speed adjustment and the horizontal speed scaling factor, the speed command is corrected to obtain the terrain-following flight control command at the current moment.

[0012] In one implementation, after correcting the velocity command using the vertical velocity adjustment and the horizontal velocity scaling factor to obtain the terrain-following flight control command for the current moment, the method further includes: If the altitude holding mode is satisfied based on the currently active terrain-following flight mode, freeze the vertical speed command in the terrain-following flight control commands.

[0013] Secondly, the present invention also provides a ground-following flight device for unmanned aerial vehicles, comprising: The navigation estimation module is used to estimate the target flight state vector of the UAV at the current moment based on the current sensor data of the UAV and the target flight state vector at the previous moment. The mode switching or holding module is used to switch or hold the current state of the UAV based on the target flight state vector and flight path altitude margin at the previous moment, as well as the effective terrain-following flight mode, to obtain the currently effective terrain-following flight mode. The guidance and control module is used to generate the current terrain-following flight control command based on the target flight state vector at the current moment in the currently active terrain-following flight mode. The command response module is used to respond to terrain-following flight control commands and execute terrain-following flight maneuvers.

[0014] This invention provides a method and apparatus for UAV terrain-following flight. First, based on the UAV's current sensor data and the target flight state vector from the previous moment, the target flight state vector of the UAV at the current moment is estimated. Then, based on the target flight state vector from the previous moment, the flight path altitude margin, and the currently active terrain-following flight mode, the current state of the UAV is switched or maintained to obtain the currently active terrain-following flight mode. Finally, under the currently active terrain-following flight mode, terrain-following flight control commands are generated based on the target flight state vector at the current moment; and terrain-following flight actions are executed in response to the terrain-following flight control commands. After estimating the target flight state vector of the UAV at the current moment, the above method uses a mode-switching mechanism based on the synergistic effect of memory and hysteresis design to switch or maintain the currently active terrain-following flight mode of the UAV based on the template flight state vector from the previous moment, thereby obtaining the currently active terrain-following flight mode. Based on this, and combined with the target flight state vector at the current moment, terrain-following flight control commands are determined, effectively suppressing state jitter and significantly improving the safety and stability of terrain-following flight.

[0015] Other features and advantages of the invention will be set forth in the following description, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention are realized and obtained through the structures particularly pointed out in the description and the drawings.

[0016] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0017] To more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0018] Figure 1 A schematic flowchart illustrating a method for unmanned aerial vehicle (UAV) ground-following flight provided in an embodiment of the present invention; Figure 2 A schematic diagram of mode switching provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of the structure of a drone's terrain-following flight device provided in an embodiment of the present invention; Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0020] Currently, existing UAV terrain-following flight technology suffers from frequent state oscillations. Based on this, the present invention provides a UAV terrain-following flight method and device, which can effectively avoid the problem of frequent state oscillations during UAV terrain-following flight, thereby significantly improving the safety and stability of terrain-following flight.

[0021] To facilitate understanding of this embodiment, a detailed description of a UAV terrain-following flight method disclosed in this embodiment of the invention will be provided first. (See [link to relevant documentation]). Figure 1 The diagram shows a flowchart of a method for unmanned aerial vehicle (UAV) to follow a terrain-following flight path. This method mainly includes the following steps S102 to S108: Step S102: Estimate the target flight state vector of the UAV at the current moment based on the current sensor data of the UAV and the target flight state vector at the previous moment.

[0022] Current sensor data, i.e., sensor data at the current moment, can include acceleration observation data (specifically z-axis acceleration observation data), vertical velocity observation data, and ground altitude observation data. For example, z-axis acceleration observation data is provided by an IMU (Inertial Measurement Unit), vertical velocity observation data is provided by GNSS (Global Navigation Satellite System), and ground altitude observation data is provided by SLAM (Simultaneous Localization and Mapping).

[0023] The flight state vector is a set of states characterizing the vertical motion characteristics of an unmanned aerial vehicle (UAV), including numerical values ​​corresponding to states such as acceleration bias error, vertical velocity, and ground altitude. The flight state vector is divided into an initial flight state vector and a target flight state vector. The initial flight state vector at the current moment is predicted based on acceleration observation data and the target flight state vector from the previous moment, specifically including the estimated values ​​of acceleration bias error, vertical velocity, and ground altitude. The target flight state vector at the current moment is obtained by updating the initial flight state vector based on vertical velocity observation data, or by using vertical velocity observation data and ground altitude observation data, specifically including the target values ​​of acceleration bias error, vertical velocity, and ground altitude.

[0024] In one implementation, an extended Kalman filter (EKF) framework based on three-state dual observation is adopted. The target flight state vector at the previous time step is used as a prior estimate, and the current sensor data is used as the observation input. Through a two-stage iterative operation of prediction and update, the target flight state vector at the current time step is output.

[0025] Step S104: Based on the target flight state vector and flight path altitude margin at the previous moment, as well as the effective terrain-following flight mode, switch or maintain the current state of the UAV to obtain the currently effective terrain-following flight mode.

[0026] The terrain-following flight modes include HIGH mode, NORMAL mode, and LOW mode. HIGH mode is a terrain-following flight strategy aimed at increasing the drone's altitude above the ground; NORMAL mode is a terrain-following flight strategy aimed at maintaining a constant altitude above the ground; and LOW mode is a terrain-following flight strategy aimed at decreasing the drone's altitude above the ground. An active terrain-following flight mode can also be referred to as the terrain-following flight mode active at the previous moment, and the currently active terrain-following flight mode can also be referred to as the terrain-following flight mode active at the current moment. The terrain-following flight modes active at the previous moment and the current moment may be the same or different.

[0027] The flight path altitude margin is the amount by which the drone is above the flight path.

[0028] State switching refers to the operation of switching from one active ground-following flight mode to another, while state holding refers to the operation of keeping the active ground-following flight mode unchanged.

[0029] In one implementation, a multi-mode memory state machine based on a hysteresis band design is used to determine whether the target altitude and flight path altitude margin in the target flight state vector at the previous moment meet the preset mode switching conditions for switching from an active terrain-following flight mode to another terrain-following flight mode. If the preset mode switching conditions are met, the mode switching operation corresponding to the preset mode switching conditions is executed; otherwise, a mode holding operation is executed. The multi-mode memory state machine features a hysteresis band design and memory capability. The hysteresis band design refers to setting asymmetric switching thresholds between the three states, forming a dual criterion of altitude recovery and flight path margin. The memory capability means that the mode selection of the state machine strictly depends on the target flight state vector at the previous moment. This design gives the mode switching a time delay characteristic, avoiding false triggering caused by current instantaneous measurement noise or sudden terrain changes.

[0030] The preset mode switching conditions refer to the logical criteria between different ground-following flight modes. These conditions are determined based on one or more of the following: a preset minimum safe ground altitude, a preset maximum safe ground altitude, a preset altitude hysteresis loop, and a preset heading margin hysteresis loop. The specific settings can be configured based on actual engineering requirements, and this embodiment of the invention does not impose any limitations on this. Specifically, the minimum safe ground altitude is the lowest permissible ground altitude set to avoid ground contact collisions; the maximum safe ground altitude is the highest permissible ground altitude set to meet airspace or route constraints; the altitude hysteresis loop, also known as the hysteresis band width, is a tolerance range set in the altitude direction, representing the anti-shake dead zone for vertical altitude judgment between HIGH and LOW modes; and the heading margin hysteresis loop, also known as the altitude margin threshold, is a tolerance range set on the route altitude margin, representing the track direction distance margin criterion set in HIGH mode to prevent overshooting the route.

[0031] Step S106: In the currently active terrain-following flight mode, generate terrain-following flight control commands for the current moment based on the target flight state vector at the current moment.

[0032] Terrain-following flight control commands refer to the command signals used to drive the flight control system of an unmanned aerial vehicle (UAV) to perform speed adjustments, including horizontal speed commands and vertical speed commands.

[0033] In one implementation, in the currently active terrain-following flight mode, the target flight altitude is determined by combining the target flight state vector at the current moment. Based on this, the dynamic integral limit value of the integral-proportional (PI) controller is determined to determine the vertical speed adjustment amount at the current moment. At the same time, the horizontal speed scaling factor corresponding to the currently active terrain-following flight mode is determined. Finally, the speed command is corrected using the vertical speed adjustment amount and the horizontal speed scaling factor to obtain the terrain-following flight control command at the current moment.

[0034] Step S108: Execute ground-following flight maneuvers in response to ground-following flight control commands.

[0035] In one example, based on the vertical and horizontal velocity components determined in the terrain-following flight control command, the corresponding velocity command is output to the flight control system, driving the UAV to perform the corresponding vertical and horizontal movements, thereby achieving terrain-following flight.

[0036] The UAV terrain-following flight method provided in this embodiment of the invention estimates the target flight state vector of the UAV at the current moment, and then switches or maintains the UAV's active terrain-following flight mode based on the template flight state vector at the previous moment through a mode switching mechanism based on the synergistic effect of memory and hysteresis design, thereby obtaining the currently active terrain-following flight mode. Based on this, the terrain-following flight control command is determined in combination with the target flight state vector at the current moment, effectively suppressing state jitter, thereby significantly improving the safety and stability of terrain-following flight.

[0037] In one embodiment, the UAV terrain-following flight method provided by this invention is divided into a sensor layer, a navigation estimation layer, and a guidance and control layer. The sensor layer consists of an IMU, GNSS, and SLAM, used to acquire acceleration observation data (specifically z-axis acceleration observation data), vertical velocity observation data, and ground altitude observation data, respectively. The navigation estimation layer employs a three-state dual-measurement extended Kalman filter to estimate the z-axis acceleration zero-bias error of the IMU online and fuses the vertical velocity observation data provided by GNSS and the ground altitude observation data provided by SLAM to suppress observation interference caused by obstacles. The guidance and control layer employs a memory-based state machine based on HIGH, NORMAL, and LOW modes, combined with a dynamic integral limiting mechanism and a low-altitude adaptive deceleration strategy to achieve stable terrain-following flight in altitude-holding mode. Finally, the speed command required for terrain-following flight is output, which is the aforementioned terrain-following flight control command.

[0038] To facilitate understanding, the state-space model of the three-state dual-measurement extended Kalman filter will first be explained.

[0039] (1) Flight state vector: In this embodiment of the invention, the flight state vector is defined as a 3-dimensional state vector, and the specific expression is as follows: ; in, This is the initial flight state vector; This is the estimated value of the zero-bias error of the z-axis acceleration of the IMU; a positive value indicates that the measured value is too large. This is an estimate of the vertical velocity, with upward as the positive direction; This is the estimated altitude above the ground; a positive value indicates that the drone is above the ground.

[0040] Compared to traditional two-state filters , The observation source is susceptible to environmental interference such as terrain obstruction, multipath effects, and obstacle reflections, resulting in insufficient robustness of state estimation and decreased accuracy of ground altitude fusion. This invention introduces a z-axis acceleration zero-bias error estimate as a third state into the flight state vector, forming a three-state filter. This zero-bias is estimated and compensated online through a dual-measurement mechanism, effectively suppressing long-term integral drift.

[0041] (2) Discrete-time equations of motion: ; Its unfolded form is as follows: ; ; ; in, The target flight state vector; This is the state transition matrix; To control the input matrix; = , which is the z-axis acceleration observation data measured by the IMU; = , which is process noise, satisfies , The process noise corresponding to the estimated value of the z-axis acceleration zero bias error. The process noise corresponding to the vertical velocity estimate. This represents the process noise corresponding to the estimated ground altitude. For process noise covariance; The target value for zero bias error of z-axis acceleration; The target value for vertical velocity; This is the target altitude value above the ground. For time difference.

[0042] State transition matrix (abbreviated as) The expression for ) is as follows: ; The expression for the control input matrix is ​​as follows: ; The expression for the process noise covariance is as follows: ;in, The standard deviation of the acceleration deviation process noise. The standard deviation of the speed process noise. The standard deviation of high process noise.

[0043] (3) Dual measurement observation model: In this embodiment of the invention, a 2D observation matrix (denoted as the dual observation vector) is defined, and its specific expression is as follows: ; in, For a two-observation vector, For the current moment The vertical velocity observation data provided by GNSS For the current moment The invention utilizes ground altitude observation data provided by SLAM. In this embodiment, a dual-observation fusion architecture is employed: GNSS-provided vertical velocity observation data serves as a long-term stability reference, while SLAN-provided ground altitude observation data serves as a real-time terrain following benchmark. The two complement each other in both time scale and physical dimension, enhancing the overall accuracy and dynamic response capability of state estimation.

[0044] The expression for the observation equation is as follows: ; in, For the observation matrix, To observe the noise, , To observe the noise covariance.

[0045] The expression for the observation matrix is ​​as follows: ; The expression for the observation noise covariance is as follows: ;in, The standard deviation of GNSS velocity observation noise. This represents the standard deviation of SLAM height observation noise.

[0046] Based on the foregoing definition, this invention provides a specific implementation method for the UAV terrain-following flight method, as shown below: Step 1: Obtain the target state vector of the UAV at the previous moment. and target covariance matrix .

[0047] Step 2: Acquire the current sensor data of the UAV, including z-axis acceleration observation data. Vertical velocity observation data Earth altitude observation data .

[0048] Step 3: Based on the target flight state vector at the previous moment, predict the initial flight state vector and the initial covariance matrix at the current moment.

[0049] Specifically, the expression for state prediction is as follows: ; Expanded to: ; ; ; in, , , Indicates the current time The zero-bias error estimates of acceleration, vertical velocity, and ground altitude are calculated. , , Indicates the previous moment The target values ​​for zero-biased acceleration, vertical velocity, and ground altitude are set below.

[0050] Specifically, the expression for covariance matrix prediction is as follows: ; in, , Each represents the current time. The initial covariance matrix at the previous time step The target covariance matrix is ​​denoted as the covariance matrix. The covariance matrix for The expression for a symmetric positive definite matrix is ​​as follows: ; In practical applications, symmetry is used to store only the 6 elements of the upper triangle, that is... .

[0051] Step 4: Determine if the ground altitude observation data is invalid.

[0052] In one instance, if the duration of ground altitude observations via SLAM (referred to as the altitude observation market) exceeds a duration threshold, the ground altitude observation data is determined to be invalid. Then it is determined to be invalid, where, For the duration of high-altitude observation, This is a duration threshold (typically 500ms). In another example, if the ground altitude observation data exceeds the effective altitude limit, the ground altitude observation data is determined to be invalid.

[0053] Step 5, assuming the ground altitude observation data is not invalid: Based on the vertical velocity observation data, determine whether to downgrade from dual-observation mode to single-observation mode to determine the target observation mode and its corresponding target observation vector. Then, based on the target observation vector and the target flight state vector at the previous moment, update the initial flight state to obtain the target flight state vector at the current moment. Dual-observation mode is a mode that observes based on both ground altitude and vertical velocity observation data, while single-observation mode is a mode that observes based solely on ground altitude observation data.

[0054] The process for determining whether to downgrade from dual-observation mode to single-observation mode is as follows: If the vertical velocity observation data is not invalid, then dual-observation mode is executed; if the vertical velocity observation data is invalid, then the process is downgraded from dual-observation mode to single-observation mode.

[0055] The process of updating the initial flight state vector to obtain the target flight state vector is as follows: Determine the residual matrix between the target observation vector and the initial flight state vector; update the initial flight state vector based on the target observation noise variance and residual matrix at the current moment to obtain the target flight state vector at the current moment; where, in dual-observation mode, the target observation vector is also the dual-observation vector, which is based on vertical velocity observation data. Earth altitude observation data The constructed residual matrix includes vertical velocity residuals and ground altitude residuals; in single-observation mode, the target observation vector is also the single-observation vector, which is based on ground altitude observation data. The constructed residual matrix includes the ground altitude residual; the target observation noise variance is determined based on the target observation noise variance at the previous time step, the ground altitude residual at the current time step, and their corresponding statistics.

[0056] This invention provides specific implementation methods for Kalman filter updates in dual-observation mode and single-observation mode, respectively: Scenario 1: If the GNSS vertical velocity observation data is valid, execute the dual-observation mode. The specific process of the dual-observation mode is as follows: (1.1) Based on the initial flight state vector and the two observation vectors at the current moment, determine the two observation residuals at the current moment, including the vertical velocity residuals. and ground height residual The expression for the two-observation vector is as follows: ; Expanded to: ; in, For a two-observation vector, For a two-observation vector, For the observation matrix, For the current moment The initial flight state vector is given below. For vertical velocity observation data, For the current moment The estimated vertical velocity in the initial flight state vector. For Earth altitude observation data, For the current moment The ground altitude estimate in the initial flight state vector. For vertical velocity residuals, This represents the residual height relative to the ground.

[0057] (1.2) Determine the innovation covariance at the current time based on the initial covariance matrix at the current time and the adaptively adjusted target observation noise variance at the current time; wherein, the target observation noise variance is determined based on the target observation noise variance at the previous time, the ground height residual at the current time and its corresponding statistics.

[0058] Among them, innovation covariance The expression is as follows: ; Expand the innovation covariance as matrix: ; in, The target observation noise variance is adaptively adjusted.

[0059] (1.3) Determine the current time based on the initial covariance matrix and innovation covariance at the current time. Kalman gain matrix : ; Kalman gain matrix for The matrix, whose expansion form is shown below: ; For the current moment The initial covariance matrix is ​​given below; That is, the inverse of the innovation covariance, its expression is as follows: , .

[0060] (1.4) Determine the target flight state vector at the current time based on the double observation residual, the initial flight state vector at the current time and the Kalman gain matrix, and determine the target covariance matrix at the current time based on the initial covariance matrix at the current time and the Kalman gain matrix.

[0061] The process of determining the target flight state vector is as follows: ; in, For the current moment The target flight state vector is below. For the current moment The initial flight state vector is given below. For the current moment The Kalman gain matrix under the given conditions For the current moment The double-observation residuals.

[0062] To ensure numerical stability, this embodiment of the invention employs Josephian covariance updates, thereby ensuring that the variance matrix remains symmetric and positive definite. The process for determining the target covariance matrix is ​​as follows: ; in, For the current moment The target covariance matrix is ​​as follows. For the current moment The initial covariance matrix is ​​given below. for identity matrix For the current moment The Kalman gain matrix under the given conditions For the observation matrix, For the current moment The observed noise covariance.

[0063] Scenario 2: When GNSS vertical velocity observation data is invalid, the mode is downgraded from dual-observation mode to single-observation mode. The specific process for single-observation mode is as follows: (2.1) Based on the initial flight state vector and single observation vector at the current moment, determine the single observation residual at the current moment, including the ground altitude residual.

[0064] In one instance, when GNSS vertical velocity observation data is unavailable, the target observation vector degenerates into a single observation vector. The expression is as follows: ; in, For the current moment The following is ground-level height observation data.

[0065] In one instance, when GNSS vertical velocity observation data is unavailable, the observation matrix degenerates to: .

[0066] Based on this, the expression for the single-observation residual is as follows: ; in, For single-observation residuals, This represents the ground altitude estimate in the initial covariance matrix at the current moment.

[0067] (2.2) Determine the innovation covariance at the current time based on the initial covariance matrix at the current time and the adaptively adjusted target observation noise variance at the current time; wherein, the target observation noise variance is determined based on the target observation noise variance at the previous time, the ground height residual at the current time and its corresponding statistics.

[0068] Innovation Covariance The expression is as follows: ; in, For the current moment The value in the 3rd row and 3rd column of the initial covariance matrix. The target observation noise variance is adaptively adjusted.

[0069] (2.3) Determine the current time based on the initial covariance matrix and innovation covariance at the current time. Kalman gain matrix under .

[0070] The expression for the Kalman gain matrix is ​​as follows: ; in, , , Each represents the current time. The values ​​in the first row and third column, the second row and third column, and the third row and third column of the initial covariance matrix.

[0071] (2.4) Determine the target flight state vector at the current time based on the single observation residual, the initial flight state vector at the current time, and the Kalman gain matrix, and determine the target covariance matrix at the current time based on the initial covariance matrix and the Kalman gain matrix at the current time.

[0072] The process of determining the target flight state vector is as follows: ; in, For the current moment The target flight state vector is below. For the current moment The initial flight state vector is given below. For the current moment The Kalman gain matrix under the given conditions For the current moment The double-observation residuals.

[0073] To ensure numerical stability, this embodiment of the invention uses Joseph form covariance update to ensure that the variance matrix remains symmetric and positive definite. For details, please refer to (1.4) above. This embodiment of the invention will not repeat the details.

[0074] In this embodiment of the invention, through the dual / single observation automatic switching mechanism, when the GNSS vertical velocity observation fails, the system seamlessly degrades to a single observation mode that relies solely on SLAM for ground altitude, ensuring that the filtered estimation remains effective and improving overall robustness.

[0075] Regarding the adaptive target observation noise variance in (1.2) and (2.2) above... To prevent height observation anomalies caused by obstacles, the normalized innovation square (NIS) check is used to verify the target observation noise variance in both dual-observation and single-observation modes. Adaptive adjustment is performed, and the adaptive adjustment process is as follows: (3.1) Determine the NIS statistic for height observation based on the innovative covariance and corresponding observation residuals in the dual-observation mode or single-observation mode.

[0076] In one example, under dual-observation mode: Innovation Covariance The expression is: ; NIS statistic The expression is: , This refers to the Earth observation residuals under dual observation mode; In another example, under single-observation mode: Innovation Covariance The expression is: ; NIS statistic The expression is: , This represents the Earth observation residual under dual observation mode.

[0077] (3.2) Based on the sign of Earth observation residuals and NIS statistics The initial observation noise variance is determined by its relationship with a preset statistical threshold. For example, this is used when the Earth observation residuals are negative and the NIS statistic... If the variance exceeds a first preset threshold, the basic observation noise variance is adjusted using a preset first adjustment coefficient to obtain the initial observation noise variance; when the Earth observation residual is negative and the NIS statistic is... If the variance exceeds the second preset threshold, the basic observation noise variance is adjusted using a preset second adjustment coefficient to obtain the initial observation noise variance; otherwise, the basic observation noise variance is used as the initial observation noise variance. The first preset threshold is less than the second preset threshold, and the first adjustment coefficient is less than the second adjustment coefficient.

[0078] Specifically: ; in, The initial observation noise variance, Based on the observation noise variance, , These are the first adjustment factor and the second adjustment factor, respectively. , These are the first preset threshold and the second preset threshold, respectively.

[0079] In this embodiment of the invention, negative innovation ( The fact that the observed ground altitude data is lower than the estimated ground altitude may be due to obstacles; therefore, a stricter threshold is used. Increased variance of basic observation noise Times; positive innovation ( (i.e., increased ground altitude): Ground altitude observation data exceeding the estimated ground altitude may be due to terrain subsidence. A threshold is used instead. More stringent thresholds Increased variance of basic observation noise Times. In practical implementation, < and, < To ensure that altitude detection slightly increases the noise variance when negative innovations occur, it avoids excessive drone response due to false detections in SLAM. Similarly, it increases the base observation noise variance by a larger factor when positive innovations occur to avoid false detections. The smaller adjustment factor for negative innovations is used because it's necessary to isolate false detections as much as possible while ensuring drone safety, while the adjustment factor for positive innovations does not affect drone safety. Therefore, this embodiment of the invention uses direction-sensitive NIS statistics detection, employing different thresholds for negative innovations (potentially caused by obstacles) and positive innovations (potentially caused by terrain descent) to balance safety and sensitivity.

[0080] (3.3) The target observation noise variance is obtained by performing a first-order low-pass filter on the initial observation noise variance to avoid drastic changes in the observation noise. The specific expression is as follows: ; in, For the current moment The target observation noise variance used below For the previous moment The target observation noise variance used below For smoothing coefficients, For the current moment The initial observation noise variance used below.

[0081] Step 6, in the case of the failure of ground altitude observation data: the ground altitude estimate is incrementally updated with the maximum ground altitude as a constraint. Based on the zero-bias error estimate of acceleration, the estimate of vertical velocity and the incrementally updated ground altitude estimate, the target flight state vector at the current moment is determined, and the initial covariance matrix is ​​used as the target covariance matrix at the current moment.

[0082] The expression for incrementally updating the ground elevation estimate is shown below: ; in, This is an incrementally updated estimate of the ground altitude. This is an estimate of the ground altitude. This is the rate of ascent coefficient, with a value of 0.02.

[0083] In practical applications, because the accuracy of SLAM observations decreases as the ground altitude increases, this embodiment of the invention sets an effective altitude limit for SLAM altitude observations. When the ground altitude observation data exceeds the effective altitude limit, the system completely rejects the SLAM altitude observation data. The design principle is as follows: within the SLAM failure range, the navigation estimation layer continuously outputs a slowly increasing trending altitude estimate, prompting the guidance and control layer to generate a gentle descent command. This drives the UAV to gradually reduce its flight altitude within the safe altitude margin of the planned route, thereby actively returning to the effective SLAM observation range and restoring high-precision ground altitude feedback.

[0084] Step 7: Using a three-mode memory state machine, determine the currently active terrain-following flight mode based on the ground altitude estimate in the target flight state vector at the previous moment, the flight path altitude margin at the previous moment (i.e., the amount by which the UAV is above the flight path), and the preset minimum safe ground altitude, maximum safe ground altitude, altitude hysteresis loop, and heading margin hysteresis loop.

[0085] In one implementation, the ground altitude target value in the target flight state vector at the previous moment is first extracted; then, a three-mode memory state machine is used to determine whether the ground altitude target value and the flight path altitude margin at the previous moment meet the preset mode switching conditions for switching from the already effective terrain-following flight mode to other terrain-following flight modes; if so, the already effective terrain-following flight mode is switched to other terrain-following flight modes; if not, the currently effective terrain-following flight mode is maintained to obtain the currently effective terrain-following flight mode.

[0086] Among them, the flight path altitude margin That is, the difference between the drone's current altitude and its flight path altitude, denoted as... , This is the current altitude of the drone. For flight path altitude. The terrain-following flight mode includes ascent mode, altitude hold mode, and descent mode. Specifically, a three-mode memory state machine is defined: Compared to traditional single-threshold judgment, the embodiments of the present invention effectively avoid frequent oscillations through the memory and hysteresis loop design of the three-mode memory state machine.

[0087] The preset mode switching conditions are determined based on one or more of the following: minimum safe ground altitude, maximum safe ground altitude, altitude hysteresis, and heading margin hysteresis. The expression for the preset mode switching conditions is as follows: ; For the current moment The ground-following flight mode takes effect below. For the previous moment The ground-following flight mode takes effect below. For the previous moment The target height above the ground is below. For the previous moment The altitude margin of the flight path.

[0088] For details, see Figure 2 The diagram illustrates a mode switching mechanism. Embodiments of the present invention provide the following preset mode switching conditions: The preset mode switching conditions for switching from altitude hold mode to descent mode include: the target ground altitude value at the previous moment is less than the minimum safe ground altitude, that is: Then maintain from height ( Switch to descent mode. )model.

[0089] The preset mode switching conditions for switching from altitude hold mode to ascent mode include: the target ground altitude value at the previous moment is greater than the maximum safe ground altitude, and the flight path altitude margin at the previous moment is greater than the heading margin hysteresis, that is: Then maintain from height ( Switch from ) mode to ascending ( )model.

[0090] The preset mode switching conditions for switching from descent mode to altitude hold mode include: the target ground altitude value at the previous moment is greater than the first altitude threshold, and the flight path altitude margin at the previous moment is positive. The first altitude threshold is determined based on the minimum safe ground altitude and altitude hysteresis. For example, the sum of the minimum safe ground altitude and altitude hysteresis can be used as the first altitude threshold, that is: Then from the decrease ( Switch mode to height hold ( )model.

[0091] The preset mode switching conditions for switching from ascent mode to altitude hold mode include: the target ground altitude value at the previous moment is less than a second altitude threshold. The second altitude threshold is determined based on the maximum ground altitude and altitude hysteresis, for example, the difference between the maximum ground altitude and the altitude hysteresis is used as the second altitude threshold, that is: Then from the rise ( Switch mode to height hold ( )model.

[0092] in, =30m is the minimum safe height above the ground =33m is the maximum safe height above the ground. It is a high hysteresis loop. =0.3m is the heading margin hysteresis. In this embodiment of the invention, control stability can be guaranteed by a double-layer hysteresis design, namely, altitude hysteresis and heading margin replacement.

[0093] Step 8: Extract the target altitude value above the ground from the target flight state vector at the current moment; under the currently active terrain-following flight mode, determine the UAV's flight altitude at the current moment based on the target altitude value above the ground at the current moment, the flight path altitude margin at the current moment, the minimum safe altitude above the ground, the maximum safe altitude above the ground, and the altitude hysteresis.

[0094] In one implementation, while maintaining a height ( In this mode, the target altitude value above the ground at the current moment will be... As a target for flight altitude; during descent ( () mode, minimum safe ground altitude and high hysteresis The sum of the values ​​is used as the target for flight altitude; during ascent ( In each mode, the maximum safe altitude to the ground is determined. With high hysteresis The difference, the target altitude value above the ground at the current moment Altitude margin at the current moment The difference between the two values ​​is used, and the maximum of the two differences is taken as the target flight altitude. The specific expression is as follows: ; Among them, height maintenance ( The ) mode locks the current altitude above the ground to suppress unnecessary adjustments and avoid control oscillations; descent ( The ascent mode sets the target flight altitude as the sum of the minimum safe ground altitude and the altitude hysteresis loop to reserve a safety margin; The ascent mode dynamically associates the target flight altitude with the flight path altitude margin to prevent excessively rapid descent from breaking the flight path. In this embodiment of the invention, the ascent ( The mode uses the flight path altitude margin to dynamically correct the flight altitude target. Compared with a fixed target altitude, the embodiments of the present invention can effectively prevent the flight path from being breached.

[0095] Step 9: Determine the dynamic integral limit value based on the altitude error between the current ground altitude target value and the flight altitude target value. Then, through the proportional-integral controller, determine the terrain-following flight control command for the current moment based on the dynamic integral limit value. Specifically, this includes: (4.1) Determine the altitude error between the target flight altitude at the current moment and the target altitude value above the ground in the target flight state vector at the current moment, that is: ,in, For height error, This represents the target altitude value in the target flight state vector at the current moment. The target flight altitude at the current moment.

[0096] (4.2) The vertical velocity adjustment at the current moment is determined using a proportional-integral controller based on the altitude error and the dynamic integral limit. Specifically, this includes: I. Determine the integration upper limit (i.e., the dynamic integration limit value) based on the current flight path altitude margin and a preset proportional control gain parameter. In this embodiment of the invention, the integration upper limit is dynamically adjusted according to the current flight path altitude margin, as shown in the following expression: ;in, For the current moment The maximum points limit, For the current moment The altitude margin of the flight path, This refers to the proportional gain of the proportional-integral controller.

[0097] Second, based on the current altitude error, the current integration upper limit, and the integration gain of the proportional-integral controller, determine the integration accumulation at the next moment. In this embodiment of the invention, the integration accumulation at the next moment is determined according to the following formula: ;in, , For the next moment Current moment The corresponding points accumulation, For the current moment The height error below, The integral gain of the proportional-integral controller, This is the maximum value of the ascent / descent speed control value. For the current moment Upper limit of integration, saturation function .

[0098] Referring to the above expression for determining the integral accumulation at the next time step, the integral accumulation at the current time step can be determined based on the relevant data from the previous time step.

[0099] In this embodiment of the invention, the integration upper limit is dynamically associated with the flight path altitude margin. Compared with a fixed upper limit, it can automatically converge the descent command when the UAV approaches the flight path, effectively preventing it from breaking through the flight path.

[0100] III. Determine the vertical speed adjustment based on the current altitude error, proportional gain, and the current integral accumulation. Specifically: ;in, For the current moment The corresponding z-direction velocity adjustment (i.e., the vertical velocity adjustment). For proportional gain, For the current moment The corresponding height error, For the current moment The corresponding points accumulation.

[0101] In this embodiment of the invention, the integration limit is dynamically correlated with the flight path altitude margin to ensure that the descent speed is automatically suppressed when the aircraft approaches the flight path, thus preventing it from breaking through the flight path.

[0102] (4.3) Determine the target horizontal velocity scaling factor corresponding to the currently active ground-following flight mode based on the target altitude target value in the target flight state vector at the current moment and the target horizontal velocity scaling factor at the previous moment.

[0103] For horizontal speed, this embodiment of the invention proposes adaptive scaling of horizontal speed. First, it is determined whether the current ground-following flight mode is a descent (LOW) mode. If so, the initial horizontal speed scaling factor is determined based on the ratio between the target altitude target value in the target flight state vector at the current moment and the minimum safe altitude to the ground. If not, the initial horizontal speed scaling factor is set to a specified value (such as 1). Then, the initial horizontal speed scaling factor at the current moment is subjected to a first-order low-pass filter in combination with the target horizontal speed scaling factor at the previous moment to obtain the target horizontal speed scaling factor at the current moment.

[0104] The initial horizontal velocity scaling factor at the current moment is determined using the following formula: ; The initial horizontal velocity scaling factor at the current moment is subjected to a first-order low-pass filter according to the following formula: ; in, For the current moment The target horizontal velocity scaling factor is below. For the current moment The initial horizontal velocity scaling factor is below. For the previous moment The target horizontal velocity scaling factor is below. This is the filtering time constant (with a value of 0.5s).

[0105] (4.4) Use the vertical speed adjustment and horizontal speed scaling factor to correct the speed command in order to obtain the terrain-following flight control command at the current moment.

[0106] In one example, the horizontal x-axis and horizontal y-axis speed commands during normal drone flight can be multiplied by a horizontal speed scaling factor to obtain the corrected horizontal x-axis and horizontal y-axis speed commands; and the vertical z-axis speed command during normal drone flight can be added to a vertical speed adjustment amount to obtain the corrected vertical z-axis speed command. The specific expressions are shown below: ; ; ; in, The corrected speed command for the horizontal x-axis. The corrected speed command for the horizontal y-axis. This is the corrected velocity command for the vertical z-axis. This is the horizontal x-axis speed command during normal flight. This is the speed command for the horizontal y-axis during normal flight. This is the vertical z-axis velocity command during normal flight. For the current moment The target horizontal velocity scaling factor is below. For the current moment The corresponding velocity adjustment in the z-direction.

[0107] In this embodiment of the invention, a square function is used to scale the horizontal velocity at low altitudes, so that the forward velocity decreases more rapidly as the altitude decreases, significantly improving the vertical maneuver margin and flight safety; compared with linear scaling, it is more sensitive and suppresses more quickly.

[0108] Step 10: If the altitude holding mode is satisfied based on the currently active terrain-following flight mode, freeze the vertical speed command in the terrain-following flight control command.

[0109] In one implementation, when switching from LOW mode to NORMAL mode, and If the value is greater than or equal to 0, the altitude hold mode is determined to be satisfied; or, when switching from HIGH mode to NORMAL mode, and Greater than or equal to heading margin hysteresis If the condition is met, the altitude preservation mode is determined to be satisfied; otherwise, the altitude preservation mode is determined not to be satisfied. The specific expression is as follows: ; in, This is the altitude preservation mode flag; a value of 1 indicates that the altitude preservation mode is met, and a value of 0 indicates that the altitude preservation mode is not met.

[0110] exist When =1, ,in, This represents the z-position in the EF coordinate system recorded at the moment of mode switching. This is the position control command for the drone's altitude controller, meaning to maintain the current altitude. Optionally, the vertical speed command in the terrain-following flight control commands can also be frozen to keep it constant at the current altitude.

[0111] In this embodiment of the invention, after altitude protection is deactivated, the current altitude is immediately locked and the waypoint altitude command is overridden to avoid repeated triggering caused by sudden changes in flight path altitude, thereby effectively suppressing control oscillations and improving flight stability.

[0112] Step 11: Proceed to the next iteration k+1 and return to execute step 1.

[0113] In summary, the embodiments of the present invention can achieve high-precision ground estimation over complex terrain, and ensure the safety and stability of the UAV's terrain-following flight through an adaptive altitude protection state machine and speed coordination control. The embodiments of the present invention have at least the following characteristics: (1) Highly robust ground altitude estimation: The dual observation architecture that integrates GNSS vertical velocity and SLAM ground altitude overcomes the shortcomings of single sensors being susceptible to environmental interference and significantly improves the anti-interference capability of altitude estimation. In principle, GNSS velocity provides a long-term stable but ground-based vertical motion reference, while SLAM altitude provides real-time ground information but is susceptible to obstacle disturbances; the two complement each other and are integrated to balance stability and scene adaptability.

[0114] (2) Intelligent adaptive altitude protection: It adopts a three-mode memory-type protection state machine (NORMAL / LOW / HIGH) and a dual hysteresis loop (altitude hysteresis loop + route margin hysteresis loop) design to avoid the frequent start-stop and control oscillations caused by the lack of state memory in traditional single threshold criteria. Exiting protection requires the simultaneous fulfillment of both altitude recovery and route margin requirements, which greatly reduces the frequency of state switching.

[0115] Compared to the problem of a fixed points cap potentially leading to route overshooting, this embodiment of the invention dynamically associates the points cap with the route altitude margin. The closer the drone gets to the flight path, the smaller the upper limit becomes, thereby automatically suppressing the amplitude of the descent command, effectively preventing it from breaking the flight path, and increasing the safety margin by 50%.

[0116] (3) Cooperative speed control: Low-altitude adaptive deceleration: Compared to the problem of insufficient vertical maneuverability caused by fixed-speed flight, the embodiment of this invention improves the vertical maneuverability margin by 40%. Principle analysis: Traditional methods maintain high-speed flight at low altitudes, limiting vertical maneuverability when a sudden climb is needed; the embodiment of this invention uses a square function to scale the horizontal velocity, with the velocity decreasing faster at lower altitudes, reserving more thrust for vertical maneuvers.

[0117] Coordination of Horizontal and Vertical Speeds: Compared to the problem of abrupt trajectory changes that may result from independently controlling horizontal and vertical speeds, the flight stability of this invention is significantly improved. Principle Analysis: Low-pass filtering smooths the speed scaling factor, avoiding abrupt speed command changes; altitude hold mode locks the current altitude over waypoint commands, preventing sudden changes in flight path altitude from triggering protection again, thus achieving a smooth transition.

[0118] Based on the foregoing embodiments, this invention provides a ground-following flight device for unmanned aerial vehicles (UAVs). See [link to previous embodiment]. Figure 3 The diagram shows a structural schematic of a drone's terrain-following flight device, which mainly includes the following parts: The navigation estimation module 302 is used to estimate the target flight state vector of the UAV at the current moment based on the current sensor data of the UAV and the target flight state vector at the previous moment. The mode switching or holding module 304 is used to switch or hold the current state of the UAV based on the target flight state vector and flight path altitude margin at the previous moment, as well as the effective terrain-following flight mode, to obtain the currently effective terrain-following flight mode. The guidance and control module 306 is used to generate a ground-following flight control command at the current moment based on the target flight state vector at the current moment in the currently effective ground-following flight mode. Command response module 308 is used to respond to terrain-following flight control commands and execute terrain-following flight maneuvers.

[0119] The UAV terrain-following flight device provided in this embodiment of the invention estimates the target flight state vector of the UAV at the current moment, and then switches or maintains the active terrain-following flight mode of the UAV based on the template flight state vector at the previous moment through a mode switching mechanism based on the synergistic effect of memory and hysteresis design, thereby obtaining the currently active terrain-following flight mode. Based on this, the terrain-following flight control command is determined in combination with the target flight state vector at the current moment, effectively suppressing state jitter, thereby significantly improving the safety and stability of terrain-following flight.

[0120] In one implementation, the current sensor data includes at least vertical velocity observation data and ground altitude observation data; the navigation estimation module 302 is specifically used for: Based on the target flight state vector at the previous moment, predict the initial flight state vector at the current moment; wherein, the initial flight state vector includes the acceleration zero bias error estimate, the vertical velocity estimate, and the ground altitude estimate; If the ground altitude observation data is not invalid, determine whether to downgrade from dual observation mode to single observation mode based on the vertical velocity observation data, so as to determine the target observation mode and its corresponding target observation vector; wherein, dual observation mode is a mode that observes based on ground altitude observation data and vertical velocity observation data, and single observation mode is a mode that observes based only on ground altitude observation data. Based on the target observation vector and the target flight state vector at the previous moment, the initial flight state is updated to obtain the target flight state vector at the current moment.

[0121] In one implementation, the navigation estimation module 302 is specifically used for: Determine the residual matrix between the target observation vector and the initial flight state vector; Based on the target observation noise variance and residual matrix at the current moment, the initial flight state vector is updated to obtain the target flight state vector at the current moment; In the dual-observation mode, the residual matrix includes the vertical velocity residual and the ground altitude residual; in the single-observation mode, the residual matrix includes the ground altitude residual; the target observation noise variance is determined based on the target observation noise variance at the previous moment, the ground altitude residual at the current moment, and their corresponding statistics.

[0122] In one implementation, the navigation estimation module 302 is specifically used for: The estimated ground altitude is updated incrementally with the maximum ground altitude as a constraint. Based on the zero-biased acceleration error estimate, the vertical velocity estimate, and the incrementally updated ground altitude estimate, the target flight state vector at the current moment is determined.

[0123] In one implementation, the mode switching or holding module 304 is specifically used for: Extract the target altitude value from the target flight state vector at the previous moment; Determine whether the target altitude value above the ground and the altitude margin of the flight path at the previous moment meet the preset mode switching conditions for switching from the active terrain-following flight mode to other terrain-following flight modes. If yes, switch from the currently active terrain-following flight mode to another terrain-following flight mode; otherwise, retain the currently active terrain-following flight mode to obtain the currently active terrain-following flight mode. The terrain-following flight mode includes ascent mode, altitude hold mode, and descent mode. The preset mode switching conditions are determined based on one or more of the following: minimum safe ground altitude, maximum safe ground altitude, altitude hysteresis, and heading margin hysteresis. Altitude hysteresis is a tolerance range set in the altitude direction, and heading margin hysteresis is a tolerance range set in the heading altitude margin.

[0124] In one implementation, the preset mode switching condition for switching from altitude holding mode to descent mode includes: the target ground altitude value at the previous moment is less than the minimum safe ground altitude; The preset mode switching conditions for switching from altitude hold mode to ascent mode include: the target ground altitude value at the previous moment is greater than the maximum safe ground altitude, and the route altitude margin at the previous moment is greater than the heading margin hysteresis. The preset mode switching conditions for switching from descent mode to altitude hold mode include: the target ground altitude value at the previous moment is greater than the first altitude threshold, and the flight path altitude margin at the previous moment is positive. The first altitude threshold is determined based on the minimum safe ground altitude and altitude hysteresis. The preset mode switching conditions for switching from ascent mode to altitude hold mode include: the target ground altitude value at the previous moment is less than the second altitude threshold, which is determined based on the maximum ground altitude and altitude hysteresis.

[0125] In one embodiment, the guidance control module 306 is specifically used for: Extract the target altitude value from the target flight state vector at the current moment; In the currently active ground-following flight mode, the flight altitude target is determined based on the ground altitude target value at the current moment; The dynamic integral limit value is determined based on the altitude error between the ground target altitude value and the flight target altitude value at the current moment; Based on the dynamic integral limit value, the terrain-following flight control command at the current moment is determined.

[0126] In one embodiment, the guidance control module 306 is specifically used for: The vertical velocity adjustment at the current moment is determined based on the altitude error and the dynamic integral limit value. Determine the horizontal speed scaling factor corresponding to the currently active terrain-following flight mode; By using the vertical speed adjustment and the horizontal speed scaling factor, the speed command is corrected to obtain the terrain-following flight control command at the current moment.

[0127] In one embodiment, a height-maintaining module is also included for: If the altitude holding mode is satisfied based on the currently active terrain-following flight mode, freeze the vertical speed command in the terrain-following flight control commands.

[0128] The device provided in this embodiment of the invention has the same implementation principle and technical effect as the aforementioned method embodiment. For the sake of brevity, any parts not mentioned in the device embodiment can be referred to the corresponding content in the aforementioned method embodiment.

[0129] This invention provides an electronic device, specifically, the electronic device includes a processor and a memory; the memory stores a computer program, which, when run by the processor, executes the method described in any of the above embodiments.

[0130] Figure 4 The present invention provides a schematic diagram of the structure of an electronic device 100, which includes a processor 40, a memory 41, a bus 42 and a communication interface 43. The processor 40, the communication interface 43 and the memory 41 are connected through the bus 42. The processor 40 is used to execute executable modules, such as computer programs, stored in the memory 41.

[0131] The memory 41 may include high-speed random access memory (RAM) or non-volatile memory, such as at least one disk storage device. Communication between this system network element and at least one other network element is achieved through at least one communication interface 43 (which can be wired or wireless), such as the Internet, wide area network, local area network, metropolitan area network, etc.

[0132] Bus 42 can be an ISA bus, PCI bus, or EISA bus, etc. The bus can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 4 The symbol is represented by a single double-headed arrow, but this does not mean that there is only one bus or one type of bus.

[0133] The memory 41 is used to store programs. After receiving an execution instruction, the processor 40 executes the program. The method executed by the device for defining the flow process disclosed in any of the foregoing embodiments of the present invention can be applied to the processor 40 or implemented by the processor 40.

[0134] Processor 40 may be an integrated circuit chip with signal processing capabilities. In implementation, each step of the above method can be completed by the integrated logic circuitry in the hardware of processor 40 or by instructions in software form. Processor 40 can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc.; it can also be a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this invention. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the methods disclosed in the embodiments of this invention can be directly embodied in the execution of a hardware decoding processor, or executed by a combination of hardware and software modules in the decoding processor. The software modules can reside in random access memory, flash memory, read-only memory, programmable read-only memory, electrically erasable programmable memory, registers, or other mature storage media in the art. The storage medium is located in memory 41. The processor 40 reads the information in memory 41 and, in conjunction with its hardware, completes the steps of the above method.

[0135] The computer program product of the readable storage medium provided in the embodiments of the present invention includes a computer-readable storage medium storing program code. The instructions included in the program code can be used to execute the methods described in the foregoing method embodiments. For specific implementation, please refer to the foregoing method embodiments, which will not be repeated here.

[0136] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0137] Finally, it should be noted that the above-described embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit it. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments within the technical scope disclosed in the present invention, or make equivalent substitutions for some of the technical features; and these modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for unmanned aerial vehicle (UAV) ground-following flight, characterized in that, include: Based on the current sensor data of the UAV and the target flight state vector at the previous moment, the target flight state vector of the UAV at the current moment is estimated; Based on the target flight state vector and flight path altitude margin at the previous moment, as well as the active terrain-following flight mode, the current state of the UAV is switched or maintained to obtain the currently active terrain-following flight mode. In the currently active terrain-following flight mode, generate terrain-following flight control commands at the current moment based on the target flight state vector at the current moment; The system executes terrain-following flight maneuvers in response to the terrain-following flight control commands.

2. The UAV terrain-following flight method according to claim 1, characterized in that, The current sensor data includes at least vertical velocity observation data and ground altitude observation data; navigation estimation is performed based on the current sensor data of the UAV and the target flight state vector at the previous moment to obtain the target flight state vector at the current moment, including: Based on the target flight state vector at the previous moment, predict the initial flight state vector at the current moment; wherein, the initial flight state vector includes an estimated value of zero-bias acceleration error, an estimated value of vertical velocity, and an estimated value of altitude above the ground; If the ground altitude observation data is not invalid, determine whether to downgrade from dual observation mode to single observation mode based on the vertical velocity observation data, so as to determine the target observation mode and its corresponding target observation vector; wherein, the dual observation mode is a mode of observation based on the ground altitude observation data and the vertical velocity observation data, and the single observation mode is a mode of observation based only on the ground altitude observation data; Based on the target observation vector and the target flight state vector at the previous moment, the initial flight state is updated to obtain the target flight state vector at the current moment.

3. The UAV terrain-following flight method according to claim 2, characterized in that, Based on the target observation vector and the target flight state vector at the previous moment, the initial flight state is updated to obtain the target flight state vector at the current moment, including: Determine the residual matrix between the target observation vector and the initial flight state vector; The initial flight state vector is updated based on the target observation noise variance and the residual matrix at the current moment to obtain the target flight state vector at the current moment. In the dual-observation mode, the residual matrix includes vertical velocity residual and ground altitude residual; in the single-observation mode, the residual matrix includes ground altitude residual; the target observation noise variance is determined based on the target observation noise variance at the previous moment, the ground altitude residual at the current moment, and their corresponding statistics.

4. The UAV terrain-following flight method according to claim 2, characterized in that, In the event that the Earth altitude observation data is unavailable, the method further includes: The estimated ground altitude is incrementally updated with the maximum ground altitude as a constraint. Based on the zero-bias error estimate of acceleration, the estimate of vertical velocity, and the incrementally updated estimate of ground altitude, the target flight state vector at the current moment is determined.

5. The method for UAV ground-following flight according to claim 1, characterized in that, Based on the target flight state vector and flight path altitude margin at the previous moment, and the active terrain-following flight mode, the current state of the UAV is switched or maintained to obtain the currently active terrain-following flight mode, including: Extract the ground altitude target value from the target flight state vector at the previous moment; Determine whether the target altitude value and flight path altitude margin at the previous moment meet the preset mode switching conditions for switching from the active ground-following flight mode to other ground-following flight modes; If yes, then switch from the active terrain-following flight mode to another terrain-following flight mode; otherwise, keep the currently active terrain-following flight mode to obtain the currently active terrain-following flight mode. The ground-following flight mode includes ascent mode, altitude hold mode, and descent mode. The preset mode switching condition is determined based on one or more of the following: minimum safe ground altitude, maximum safe ground altitude, altitude hysteresis, and heading margin hysteresis. The altitude hysteresis is a tolerance range set in the altitude direction, and the heading margin hysteresis is a tolerance range set in the heading altitude margin.

6. The UAV terrain-following flight method according to claim 5, characterized in that, The preset mode switching conditions for switching from the altitude holding mode to the descent mode include: the target ground altitude value at the previous moment is less than the minimum safe ground altitude; The preset mode switching conditions for switching from the altitude holding mode to the ascent mode include: the target ground altitude value at the previous moment is greater than the maximum safe ground altitude, and the route altitude margin at the previous moment is greater than the heading margin hysteresis. The preset mode switching conditions for switching from the descent mode to the altitude holding mode include: the target ground altitude value at the previous moment is greater than a first altitude threshold, and the flight path altitude margin at the previous moment is positive, wherein the first altitude threshold is determined based on the minimum safe ground altitude and the altitude hysteresis. The preset mode switching conditions for switching from the ascent mode to the altitude holding mode include: the target altitude value above the ground at the previous moment is less than a second altitude threshold, the second altitude threshold being determined based on the maximum altitude above the ground and the altitude hysteresis.

7. The method for unmanned aerial vehicle (UAV) ground-following flight according to claim 1, characterized in that, In the currently active terrain-following flight mode, based on the target flight state vector at the current moment, terrain-following flight control commands are generated, including: Extract the ground altitude target value from the target flight state vector at the current moment; In the currently active ground-following flight mode, the flight altitude target is determined based on the ground altitude target value at the current moment; The dynamic integral limit value is determined based on the altitude error between the ground altitude target value and the flight altitude target at the current moment; Based on the dynamic integral limit value, the terrain-following flight control command at the current moment is determined.

8. The method for unmanned aerial vehicle (UAV) ground-following flight according to claim 7, characterized in that, Based on the dynamic integral limit value, the terrain-following flight control command at the current moment is determined, including: The vertical velocity adjustment amount at the current moment is determined based on the altitude error and the dynamic integral limit value. Determine the horizontal speed scaling factor corresponding to the currently active terrain-following flight mode; Using the vertical speed adjustment and the horizontal speed scaling factor, the speed command is corrected to obtain the terrain-following flight control command at the current moment.

9. The method for unmanned aerial vehicle (UAV) ground-following flight according to claim 8, characterized in that, After correcting the speed command using the vertical speed adjustment and the horizontal speed scaling factor to obtain the terrain-following flight control command at the current moment, the method further includes: If the altitude holding mode is satisfied based on the currently active terrain-following flight mode, the vertical speed command in the terrain-following flight control command is frozen.

10. A ground-following flight device for unmanned aerial vehicles, characterized in that, include: The navigation estimation module is used to estimate the target flight state vector of the UAV at the current moment based on the current sensor data of the UAV and the target flight state vector at the previous moment; The mode switching or holding module is used to switch or hold the current state of the UAV based on the target flight state vector and flight path altitude margin at the previous moment, as well as the effective terrain-following flight mode, so as to obtain the currently effective terrain-following flight mode. The guidance and control module is used to generate a ground-following flight control command at the current moment based on the target flight state vector at the current moment in the currently effective ground-following flight mode. The command response module is used to execute ground-following flight actions in response to the ground-following flight control command.

Images