A barometer data abnormal state unmanned plane safe landing control method

Abstract

This invention discloses a control method for safe landing of a UAV in abnormal barometer data states. It is based on a dual-loop cascade PID control algorithm for UAV barometer altitude hold-up, with an outer loop of altitude and an inner loop of velocity. The altitude measurement value of the altitude loop and the velocity measurement value of the velocity loop are respectively obtained by fusing the relative altitude H of the UAV measured by the barometer as the observation value and fusing the UAV IMU data using a Kalman filter algorithm. 融合 and fusion speed V 融合 The control method of this invention is not easily affected by barometer fluctuations, and can control the drone throttle more stably and linearly, reducing the possibility of full-amplitude throttle fluctuations, thereby improving the stability and robustness of drone landing, avoiding problems such as large-scale drone ascent and descent, and the drone's payload becoming lighter, resulting in the inability to reach the target speed. The control method of this invention can be unified into the automatic landing mode to simplify the control logic and improve the robustness of control.

Description

Technical Field

[0001] This invention relates to the field of multi-rotor unmanned aerial vehicle (UAV) technology, and more specifically to a control method for the safe landing of a UAV in the event of abnormal barometer data. Background Technology

[0002] Multirotor drones, with their significant advantages such as strong vertical takeoff and landing capabilities, flexible operation, and wide applicability, have been increasingly widely used in many fields, including model aircraft entertainment, agricultural plant protection, fire rescue, and aerial photography and surveying. Achieving precise and stable altitude control is the core prerequisite for multirotor drones to safely and efficiently complete various tasks.

[0003] Currently, mainstream altitude-keeping sensors for drones include barometers, GPS, ultrasonic sensors, and vision systems. These sensors, based on different measurement principles, exhibit significant differences in performance and applicable scenarios: GPS obtains absolute altitude through satellite positioning. While suitable for long-distance navigation in open areas, it is prone to decreased positioning accuracy (typically 3-10 meters error) in complex terrain (such as urban canyons or high-rise buildings) or signal-blocked environments. Furthermore, its data update rate is insufficient to meet the demands of dynamic altitude control, and it is generally not used as a primary altitude-keeping sensor. Ultrasonic sensors rely on sound wave reflection for ranging, achieving high-precision measurements within 5 meters at low altitudes and on flat ground. However, they are limited by their range and are susceptible to interference from obstacle shapes, materials, and environmental noise. Vision systems (such as optical flow and stereo vision) estimate relative altitude through ground texture displacement. They perform excellently in well-lit, feature-rich low-altitude environments, but are prone to failure in low-light, water-based, or plain-colored ground environments without texture.

[0004] As a sensor that converts relative altitude into atmospheric pressure, the barometer is unaffected by terrain obstruction and can quickly respond to altitude changes, making it the main sensor for altitude control at mid-to-high altitudes (above 5 meters). However, its measurement accuracy is heavily dependent on the stability of the atmospheric environment. When encountering strong local airflow (such as gusts between tall buildings or airflow in canyons) or sudden temperature changes (such as in areas where cold and hot air currents converge), the air pressure value will fluctuate drastically, resulting in a deviation of ±(0.5-2) meters in the converted relative altitude. Moreover, barometer malfunctions are mostly manifested as abnormal jumps in air pressure data or maintaining a fixed value that no longer changes with air pressure, rather than faults that can be easily identified through the program, such as errors in reading the barometer chip ID or communication errors with the MCU. This can easily lead to uncontrollable phenomena such as large jumps in altitude or continuous ascent of the UAV, seriously affecting the stability of altitude control. In particular, a crash after uncontrolled continuous ascent will pose a significant safety hazard to people and property on the ground.

[0005] In the prior art, the altitude of the barometer is generally determined by fusing IMU data and adopting a double-loop cascade PID to control the altitude of the UAV. In the case of abnormal barometer data as described above, the fusion algorithm is prone to cause the overall altitude judgment to be distorted due to incorrect observation quantities. Due to the jump or failure of the feedback data, it is no longer possible to control the UAV to land safely at a certain speed through closed-loop PID. Moreover, the payload of the UAV is different in different usage scenarios. If a fixed preset motor throttle is used for landing, the problem will occur that the UAV may rise when it is lightly loaded and descend too fast when it is heavily loaded, and safe landing cannot be achieved. Summary of the Invention

