A multi-rotor unmanned aerial vehicle hovering method

Abstract

This invention provides a hovering method for a multi-rotor unmanned aerial vehicle (UAV), comprising: step S1, obtaining the attitude angle and altitude position of the UAV body through multiple sensors, and establishing a dynamic model of the UAV based on the attitude angle and altitude position; step S2, designing a PID-modified complementary filter algorithm; step S3, designing a quaternion-based extended Kalman filter algorithm; and step S4, inputting the data analyzed and designed by the extended Kalman filter algorithm in step S3 into the power system of the UAV to achieve hovering control of the UAV. Compared with related technologies, the hovering method of this invention for multi-rotor UAVs has high practicality, good anti-interference performance, high control accuracy, easy parameter tuning, and low system computation.

Description

Technical Field

[0001] This invention relates to the field of drone hovering technology, and more particularly to a hovering method for multi-rotor drones. Background Technology

[0002] With the development of aviation and electronic technologies, multi-rotor drones, due to their high maneuverability, safety, and reliability, have found wide applications in various fields such as military intelligence reconnaissance, agricultural plant protection, factory inspection, forest fire fighting, and aerial photography, requiring precise hovering. Simultaneously, in increasingly complex scenarios, the hovering accuracy of multi-rotor drones is being demanded to be even higher. The hovering accuracy of multi-rotor drones largely determines the quality of mission completion. Therefore, multi-rotor drone hovering methods have significant engineering importance and value in practical applications.

[0003] The hovering methods for multi-rotor UAVs in related technologies generally employ fuzzy adaptive cascade PID control, sliding mode control hovering, neural network training hovering, and optical flow positioning hovering.

[0004] However, the fuzzy adaptive cascade PID control method directly uses the error between the system input and output for hovering control, without relying on the system model. Furthermore, its algorithm is susceptible to external disturbances and suffers from excessive overshoot. When applied to environments with significant or complex disturbances, it struggles to ensure stability during hovering of multi-rotor UAVs. The real-time requirements of UAV hovering systems are high, while the adaptive cascade PID control algorithm relies on accurate models and powerful computing capabilities. The computational load and real-time performance of the fuzzy adaptive cascade PID control method's system solution need improvement. Sliding mode control hovering methods exhibit strong robustness to uncertain model systems and external factors, and respond quickly to control objectives. However, ordinary sliding mode control hovering methods suffer from problems such as the inability to converge steady-state errors within a reasonable time and jitter along the sliding surface during sliding, and are difficult to implement. Neural network training hovering methods possess self-organizing and self-learning capabilities, strong fault tolerance, and robustness, but their stability is poor, and their learning speed is slow, making it difficult to meet the requirements of real-time UAV hovering control. The accuracy of optical flow positioning hovering methods can be affected by any factor that might influence visual judgment. When the ground is a single color, has a repetitive pattern, is a mirror, water surface, or a highly reflective surface, it will severely impact the drone's optical flow camera's judgment. Even if the drone deviates, its optical flow positioning system will not detect it. Therefore, optical flow positioning hovering methods are only suitable for relatively simple flat environments. For complex, uneven ground environments, the error of optical flow hovering methods is larger, failing to achieve the ideal hovering height and hovering area, and thus failing to meet the precise hovering requirements of multi-rotor drones. Summary of the Invention

[0005] To address the shortcomings of the existing technologies, this invention proposes a hovering method for multi-rotor UAVs that is highly practical, has good anti-interference performance, high control precision, easy parameter tuning, and low system computation.

[0006] To address the aforementioned technical problems, embodiments of the present invention provide a hovering method for a multi-rotor unmanned aerial vehicle (UAV), the method comprising the following steps:

[0007] Step S1: Obtain the attitude angle and altitude position of the multi-rotor UAV through multiple sensors of the multi-rotor UAV, and establish a dynamic model of the multi-rotor UAV based on the attitude angle and altitude position;

[0008] Step S2: Design an improved PID complementary filter algorithm; specifically, by using a PID controller to adjust the cutoff frequency of the filter of the multi-rotor UAV, eliminate static error time, suppress data vibration, and enhance the system's speed.

[0009] Step S3: Design of Extended Kalman Filter Algorithm based on Quaternions; Specifically, using the quaternions obtained in Step S2 as input data, combined with the dynamic model established in Step S1, the Extended Kalman Filter algorithm is used for analysis and design to obtain the estimated attitude parameters and vertical acceleration, velocity, or position parameters of the multi-rotor UAV in each update cycle.