[0006] The technical problem to be solved by this invention is to overcome the above-mentioned shortcomings of the prior art: to provide a control method for the safe landing of a UAV in the state of abnormal barometer data.

[0007] The technical solution of this invention is as follows: A control method for the safe landing of a UAV in the state of abnormal barometer data, based on the cascade PID altitude control algorithm, the UAV realizes altitude-keeping flight by dynamically adjusting the magnitude of the average motor throttle value T through the sum of the basic throttle amount T

[0008] , c , min ,

[0011] , c ,

[0010] , p ,

[0009] , c , p , , base , , , , p and the output value of the cascade PID; when the UAV enters the forced landing mode, the UAV controller first sets the output of the outer loop PID to zero, sets the target speed of the inner loop PID to V0, and then calculates the altitude change amount dH of the UAV every dt time and compares it with V0×dt. If dH < V0×dt, the target speed of the inner loop PID increases until it is greater than or equal to the maximum preset threshold V max and then remains unchanged. When the target speed of the inner loop PID reaches V max , if dH < V0×dt still holds, the basic throttle amount T base decreases until T base is less than or equal to the lowest preset threshold T min and then remains unchanged.

[0008] As an optimization, the V0 is a positive landing speed.

[0009] As an optimization, the altitude change amount dH = H p - H c , H c is the current fused altitude value, H p is the previous fused altitude value, and the time interval between H p and H c is dt.

[0010] As an optimization, the cascade PID is a double-loop cascade PID composed of an outer loop altitude loop and an inner loop speed loop.

[0011] As an optimization, the altitude measurement value of the altitude loop and the speed measurement value of the speed loop are respectively obtained by fusing barometer data and IMU data.

[0012] As an optimization, the output value of the cascaded PID is limited so that the maximum value of the cascaded PID output value is close to the minimum preset threshold T. min The sum is less than the hovering throttle of the drone when it is unloaded.

[0013] As an optimization, after the drone takes off, the variance Var of the barometer data is calculated in real time using the sliding window method. When the variance Var is greater than the set threshold or the variance Var is 0 for a certain period of time, it is judged that the barometer data is abnormal and the drone enters the emergency landing mode.

[0014] As an optimization, the window size of the sliding window method is 50-100.

[0015] As an optimization, the threshold value for the variance is set to be 3-5 times the normal data variance of the barometer; the normal data variance of the barometer is obtained by statistically calculating the variance of the barometer data during normal flight of the UAV.

[0016] As an optimization, the drone also includes a laser ranging module. After entering forced landing mode, data from the laser ranging module is read every dt time. If the laser ranging module data is valid and the value is less than its measurement range, the altitude observation is switched from barometer measurement to laser ranging module measurement; upon recovery, T... base With the initial value and the inner loop PID target speed set to V0, the drone forced landing control logic is executed again.

[0017] The specific control methods are as follows:

[0018] 1) During drone initialization, barometer data is read to calculate the drone's initial altitude A0. After initialization, barometer data is periodically read to calculate the drone's current altitude A in real time. c This allows for the real-time calculation of the drone's relative height H = A relative to its initial position. c - A0;

[0019] 2) Using the measured relative height H as the observation value, the Kalman filter algorithm is used to fuse the data from the UAV's inertial measurement unit (IMU) to obtain the fused height H. 融合 and fusion speed V 融合 Then with H 融合 and V 融合 To measure the feedback value, a dual-loop cascade PID controller is used to dynamically adjust the average motor throttle value T of the multi-rotor UAV based on the set target altitude to achieve altitude-hold flight; wherein, the average motor throttle value T is equal to the base throttle value T. base The sum of the output values ​​of the cascaded PID controllers;

[0020] 3) After the UAV takes off, the variance Var of the relative height H is calculated using the sliding window method. When the variance Var is greater than the set threshold or the variance Var is 0 for a certain period of time, it is determined that the barometer data is abnormal; the UAV enters the forced landing mode;

[0021] 4) After entering the forced landing mode, the UAV controller first sets the outer loop PID output to zero, and sets the inner loop PID target speed to the initial landing speed V0 (V0>0). The controller calculates H every time interval dt 融合 The change amount dH = H p - H c , where H p is the previous fused height value, and H c is the current fused height value. Then update H p The value is equal to the current fused height value H c for the next calculation of dH. If dH < V0×dt, the inner loop PID target speed is set to V0 + N×dV, where N is a positive integer representing the Nth calculation of dH by the controller; dV is a positive step value. When the inner loop PID target speed is greater than or equal to the maximum preset threshold V max , keep the inner loop PID target speed equal to V<s max , when the inner loop PID target speed is equal to V max , continue to calculate every time interval dt and determine whether the condition dH < V0×dt holds. If dH < V0×dt still holds, then after each judgment, T base is decremented by a fixed positive step value dT base , until T base is less than the lowest preset threshold T min , keep T base =T min to achieve the safe landing of the UAV.

[0022] The beneficial effects of the present invention are as follows:

[0023] 1) When the present invention enters the forced landing mode, the outer loop PID output is set to zero, and the inner loop PID target speed is set to the initial landing speed, which can reduce the large step change of the outer loop PID output caused by the large fluctuation and jump of the relative height data of the barometer. Since the inner loop PID speed is mainly dominated by the acceleration sensor of the IMU in the Kalman fusion process, the data of the IMU acceleration sensor is relatively stable and is not easily affected by the barometer fluctuation. Therefore, it can control the throttle of the UAV more stably and linearly, reduce the possibility of full throttle fluctuation, and thus improve the stability and robustness of the UAV landing, and avoid causing large up and down movements of the UAV.

[0024] 2) If the variance Var of the barometer output altitude data remains 0 continuously, it indicates that the barometer altitude data failure is manifested as outputting a fixed value and no longer reflecting altitude changes. Compared with decreasing the base throttle amount T at a fixed step value base The algorithm for implementing forced landing. In the present invention, the inner-loop PID can effectively utilize the instantaneous speed change estimated by the IMU accelerometer, and can effectively avoid the risk of the UAV crashing due to continuously reducing the base throttle amount一味地导致快速下降.

[0025] 3) The controller of the present invention calculates the change dH of the interval landing altitude every time interval dt; if dH < V0×dt, first slowly increase the inner-loop PID target speed until reaching the maximum preset threshold V max and then, if dH < V0×dt, decrease the base throttle value T base The above control method can solve the problem that the PID control cannot reach the target speed due to the data fluctuation of the barometer caused by ascending turbulence. Although the above turbulence causes the barometer to fluctuate, the barometer can still normally reflect altitude changes as a whole. If the descending speed cannot reach the preset speed of the inner-loop PID only due to ascending air current, the target speed can be increased to make dH > V0×dt and maintain the base throttle value T base Combine the output of the inner-loop PID to control the UAV to land at a constant speed. The above control method can also solve the problem that the PID control cannot reach the target speed due to the decrease in the load of the UAV during the forced landing process.

[0026] 4) When the control method of the present invention manually enters the forced landing mode in the normal mode of the barometer data, if the condition dH < V0×dt never holds, the UAV will land at a constant speed with the inner-loop PID target speed V0. Therefore, the control method of the present invention can be unified into the automatic landing mode to simplify the control logic and improve the robustness of the control.

[0027] 5) The load of the UAV is not the same in different usage scenarios. If a fixed preset motor throttle is used for landing, problems such as the UAV may rise instead when it is lightly loaded and the descending speed is too fast when it is heavily loaded will occur, and safe and stable landing cannot be achieved. The maximum value of the output of the cascade PID of the present invention and the lowest preset threshold T min The sum is less than the hover throttle of the UAV when it is unloaded, so as to ensure that the UAV can also land by decreasing the base throttle value T base to the maximum throttle less than its own weight maintenance throttle, avoiding the situation that the UAV still continues to rise under the worst conditions. It effectively solves the problem of safe landing of the UAV under different loads, so that the UAV has the ability to cope with various barometer failures and environmental disturbances. Brief Description of the Drawings

[0028] Figure 1This is a schematic diagram of the cascaded PID principle in Example 1.