[0010] Step S4: Input the data analyzed and designed by the extended Kalman filter algorithm in step S3 into the power system of the multi-rotor UAV to achieve hovering control of the multi-rotor UAV. The hovering control is specifically as follows: when the multi-rotor UAV receives a hovering command from the outside, if the multi-rotor UAV deviates due to external factors, the acceleration value measured by the accelerometer of the multi-rotor UAV is integrated to calculate the displacement. Based on the displacement, the power system is controlled to move the same amount of displacement in the opposite direction of the displacement, so as to achieve rapid correction of the multi-rotor UAV during the hovering process and return to the initial hovering position.

[0011] Preferably, in step S1, the sensor includes an accelerometer, a gyroscope, a magnetometer, an ultrasonic sensor, and a barometer.

[0012] Preferably, step S1 includes:

[0013] Step S11: Obtain multiple sensor data through the sensor;

[0014] Step S12: Calculate attitude information using the data obtained from the accelerometer, the gyroscope, and the magnetometer;

[0015] Step S13: Calculate the altitude fusion estimate and optimal altitude value using the data obtained from the accelerometer, the ultrasonic waves, and the barometer.

[0016] Preferably, in step S12, the attitude information includes attitude angle x. acc and attitude angle y gyro And satisfy the following formula:

[0017]

[0018] Among them, u H The high-frequency noise introduced by the accelerometer, u L The low-frequency noise introduced by the gyroscope, x is a variable of the magnetometer, and y is another variable of the magnetometer;

[0019] In step S13, the data obtained by the accelerometer and the data obtained by the ultrasonic wave are fused together to determine the altitude, satisfying the following formula:

[0020] a zN =ga z ;

[0021] Among them, a zN The value of the Z-axis in the Earth coordinate system output by the accelerometer is given by g, where g is the acceleration due to gravity on Earth, and a is the acceleration due to gravity on Earth. z This represents the acceleration along the Z-axis of the Earth coordinate system, where the Z-axis is the direction of gravity.

[0022] The initial height is h0, and the height fusion estimate at any time is h0. ac It satisfies the following formula:

[0023]

[0024] v0 is the initial velocity of the multi-rotor UAV, T s The data obtained by the ultrasound;

[0025] The optimal height is h, which satisfies the following formula:

[0026] h = k × h ac (1-k)h aq ;

[0027] h aq The height fusion estimate is obtained by fusing the data obtained from the accelerometer and the data obtained from the barometer, where k is a variable parameter.

[0028] Preferably, step S2 includes:

[0029] Step S21: Based on the data obtained from the accelerometer, the data obtained from the magnetometer, the Earth's gravitational acceleration, and the error of the geomagnetic field strength, the gyroscope data is compensated using the PID improved complementary filtering algorithm;

[0030] Step S22: Update the quaternion using the data from the compensated gyroscope through the first-order Runge-Kutta equation.

[0031] Preferably, in step S21, the error of the geomagnetic field strength is... Satisfy the following formula:

[0032]

[0033] for The value is obtained after normalization. Let a be the vector parameter of the accelerometer. x a y a z These represent the accelerations along the X-axis, Y-axis, and Z-axis, respectively, with T being a variable parameter.

[0034] Let b be the gravitational acceleration in the body coordinate system of the multi-rotor UAV;

[0035] for The value is obtained after normalization. Let m be the vector parameter of the magnetometer. x m y m z These represent the magnetic force values ​​along the X-axis, Y-axis, and Z-axis, respectively.

[0036] The geomagnetic intensity of the B series;

[0037] The gravitational accelerations in the Earth coordinate system N are [0 0 0] and [0 0 0], respectively. T With geomagnetic intensity [b x 0 z ] T After rotation matrix transformation, b is obtained. x Let b be the geomagnetic intensity along the X-axis. z The geomagnetic intensity is along the Z-axis, and the rotation transformation matrix is: Satisfy the following formula:

[0038]

[0039] b is a parameter, N is a parameter;

[0040] q0, q1, q2, and q3 are the four component parameters of the quaternion;

[0041] In step S22, the output of the PID improved complementary filter algorithm is: Satisfy the following formula:

[0042]

[0043]

[0044]

[0045]

[0046] K p K is the proportional adjustment coefficient. i K is the integral adjustment coefficient, t is the time parameter, and K is the integral adjustment coefficient. d The differential adjustment coefficient;