[0029] Figure 2 This is a schematic diagram of the control flow after entering the forced landing mode in Example 1. Detailed Implementation

[0030] The present invention will be further described in detail below with reference to specific embodiments, but the present invention is not limited to the following specific embodiments.

[0031] Example 1

[0032] This embodiment is based on a dual-loop cascaded PID control algorithm for a quadrotor UAV barometer altitude hold-up control, with an outer loop being an altitude loop and an inner loop being a velocity loop. Figure 1 As shown, dual-loop cascaded PID is a commonly used loop control algorithm, consisting of two PID loops, referred to as the outer loop and the inner loop. The outer loop controls the aircraft's altitude, while the inner loop controls the speed. The inner loop (speed loop) has a faster response and can quickly suppress disturbances; the outer loop (altitude loop) focuses on final altitude accuracy, improving system robustness, and is a conventional technology. Taking a UAV equipped with a main controller STM32F411CEU6, an inertial measurement unit MPU6500, and a barometer DPS368 as an example, during UAV initialization, the main controller STM32F411CEU6 reads data from the barometer DPS368 to calculate the initial altitude A0 of the UAV. After initialization, it periodically reads the barometer data every 2ms to calculate the current altitude A of the UAV in real time. c This allows for the real-time calculation of the drone's relative height H = A relative to its initial position. c - A0;

[0033] Then, the measured relative height H is used as the height observation value of the Kalman filter algorithm, and fused with the vertical acceleration measured by the UAV IMU in the inertial coordinate system to obtain the fused height H. 融合 and fusion speed V 融合 The Kalman filter algorithm is a conventional technique. Because barometer measurements alone have significant latency, their smoothness and real-time performance are inferior to those of the accelerometer and gyroscope in an IMU (Inertial Measurement Unit). Therefore, it is generally necessary to fuse barometer data with IMU data to significantly reduce data latency and improve the real-time performance and smoothness of the fused data. The state prediction model for the prediction stage of the Kalman filter algorithm is as follows:

[0034]

[0035] Among them, D t In this embodiment, D represents the data sampling time interval. t = 2ms; S t and S t-1These are the drone's current altitude and D. t The height V before the time interval t and V t-1 These are the current vertical velocity and D of the drone, respectively. t The vertical velocity prior to the time interval, where 'a' is the vertical acceleration measured by the current IMU inertial measurement unit MPU6500 in the inertial coordinate system.

[0036] The above equation can be written in matrix form as follows:

[0037]

[0038] According to the state update equation of Kalman filtering:

[0039]

[0040] The state vector at the current moment D t State vector before the time interval

[0041] .

[0042] State transition matrix

[0043] Control input matrix

[0044] Control Vector .

[0045] Since the measured relative height H is used as the height observation value for the Kalman filter algorithm, the observation vector of the Kalman filter... The observation matrix is The observation noise covariance is set to the barometer DPS368 variance. The initial prediction error covariance P and the initial process noise covariance Q in the Kalman filter measurement update stage can be set as identity matrices, which will be automatically iteratively updated. Since Kalman filtering is an existing technology, the equations for covariance prediction and measurement update stages will not be elaborated here.

[0046] Then with H 融合 and V 融合 The altitude measurement value of the cascaded PID altitude loop and the speed measurement value of the speed loop are used to dynamically adjust the average motor throttle value T according to the set target altitude to achieve altitude hold flight of the UAV. In order to facilitate the adjustment and control of the UAV altitude, the average motor throttle value T in this embodiment includes the base throttle value T. base With cascaded PID output value; base throttle T baseIt provides the "baseline power" needed for drone hovering, offsetting its own gravity, so that the cascaded PID only needs to handle small altitude deviations, avoiding flight instability caused by large fluctuations in PID output. The specific throttle value of each motor of the quadcopter is also superimposed with the values ​​of three self-stabilizing PIDs: roll, pitch, and yaw. However, their final average value is 0, and altitude control is only related to the average motor throttle value T.

[0047] Base throttle T base Generally, the base throttle T is determined based on the average throttle of the four motors when the drone is hovering unloaded, or by setting the cascade PID output value to 0 and using the average throttle of the four motors when the drone just lifts off the ground when the throttle is slowly increased remotely. base The throttle position of the motor refers to the quantitative drive power of the electronic speed controller (ESC) of the drive motor, typically a linear value ranging from 0 to 2000. In this embodiment, T... base =1200, the range of the cascaded PID output value is limited to -500 to +500.

[0048] After the UAV takes off, the variance Var of the relative altitude H is calculated using the sliding window method. The sliding window method for solving data variance is a conventional technique. In this embodiment, the size of the sliding window is 50.

[0049] When the variance Var exceeds the set threshold or remains 0 for 5 seconds, it is determined that the barometer data is abnormal; the drone enters emergency landing mode. A variance Var exceeding the set threshold indicates excessive fluctuation in barometer data (e.g., strong airflow, sensor malfunction). Var = 0 for a certain period indicates sensor malfunction or communication interruption, as barometers have a high noise floor and their values ​​fluctuate continuously; a value remaining at 0 for an extended period indicates a barometer malfunction. The set threshold for variance is three times the normal data variance of the barometer; the normal data variance of the barometer is obtained by statistically calculating the data variance of the DPS368 barometer during normal drone flight.

[0050] like Figure 2 As shown, after entering the forced landing mode, the drone controller first sets the outer loop PID output to zero, that is, it disables the outer loop altitude PID, retains the inner loop speed PID, and sets the inner loop PID target speed to a landing speed of 1 m / s. The controller calculates H every 0.1s. 融合 The change in dH = H p - H c H p H is the previous fusion height value. c The current fusion height value is used; H is updated after dH calculation is complete. p The value is equal to H cThis is used for the next calculation of dH; if dH < 0.1m, the inner loop PID target speed is set to (1 + N × 0.01) m / s, where N is a positive integer representing the Nth calculation of the controller's change dH; where 0.01 is the target speed increment step size; when the inner loop PID target speed is greater than or equal to the maximum preset threshold V... max Maintain the inner loop PID target speed equal to V. max And continue to determine whether the condition dH < 0.1m is met; in this embodiment V max If the inner loop PID target speed is 5 m / s, and dH < 0.1 m, then the speed V of the IMU and barometer fusion is within acceptable limits. 融合 If it completely fails to reflect the actual speed changes of the drone, then T base Each time, it decrements by a fixed positive step value dT. base That is, when the inner loop PID target speed reaches 5m / s, if dH calculated every 0.1s is less than 0.1m, then T is set to... base Subtract dT base Until T base Less than the minimum preset threshold T min Keep T base =T min In this embodiment, the positive step size dT is fixed. base =2, minimum preset threshold T min =500. T base When the value is 500, even if the cascaded PID output reaches its maximum value + 500, the average motor throttle value T = 500 + 500 = 1000, which is less than the hovering throttle value of 1200, ensuring that the drone will not experience continuous ascent in the worst-case scenario. However, if the abnormal barometer data is due to turbulence and returns to normal during descent, then the condition that dH > 0.1m is met when the inner loop PID target speed is greater than 1m / s is satisfied. Therefore, the inner loop PID target speed and T... base The descent will no longer change, and the drone can descend at a constant speed, which also shows that this landing control logic is also applicable to normal drone descent control.

[0051] Example 2

[0052] Unlike Embodiment 1, the UAV in this embodiment also includes a bottom laser ranging module VI5300, with an effective ranging distance of 4 meters.

[0053] After entering forced landing mode, first set the outer loop PID output to zero, that is, disable the outer loop altitude PID, retain the inner loop speed PID, and set the inner loop PID target speed to a descent speed of 1 m / s; then execute the following steps:

[0054] 1) The controller calculates H every 0.1s. 融合 The change in dH = Hp - H c H p H is the previous fusion height value. c The current fusion height value is used; H is updated after dH calculation is complete. p The value is equal to H c For use in the next dH calculation;

[0055] 2) If dH < 0.1m, the inner loop PID target speed is set to (1 + N × 0.01) m / s, where N is a positive integer representing the calculation of the Nth change dH of the controller; and 0.01 is the target speed increment step value.