[0047] This is the corrected angle value. The angle value obtained by the gyroscope;

[0048] q is the quaternion, The parameter variables of the quaternion;

[0049] ω x ω y ω z These are the angle values ​​in the X-axis direction, the Y-axis direction, and the Z-axis direction, respectively.

[0050] Preferably, step S3 includes:

[0051] Step S31: Establish the nonlinear model of the extended Kalman filter algorithm;

[0052] Step S32: Linearize the function of the nonlinear model at the state point by Taylor series expansion to realize the time update of the system.

[0053] Preferably, in step S31, the nonlinear model includes a state equation and a measurement equation.

[0054] The state equation is: X k =f(X) k-1 W k-1 );

[0055] The measurement equation is: Z k =h(X) k V k );

[0056] Among them, X k Let f() be the state estimation vector of the system at time k, and let X be the state estimation vector of the system at time k. k Nonlinear function, X k-1 W is the state estimation vector of the system at time k-1. k-1 W represents the process noise of the system at time k-1. k The process noise at time k is the system noise.

[0057] Z k Let h() be the measurement vector of the system at time k, and V be the observation equation. k For measuring noise; process noise W k With measurement noise V k Irrelevant;

[0058] In step S32

[0059] The function of the nonlinear model at the state point satisfies the following formula:

[0060]

[0061]

[0062]

[0063] in, For state The state estimate after filtering using the extended Kalman filter algorithm, where f() is the filtering function;

[0064] Let P be the covariance matrix at time k. k-1 Let Q be the covariance matrix at time k. k It is the variance matrix of the system noise. The system state equations X are respectively k The variables of the Jacobian matrix; According to The measurement matrix obtained at time k;

[0065] The time update of the system satisfies the following formula:

[0066]

[0067]

[0068]

[0069] Among them, K k The extended Kalman gain at time k, According to and The state estimate at time k after the update at time k-1, where I is the parameter variable and R is the state estimate at time k-1. k To measure the variance matrix of the noise, Let be the covariance matrix at time k. The system measurement equations Z are respectively k The variables of the Jacobian matrix.

[0070] Preferably, the external factor includes wind.

[0071] Compared with existing technologies, the multi-rotor UAV hovering method of the present invention obtains the attitude angle and altitude position of the multi-rotor UAV through multiple sensors of the multi-rotor UAV in step S1, and establishes a dynamic model of the multi-rotor UAV based on the attitude angle and altitude position. Implementing step S1, the dynamic model solves the problem of low measurement accuracy of the multi-rotor UAV at different altitudes. For the hovering altitude of the multi-rotor UAV, different altitude measurement sensors are selected, which not only improves measurement accuracy and real-time performance, but also improves the redundancy of the altitude measurement system. In step S2, an improved PID complementary filtering algorithm is designed. Implementing step S2, the improved PID complementary filtering algorithm is used for the multi-attitude data fusion process of the control system of the multi-rotor UAV in practical applications. By using the PID controller to select appropriate parameters to adjust the cutoff frequency, the error between the measurement data obtained from the accelerometer and magnetometer of the multi-rotor UAV and the gravitational acceleration and geomagnetic field strength is used to compensate for the gyroscope attitude data. This comprehensively considers the static and dynamic errors of the multi-rotor UAV, suppresses the vibration of the attitude data, and enhances the speed and stability of the hovering system of the multi-rotor UAV. Step S3: Design of an Extended Kalman Filter Algorithm Based on Quaternions; Implementing Step S3, the use of the Extended Kalman Filter algorithm in conjunction with adaptive altitude fusion estimation not only improves the stability of the multi-rotor UAV but also optimizes the system model. This solves the problems of high matrix computation and computational complexity when using Kalman filtering alone for hovering, which prevents the multi-rotor UAV from achieving stable hovering under wind interference and limiting hovering altitude. Step S4: Input the data analyzed and designed by the Extended Kalman Filter algorithm in Step S3 into the power system of the multi-rotor UAV to achieve hovering control. Implementing Step S4 allows for rapid correction during the hovering process, returning the multi-rotor UAV to its initial hovering position. Attached Figure Description

[0072] The present invention will now be described in detail with reference to the accompanying drawings. The above and other aspects of the present invention will become clearer and more readily understood through the detailed description following the accompanying drawings. In the drawings:

[0073] Figure 1 This is a flowchart of the hovering method for a multi-rotor UAV according to the present invention;

[0074] Figure 2 This is a flowchart of step S1 in the hovering method of the multi-rotor UAV of the present invention;

[0075] Figure 3 This is a flowchart of step S2 in the hovering method of the multi-rotor UAV of the present invention;

[0076] Figure 4 This is a flowchart of step S3 in the hovering method of the multi-rotor UAV of the present invention. Detailed Implementation

[0077] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0078] The specific embodiments / examples described herein are specific implementations of the present invention, used to illustrate the concept of the invention, and are illustrative and exemplary, and should not be construed as limiting the implementation methods or scope of the present invention. In addition to the embodiments described herein, those skilled in the art can employ other obvious technical solutions based on the content disclosed in the claims and specification of this application. These technical solutions include those that make any obvious substitutions and modifications to the embodiments described herein, all of which are within the protection scope of the present invention.

[0079] This invention provides a hovering method for multi-rotor unmanned aerial vehicles (UAVs).

[0080] Please refer to Figure 1 As shown, Figure 1 This is a flowchart of the hovering method for multi-rotor UAVs according to the present invention.

[0081] The hovering method for the multi-rotor UAV includes the following steps:

[0082] Step S1: Obtain the attitude angle and altitude position of the multi-rotor UAV through multiple sensors of the multi-rotor UAV, and establish a dynamic model of the multi-rotor UAV based on the attitude angle and altitude position.

[0083] The sensors include accelerometers, gyroscopes, magnetometers, ultrasonic sensors, and barometers.

[0084] Please refer to Figure 2 As shown, Figure 2 This is a flowchart of step S1 in the multi-rotor UAV hovering method of the present invention. Specifically, step S1 further includes:

[0085] Step S11: Obtain multiple sensor data through the sensor.

[0086] Step S12: Calculate attitude information using the data obtained from the accelerometer, the gyroscope, and the magnetometer.

[0087] In step S12, the attitude information includes attitude angle x. acc and attitude angle y gyro And satisfy the following formula:

[0088]

[0089] Among them, u H The high-frequency noise introduced by the accelerometer, u L The low-frequency noise introduced by the gyroscope is denoted by x, which is a variable of the magnetometer, and y is another variable of the magnetometer.

[0090] Step S13: Calculate the altitude fusion estimate and optimal altitude value using the data obtained from the accelerometer, the ultrasonic waves, and the barometer.

[0091] In step S13, the data obtained by the accelerometer and the data obtained by the ultrasonic wave are fused together to determine the altitude, satisfying the following formula:

[0092] a zN =ga z ;

[0093] Among them, a zN The value of the Z-axis in the Earth coordinate system output by the accelerometer is given by g, where g is the acceleration due to gravity on Earth, and a is the acceleration due to gravity on Earth. z This represents the acceleration along the Z-axis of the Earth coordinate system, where the Z-axis is the direction of gravity.

[0094] The initial height is h0, and the height fusion estimate at any time is h0. ac It satisfies the following formula:

[0095]

[0096] v0 is the initial velocity of the multi-rotor UAV, T s The data obtained by the ultrasound;

[0097] The optimal height value is h. Considering the measurement range and accuracy of the ultrasonic wave, and the low accuracy and poor reliability of the barometer at low altitudes, an adaptive weighted filtering algorithm is used to comprehensively process the height estimates obtained from the dispersed fusion to obtain the optimal height value h, which satisfies the following formula:

[0098] h = k × h ac (1-k)h aq ;

[0099] haq The height fusion estimate is obtained by fusing the data obtained from the accelerometer and the barometer, where k is a variable parameter and h is the height fusion estimate. aq Obtained by implementing step S12.

[0100] In this embodiment, when the height h of the multi-rotor drone is lower than the range of ultrasonic waves, k takes a larger value; when the height of the multi-rotor drone is between 8 and 12 m, k takes a smaller value; and when the height is greater than 12 m, k takes a value of 0.

[0101] By implementing step S1, the problem of low measurement accuracy of the multi-rotor UAV at different altitudes is solved through the dynamic model. For the hovering altitude of the multi-rotor UAV, by selecting different altitude measurement sensors, not only is the measurement accuracy and real-time performance improved, but the redundancy of the altitude measurement system is also improved.

[0102] Step S2: Design an improved PID complementary filter algorithm; specifically, this involves using a PID controller to adjust the cutoff frequency of the multi-rotor UAV's filter, eliminate static error time, suppress data vibration, and enhance system speed. Specifically, this involves selecting a suitable K using a PID controller. p K i K d To adjust the filter of the multi-rotor drone. K p K is the proportional adjustment coefficient. p Used to accelerate the system's response speed and improve the system's adjustment accuracy. K i K is the integral adjustment coefficient. i Used to eliminate residuals. K d K is the differential adjustment coefficient. d Used to improve the dynamic performance of the system.

[0103] Please refer to Figure 3 As shown, Figure 3 This is a flowchart of step S2 in the multi-rotor UAV hovering method of the present invention. Specifically, step S2 includes:

[0104] Step S21: Based on the data obtained from the accelerometer, the data obtained from the magnetometer, the Earth's gravitational acceleration, and the error of the geomagnetic field strength, the gyroscope data is compensated using the PID improved complementary filtering algorithm.

[0105] In step S21, the error of the geomagnetic field strength is... Satisfy the following formula:

[0106]

[0107] for The value is obtained after normalization. Let a be the vector parameter of the accelerometer. x a y a z These represent the accelerations along the X-axis, Y-axis, and Z-axis, respectively, with T being a variable parameter.

[0108] Let b be the gravitational acceleration in the body coordinate system of the multi-rotor UAV;

[0109] for The value is obtained after normalization. Let m be the vector parameter of the magnetometer. x m y m z These represent the magnetic force values ​​along the X-axis, Y-axis, and Z-axis, respectively.

[0110] The geomagnetic intensity of the B series;

[0111] The gravitational accelerations in the Earth coordinate system N are [0 0 0] and [0 0 0], respectively. T With geomagnetic intensity [b x 0 z ] T After rotation matrix transformation, b is obtained. x Let b be the geomagnetic intensity along the X-axis. z The geomagnetic intensity is along the Z-axis, and the rotation transformation matrix is: Satisfy the following formula:

[0112]

[0113] b is a parameter, N is a parameter;

[0114] q0, q1, q2, q m These are the four component parameters of the quaternion.

[0115] Step S22: Update the quaternion using the data from the compensated gyroscope through the first-order Runge-Kutta equation.

[0116] In step S22, the output of the PID improved complementary filter algorithm is: Satisfy the following formula:

[0117]

[0118]

[0119]

[0120]

[0121] K p K is the proportional adjustment coefficient. i K is the integral adjustment coefficient, t is the time parameter, and K is the integral adjustment coefficient. d The differential adjustment coefficient;

[0122] This is the corrected angle value. The angle value obtained by the gyroscope;

[0123] q is the quaternion, The parameter variables of the quaternion;

[0124] ω x ω y ω z These are the angle values ​​in the X-axis direction, the Y-axis direction, and the Z-axis direction, respectively.

[0125] Step S2 is implemented by employing the improved PID complementary filtering algorithm for the multi-attitude data fusion process of the control system of the multi-rotor UAV in practical applications. By using the PID controller to select appropriate parameters to adjust the cutoff frequency, the measurement data obtained from the accelerometer and magnetometer of the multi-rotor UAV are used to compensate for the errors in gravitational acceleration and geomagnetic field strength, thereby comprehensively considering the static and dynamic errors of the multi-rotor UAV, suppressing the vibration of the attitude data, and enhancing the speed and stability of the hovering system of the multi-rotor UAV.

[0126] Step S3: Design of Extended Kalman Filter Algorithm based on Quaternions; specifically, using the quaternions obtained in step S2 as input data, combined with the dynamic model established in step S1, the Extended Kalman Filter algorithm is used for analysis and design to obtain the estimated attitude parameters and vertical acceleration, velocity, or position parameters of the multi-rotor UAV in each update cycle.

[0127] Please refer to Figure 4 As shown, Figure 4 This is a flowchart of step S2 in the multi-rotor UAV hovering method of the present invention. Specifically, step S3 includes:

[0128] Step S31: Establish the nonlinear model of the extended Kalman filter algorithm.

[0129] In step S31, the nonlinear model includes state equations and measurement equations.

[0130] The state equation is: X k =f(X) k-1 W k-1 );

[0131] The measurement equation is: Z k =h(X) k V k );

[0132] Among them, X k Let f() be the state estimation vector of the system at time k, and let X be the state estimation vector of the system at time k. k Nonlinear function, X k-1 W is the state estimation vector of the system at time k-1. k-1 W represents the process noise of the system at time k-1. k The process noise at time k is the system noise.