[0056] 3) When the inner loop PID target speed is greater than or equal to 5 m / s, maintain the inner loop PID target speed at 5 m / s. When the inner loop PID target speed is equal to 5 m / s, continue to check whether the condition dH < 0.1m is met every 0.1s. If dH < 0.1m is still true, then let T... base Subtract the step size value of 2 until T. base Keep T when less than 500 base =500.

[0057] Unlike Example 1, after entering the forced landing mode, data from the laser ranging module VI5300 is read every 0.1 seconds. If the data from the laser ranging module VI5300 is valid and less than 4 meters, the Kalman filter algorithm switches the relative height H measured by the barometer to the height data measured by the laser ranging module VI5300 to fuse the vertical acceleration measured by the MPU6500, thus obtaining the fused height H. 融合 and fusion speed V 融合 ;Restore T base =1200 and the inner loop PID target speed is set to 1m / s. Then, execute the control logic steps 1) to 3) again. When the laser fusion height H... 融合 The motor will shut off after the distance is less than 5cm for 2 seconds.

[0058] The control algorithm in this embodiment can integrate laser data when the drone approaches the ground to achieve more precise target speed descent. If T... base When the barometer data reaches the minimum value of 500, it means that the barometer data cannot reflect the actual speed of the drone. Although the drone can descend normally, the speed is not the accurate target speed. However, after connecting to the laser data, the normal descent speed can be restored, and a uniform descent effect with near-ground laser PID intervention can be achieved.

[0059] The above are merely exemplary embodiments of the present invention and do not constitute any limitation on the scope of protection of the present invention. All technical solutions formed by equivalent exchange or substitution fall within the scope of protection of the present invention.

Claims

1. A control method for safe landing of a UAV in a state of abnormal barometer data, based on a cascaded PID altitude control algorithm, characterized in that, The drone passes through the base throttle T base The average motor throttle value T is dynamically adjusted by summing the cascaded PID output values ​​to achieve constant altitude flight. When the drone enters emergency landing mode, the drone controller first sets the outer loop PID output to zero, sets the inner loop PID target speed to V0, and then calculates the drone altitude change dH every dt time interval and compares it with V0×dt. If dH < V0×dt, the inner loop PID target speed increases until it is greater than or equal to the maximum preset threshold V. max Maintain this speed until the inner loop PID target speed reaches V. max Then, if dH < V0×dt still holds, then the base throttle quantity T base Decrease until T base Less than or equal to the minimum preset threshold T min The time is maintained; V0 is the descent speed with a positive value; the altitude change dH = H p -H c H c H represents the current fusion height value. p H is the previous fusion height value. p and H c The time interval is dt; the altitude measurement value of the altitude loop and the velocity measurement value of the velocity loop are obtained by fusing barometer data with IMU data; the output value of the cascaded PID is limited so that the maximum value of the cascaded PID output value is close to the minimum preset threshold T. min The sum is less than the hovering throttle of the drone when it is unloaded.

2. The control method for safe landing of a UAV in abnormal barometer data state according to claim 1, characterized in that: The cascaded PID is a dual-loop cascaded PID consisting of an outer height loop and an inner speed loop.

3. The control method for safe landing of a UAV in abnormal barometer data state according to claim 1, characterized in that: After the drone takes off, the variance Var of the barometer data is calculated in real time using the sliding window method. When the variance Var is greater than the set threshold or the variance Var is 0 for a certain period of time, it is judged that the barometer data is abnormal and the drone enters the emergency landing mode.

4. The control method for safe landing of a UAV in abnormal barometer data state according to claim 3, characterized in that: The window size for the sliding window method is 50-100.

5. The control method for safe landing of a UAV in abnormal barometer data state according to claim 3, characterized in that: The threshold value for the variance is set to be 3-5 times the normal data variance of the barometer; the normal data variance of the barometer is obtained by statistically calculating the variance of the barometer data during normal flight of the UAV.

6. The control method for safe landing of a UAV in abnormal barometer data state according to claim 5, characterized in that: The drone also includes a laser ranging module. Upon entering forced landing mode, it reads data from the laser ranging module every dt time interval. If the laser ranging module data is valid and the altitude is below its measurement range, the altitude observation is switched from barometer measurement to laser ranging module measurement; T is then restored. base With the initial value and the inner loop PID target speed set to V0, the drone forced landing control logic is executed again.

Images