[0133] Z k Let h() be the measurement vector of the system at time k, and V be the observation equation. k For measuring noise; process noise W k With measurement noise V k Unrelated. Process noise W k With measurement noise V k Satisfy the following formula:

[0134]

[0135] Where X0 is the state estimation vector of the system at the initial moment, and E[] is the correlation function equation.

[0136] Step S32: Linearize the function of the nonlinear model at the state point by Taylor series expansion to realize the time update of the system.

[0137] In step S32

[0138] The function of the nonlinear model at the state point satisfies the following formula:

[0139]

[0140]

[0141]

[0142] in, For state The state estimate after filtering using the extended Kalman filter algorithm, where f() is the filtering function;

[0143] Let P be the covariance matrix at time k.k-1 Let Q be the covariance matrix at time k. k It is the variance matrix of the system noise. The system state equations X are respectively k The variables of the Jacobian matrix; According to The measurement matrix obtained at time k;

[0144] The time update of the system satisfies the following formula:

[0145]

[0146]

[0147]

[0148] Among them, K k The extended Kalman gain at time k, According to and The state estimate at time k after the update at time k-1, where I is the parameter variable and R is the state estimate at time k-1. k To measure the variance matrix of the noise, Let be the covariance matrix at time k. The system measurement equations Z are respectively k The variables of the Jacobian matrix.

[0149] Implementing step S3, using the extended Kalman filter algorithm in conjunction with adaptive altitude fusion estimation, not only improves the stability of the multi-rotor UAV, but also optimizes the system model. This solves the problems of the multi-rotor UAV having a large amount of matrix operations and computational load when using Kalman filtering alone for hovering, which prevents the multi-rotor UAV from hovering stably under wind interference and has limited hovering altitude.

[0150] Step S4: Input the data analyzed and designed by the extended Kalman filter algorithm in step S3 into the power system of the multi-rotor UAV to achieve hovering control of the multi-rotor UAV. The hovering control is specifically as follows: when the multi-rotor UAV receives a hovering command from the outside, if the multi-rotor UAV deviates due to external factors, the acceleration value measured by the accelerometer of the multi-rotor UAV is integrated to calculate the displacement. Based on the displacement, the power system is controlled to move the same amount of displacement in the opposite direction of the displacement, so as to achieve rapid correction of the multi-rotor UAV during the hovering process and return to the initial hovering position.

[0151] The external factors mentioned include wind.

[0152] By implementing step S4, the multi-rotor UAV can be quickly corrected during hovering and returned to its initial hovering position.

[0153] In summary, implementing steps S1 to S4 makes the multi-rotor UAV hovering method of the present invention more practical. The multi-rotor UAV hovering method of the present invention has a wider range of applications, solving the problem that optical flow positioning hovering methods in related technologies cannot accurately hover in complex planar environments. Furthermore, it reduces the number of components involved in the multi-rotor UAV, saving costs and making it more practical.

[0154] This also enhances the anti-interference performance of the multi-rotor UAV hovering method of the present invention. The multi-rotor UAV hovering method of the present invention utilizes the gyroscope, magnetometer, and accelerometer working in concert to solve problems such as drift and strong magnetic interference that occur when the multi-rotor UAV hovers outdoors or indoors with weak GPS signals. The collaboration of multiple sensors enables real-time estimation and compensation of disturbances, significantly enhancing the system's anti-interference capability.

[0155] This also results in higher control accuracy for the multi-rotor UAV hovering method of the present invention. The multi-rotor UAV hovering method of the present invention uses adaptively fused altitude data as input and, through the collaborative work of multiple sensors, reduces the impact of instability in model parameters on the controller, thereby improving control accuracy.

[0156] This also makes the parameters of the multi-rotor UAV hovering method of the present invention easier to tune. The multi-rotor UAV hovering method of the present invention uses the altitude and attitude angle obtained by adaptive altitude fusion and PID improved complementary filtering algorithm, and then filters the data obtained by the extended Kalman filter algorithm as input, which improves the disturbance estimation accuracy of the system, simplifies the structure of the self-disturbance controller, and reduces the difficulty of parameter tuning.

[0157] This also reduces the computational load of the multi-rotor UAV hovering method of the present invention. The multi-rotor UAV hovering method of the present invention uses the extended Kalman filter algorithm, employing first- or second-order Taylor series expansion and ignoring small terms to reduce the computational load, thereby improving the response speed of the multi-rotor UAV control system. Compared to related technologies such as the fuzzy adaptive cascade PID control method (which has a high computational load), the sliding mode control hovering method (which has a difficult-to-determine system model), the neural network training hovering method (which requires extensive training of the multi-rotor UAV system), and the optical flow positioning hovering method (which has a limited hovering area), the multi-rotor UAV hovering method of the present invention has the advantages of easier system model establishment, lower computational load, and faster response speed.

[0158] Compared with existing technologies, the multi-rotor UAV hovering method of the present invention obtains the attitude angle and altitude position of the multi-rotor UAV through multiple sensors of the multi-rotor UAV in step S1, and establishes a dynamic model of the multi-rotor UAV based on the attitude angle and altitude position. Implementing step S1, the dynamic model solves the problem of low measurement accuracy of the multi-rotor UAV at different altitudes. For the hovering altitude of the multi-rotor UAV, different altitude measurement sensors are selected, which not only improves measurement accuracy and real-time performance, but also improves the redundancy of the altitude measurement system. In step S2, an improved PID complementary filtering algorithm is designed. Implementing step S2, the improved PID complementary filtering algorithm is used for the multi-attitude data fusion process of the control system of the multi-rotor UAV in practical applications. By using the PID controller to select appropriate parameters to adjust the cutoff frequency, the error between the measurement data obtained from the accelerometer and magnetometer of the multi-rotor UAV and the gravitational acceleration and geomagnetic field strength is used to compensate for the gyroscope attitude data. This comprehensively considers the static and dynamic errors of the multi-rotor UAV, suppresses the vibration of the attitude data, and enhances the speed and stability of the hovering system of the multi-rotor UAV. Step S3: Design of an Extended Kalman Filter Algorithm Based on Quaternions; Implementing Step S3, the use of the Extended Kalman Filter algorithm in conjunction with adaptive altitude fusion estimation not only improves the stability of the multi-rotor UAV but also optimizes the system model. This solves the problems of high matrix computation and computational complexity when using Kalman filtering alone for hovering, which prevents the multi-rotor UAV from achieving stable hovering under wind interference and limiting hovering altitude. Step S4: Input the data analyzed and designed by the Extended Kalman Filter algorithm in Step S3 into the power system of the multi-rotor UAV to achieve hovering control. Implementing Step S4 allows for rapid correction during the hovering process, returning the multi-rotor UAV to its initial hovering position.

[0159] The above description is only a preferred embodiment of the present invention. For those skilled in the art, there will be changes in the specific implementation and application scope based on the ideas of the present invention. The content of this specification should not be construed as a limitation of the present invention.

Claims

1. A multi-copter unmanned aerial vehicle hovering method, characterized in that, The method includes the following steps: Step S1: Obtain the attitude angle and altitude position of the multi-rotor UAV using multiple sensors, and establish a dynamic model of the multi-rotor UAV based on the attitude angle and altitude position; the sensors include accelerometers, gyroscopes, magnetometers, ultrasonic sensors, and barometers; Step S2: Design an improved PID complementary filter algorithm; specifically, by using a PID controller to adjust the cutoff frequency of the filter of the multi-rotor UAV, eliminate static error time, suppress data vibration, and enhance the system's speed. Step S3: Design of Extended Kalman Filter Algorithm based on Quaternions; Specifically, using the quaternions obtained in Step S2 as input data, combined with the dynamic model established in Step S1, the Extended Kalman Filter algorithm is used for analysis and design to obtain the estimated attitude parameters and vertical acceleration, velocity, or position parameters of the multi-rotor UAV in each update cycle. Step S4: Input the data analyzed and designed by the extended Kalman filter algorithm in step S3 into the power system of the multi-rotor UAV to achieve hovering control of the multi-rotor UAV; the hovering control is specifically as follows: when the multi-rotor UAV receives a hovering command from the outside, if the multi-rotor UAV deviates due to external factors, the acceleration value measured by the accelerometer of the multi-rotor UAV is integrated to calculate the displacement, and the power system is controlled to move the same amount of displacement in the opposite direction of the displacement according to the displacement, so as to realize the rapid correction of the multi-rotor UAV during the hovering process and return to the initial hovering position; Step S2 includes: Step S21: Based on the data obtained from the accelerometer, the data obtained from the magnetometer, and the errors in the Earth's gravitational acceleration and geomagnetic field strength, the gyroscope data is compensated using the PID improved complementary filtering algorithm; Step S22: Update the quaternion using the data from the compensated gyroscope through the first-order Runge-Kutta equation; In the step S21, the error of the geomagnetic field intensity satisfies the following equation: for The value is obtained after normalization. The vector parameters of the accelerometer, These represent the accelerations along the X, Y, and Z axes, respectively. g is the gravitational acceleration in the body coordinate system b of the multicopter drone; for The value is obtained after normalization. The vector parameters of the magnetometer are... These represent the magnetic force values ​​along the X, Y, and Z axes, respectively. B is the geomagnetic intensity; The gravitational accelerations in the Earth coordinate system N are respectively. With geomagnetic intensity Obtained through rotation matrix transformation , Here, represents the geomagnetic intensity along the X and Z axes, respectively, and the rotation matrix is... It satisfies the following formula: ； b is a parameter, N is a parameter; respectively the four component parameters of the quaternion; In step S22, the output of the PID improved complementary filter algorithm is: It satisfies the following formula: ； ； ； ； Kp is a proportional regulation coefficient, Ki is an integral regulation coefficient, Kd is a derivative regulation coefficient; for the corrected angle value, for the angle value obtained by the gyroscope; q is the quaternion, is a parameter variable of the quaternion; Angle values in the X-axis, Y-axis, and Z-axis directions, respectively.

2. The hovering method for a multi-rotor UAV according to claim 1, characterized in that, Step S1 includes: Step S11: Obtain multiple sensor data through the sensor; Step S12: Calculate attitude information using the data obtained from the accelerometer, the gyroscope, and the magnetometer; Step S13: Calculate the altitude fusion estimate and optimal altitude value using the data obtained from the accelerometer, the ultrasonic waves, and the barometer.

3. The hovering method for a multi-rotor UAV according to claim 2, characterized in that, The attitude information in the step S12 includes an attitude angle and the attitude angle and satisfies the following equation: ； wherein a high frequency noise introduced for the accelerometer, a low frequency noise introduced for the gyroscope, a variable for the magnetometer, another variable for the magnetometer; In step S13, the data obtained by the accelerometer and the data obtained by the ultrasonic wave are fused together to determine the altitude, satisfying the following formula: ； in, The value of the Z-axis in the Earth coordinate system output by the accelerometer. ， For Earth's gravitational acceleration, This represents the acceleration along the Z-axis of the Earth coordinate system, where the Z-axis is the direction of gravity. The initial height value is The height fusion estimate at any time satisfies the following equation: ； an initial velocity of the multi-copter drone, data obtained from the ultrasound waves; The optimal height value is satisfying the following equation: ； fusing the data obtained by the accelerometer and the data obtained by the barometer to obtain a fused altitude estimate, variable parameters.

4. The multi-copter unmanned aerial vehicle hovering method of claim 3, wherein, Step S3 includes: Step S31: Establish the nonlinear model of the extended Kalman filter algorithm; Step S32: Linearize the function of the nonlinear model at the state point by Taylor series expansion to realize the time update of the system.

5. The multi-copter unmanned aerial vehicle hovering method of claim 4, wherein, In step S31, the nonlinear model includes state equations and measurement equations. The equation of state is: ​ The measurement equation is: ​ in, Let k be the state estimation vector of the system at time k. for Nonlinear functions This is the state estimation vector of the system at time k-1. The process noise at time k-1 of the system, The process noise at time k is the system noise. is the measurement vector at time k for system k, is the observation equation, is the measurement noise; process noise is uncorrelated; In step S32 The function of the nonlinear model at the state point satisfies the following formula: ； ； ； wherein, is a state performing the extended Kalman filter algorithm to filter the state estimate, is a filter function; is the covariance matrix at time k, is the covariance matrix at time k, is the variance matrix of the system noise, is the Jacobian matrix of the system state equation respectively; and is the measurement matrix at time k obtained from ​ The time update of the system satisfies the following formula: ； ； ； in, The extended Kalman gain at time k, According to and The state estimate at time k after updating at time k-1. Let be the covariance matrix at time k. , The system measurement equations are respectively The variables of the Jacobian matrix.

6. The hovering method for a multi-rotor unmanned aerial vehicle according to claim 1, characterized in that, The external factors mentioned include wind.

Images