A spraying unmanned aerial vehicle stability control method and system

Abstract

The present invention discloses a spraying UAV stability control method and system thereof, including: spraying UAV attitude acquisition and solution: establishing a spraying UAV dynamics model based on Newton-Euler equations in an airborne NED coordinate system; establishing a pipeline tension model based on a hyperbolic cosine function; flight parameter optimization; passing the aircraft control quantity parameters with the best implementation effect to each component of the UAV, including an IMU UAV attitude monitor with a three-axis gyroscope, a three-axis accelerometer, a three-axis geomagnetic sensor and a barometer to perform multi-sensor data acquisition and fusion, and stably track the expected attitude signal; designing an RTK positioning technology expansion detector to detect whether the flight trajectory of the UAV coincides with the predetermined trajectory; including a feedback system with a single-chip microcomputer to transmit interference signals to the control system in real time and design dynamic error feedback control. The present invention can effectively realize the stability control of the spraying UAV.

Description

Technical Field

[0001] The invention belongs to the technical field of stability control of a spraying unmanned aerial vehicle, and relates to a stability control method and a system of a spraying unmanned aerial vehicle. Background Art

[0002] In recent years, with the rapid development of the manufacturing industry, the requirements for the spraying field have become increasingly higher. The current shipbuilding and other operations that require a lot of manual painting can no longer meet their green and efficient painting needs.

[0003] Invention patent application CN201910149361.5 discloses a drone control method, a vehicle-mounted terminal and a computer device. Among them, the drone control method includes: establishing a connection with the drone by means of wireless communication. After receiving the take-off control command for the drone, determine whether the take-off conditions are met based on the current take-off environment information obtained. The current take-off environment information includes at least one of the speed information of the vehicle carrying the drone and the position information of the vehicle. If the current take-off environment information does not meet the take-off conditions, the corresponding take-off prompt information is output. If the current take-off environment information meets the take-off conditions, a take-off control command is sent to the drone to control the drone to take off. However, this invention cannot provide a drone control method that can realize intelligent take-off control of the drone carried on the vehicle, and cannot meet the development requirements of intelligent control of drones.

[0004] Invention patent CN201610128561.9 discloses a drone control method, drone, ground station and drone system. It includes: first, the drone collects the original panoramic image of the surrounding environment and the original target image of the tracking target; then converts it into a panoramic image and a target image with a frame rate interval of a preset value; and generates a composite image after marking them as odd frames and even frames respectively; the drone then sends the composite image to the ground station; finally, the ground station obtains the control mode of the drone; when the control mode is the body motion control mode, the ground station displays the odd frames in the composite image; when the control mode is the camera control mode, the ground station displays the even frames in the composite image. However, this invention cannot guarantee the reliability of drone control through the drone control method, drone, ground station and drone system, and also increases the cost of the drone transmission device. Summary of the invention

[0005] The purpose of the present invention is to overcome the defects of the prior art and provide a spray UAV stability control method and system. In view of the fact that the UAV is affected by factors such as wind force and spraying working status during operation and its load is constantly changing, intelligent real-time control can be achieved to effectively solve the problem of UAV stability control.

[0006] In order to solve the above technical problems, the following technical solutions are adopted.

[0007] A spraying UAV stability control method of the present invention comprises the following steps:

[0008] Step 1. Acquisition and solution of the spraying UAV attitude: Acquire attitude data and perform data fusion obtained by multiple sensors, including decision information fusion and real-time situation prediction, and use attitude algorithm to perform dynamic step length optimization estimation for the UAV;

[0009] Step 2. Establishment of the dynamic model of the spraying UAV: ​​Establish an airborne NED coordinate system based on the flight of the UAV, introduce the rotation matrix, Euler angle, and quaternion; establish the dynamic model of the spraying UAV based on the Newton-Euler equation in the coordinate system;

[0010] Step 3. Establish a pipeline tension model based on the hyperbolic cosine function;

[0011] Step 4. Flight parameter optimization: According to the characteristics of the UAV control system, a model predictive controller is constructed, and the yaw angle η, roll angle σ and flight altitude H of the spraying UAV are used as the flight control parameters, and the initialization parameters are processed to establish the optimization objective function of the model predictive control parameters; the genetic algorithm is used to solve the optimization objective function, and the control parameters are continuously updated to obtain the optimal value of the flight state assessment equation, and the aircraft control parameters with the best implementation effect are obtained;

[0012] Step 5. The aircraft control parameters with the best implementation effect are passed to the various components of the UAV, including the IMU UAV attitude monitor with a three-axis gyroscope, a three-axis accelerometer, a three-axis geomagnetic sensor and a barometer to obtain and fuse multi-sensor data, including its three attitude angles, namely pitch, roll and yaw, and stably track the expected attitude signal; design an RTK positioning technology expansion detector to detect whether the flight trajectory of the UAV coincides with the predetermined trajectory; include a feedback system with a single-chip microcomputer to transmit interference signals to the control system in real time and design dynamic error feedback control.

[0013] Furthermore, in step 2, the dynamic model of the spraying UAV based on the Newton-Euler equation established in the airborne NED coordinate system based on the UAV flight is:

[0014]

[0015] in, is the acceleration in the x direction, is the acceleration in the y direction, is the acceleration in the z direction, where U1, U2, U3, U4 and Ω are:

[0016]

[0017] Among them, U1, U2, U3, and U4 are the vertical, roll, pitch, and yaw control quantities, respectively. x , I y , I z are the rotational inertia of the body when rotating around the x, y, and z axes, Jr is the rotational inertia of the propeller rotor around the motor shaft; L is the distance from the center of the propeller rotor to the center of the body; m is the mass of the body; g is the acceleration of gravity; φ, θ, ψ are the roll angle, pitch angle, and yaw angle of the body respectively; are the roll angular velocity, pitch angular velocity and yaw angular velocity of the aircraft respectively; are the roll angular velocity, pitch angular velocity and yaw angular velocity of the aircraft respectively; b and d are the lift coefficient and drag coefficient of the propeller rotor respectively; Ωi is the rotational speed of each propeller rotor.

[0018] Furthermore, the process of establishing the pipeline tension model in step 3 includes:

[0019] Establish a balance equation between the sum of the lift force generated by the rotors of the drone, the pulling force of the pipeline on the drone, and the gravity of the drone itself:

[0020]

[0021] In the formula, F represents the sum of the upward lift generated by the rotation of each rotor, T represents the pull of the pipeline on the fuselage, m is the mass of the spraying drone, g is the acceleration of gravity, α is the angle between the total lift F and the horizontal direction, and β is the pitch angle. The pipeline tension model based on the hyperbolic cosine function is applied to the pipeline tension analysis of the spraying drone, and the component force matrix of the pipeline tension is obtained:

[0022]

[0023] Where, T represents the pipeline tension, T x , T y , T z It represents the component forces of tension T on the three coordinate axes, γ1 is the angle between the projection of tension T on plane O1X1Y1 and axis X1, and γ2 is the angle between tension T and plane O1X1Y1.

[0024] Furthermore, the optimization objective function of the model predictive control parameters described in step 4 is:

[0025]

[0026] Where t is time, e(t) is defined as system error, u(t) is defined as output, w1, w2, w3 are weight values, and the sum is 1; t u Used to adjust the time.

[0027] Specifically, the value of the objective function is at least not less than 0; and the larger the value is, the closer it is to the optimal solution.

[0028] A spraying drone stability control system of the present invention comprises a controller and a position sensor, an attitude sensor, an altitude sensor, a weight sensor and a motor control module of each propeller rotor that are electrically or communicatively connected to the controller; the controller adopts a single-chip microcomputer, and a computer program is stored in its storage medium, and the computer program is designed to implement the spraying drone stability control method when executed. The position sensor, attitude sensor, altitude sensor and weight sensor respectively adopt: RTK positioning system, acceleration sensor and barometer.

[0029] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0030] 1. The present invention fully considers the influence of the spray gun and pipeline, wind force, load changes and spraying working state of the spraying drone, obtains attitude data through different sensors, performs data fusion obtained by multiple sensors, and uses attitude algorithm to perform dynamic step optimization estimation on the spraying drone, enhances the dynamic performance of the drone aircraft attitude solution, and improves the accuracy of drone attitude acquisition. The attitude adjustment of the drone during the spraying process is more accurate and the response speed is faster, ensuring the stability of the drone during the spraying process and the spraying quality.

[0031] 2. Based on the airborne NED coordinate system of the UAV flight, the rotation matrix, Euler angles, and quaternions are introduced, and a dynamic model based on the Newton-Euler equations is established in the coordinate system, that is, the mathematical model of the force on the UAV.

[0032] 3. Based on the hyperbolic cosine function, the pipeline is solved by applying the hyperbolic cosine function. The expansion of its specific solution can be obtained through small parameter perturbation, and then the general solution is obtained by integration. The balance equation is listed for the sum of the lift forces generated by the rotors of the drone body, the pull of the pipeline on the drone, and the gravity of the drone itself. According to the pipeline tension model, the final component matrix of the pipeline tension is listed, and the component forces of the pipeline tension coordinate axis in the x-axis, y-axis, and z-axis directions are obtained, and the force model of the pipeline is obtained.

[0033] 4. Use genetic algorithm to solve the optimization objective function, further obtain the optimal value of the flight state assessment equation, and obtain the aircraft control parameters with the best implementation effect.

[0034] 5. After the optimized control data is passed to each component, the IMU drone attitude monitor composed of a three-axis gyroscope, a three-axis accelerometer, a three-axis geomagnetic sensor and a barometer acquires and fuses multi-sensor data, including its three attitude angles (pitch, roll, and yaw) and stably tracks the expected attitude signal. Design an RTK positioning technology extension detector to detect whether the flight trajectory of the drone coincides with the predetermined trajectory. The feedback system is composed of a single-chip microcomputer as the main component, which transmits the interference signal to the control system in real time and designs dynamic error feedback control to achieve the desired control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 The present invention is a method flow chart of an embodiment of the present invention.

[0036] Figure 2 A schematic diagram of an onboard NED coordinate system for UAV flight according to an embodiment of the present invention.

[0037] Figure 3 This is an overall force distribution diagram of a spraying drone according to an embodiment of the present invention.

[0038] Figure 4 This is a pipeline tension catenary force analysis diagram of an embodiment of the present invention. DETAILED DESCRIPTION

[0039] The present invention discloses a spraying UAV stability control method and system, which is designed to be an intelligent real-time control method for the situation that the load of the UAV is constantly changing due to the influence of the spray gun and pipeline, wind force and spraying working state during flight. It includes: solving the flight attitude data of the spraying UAV to obtain an accurate flight attitude model; establishing an airborne NED coordinate system for the UAV flight, establishing a dynamic model based on the Newton-Euler equation and a pipeline tension model based on the hyperbolic cosine function, and realizing the mathematical separation of the variable load coupling problem; after dynamically optimizing the flight parameters using a genetic algorithm, controlling the data transmission through a data transmission chain composed of an inertial measurement unit IMU and RTK positioning technology, realizing intelligent stability control of the UAV spraying, and improving the flight performance, anti-interference and reliability of the UAV.

[0040] The present invention is further described in detail below with reference to the accompanying drawings.

[0041] like Figure 1 As shown, a spraying UAV stability control method of the present invention comprises the following steps:

[0042] Step 1. Attitude acquisition and solution: Acquire attitude data and perform data fusion from multiple sensors, including decision information fusion, real-time situation prediction, etc. Use attitude algorithm to perform dynamic step length optimization estimation for the UAV.

[0043] Step 2. Establishment of UAV dynamics model: Establish the airborne NED coordinate system based on the UAV flight, and introduce the rotation matrix, Euler angle, and quaternion. The schematic diagram of the coordinate system is as follows Figure 2 . A dynamic model based on the Newton-Euler equation is established in the coordinate system.

[0044] Step 3. Establishment of pipeline tension model: Pipeline tension model based on hyperbolic cosine function.

[0045] Step 4. Flight parameter optimization: According to the characteristics of the UAV control system, a model predictive controller is constructed. The yaw angle η, roll angle σ and flight altitude H of the spraying UAV are used as flight control parameters, and the initialization parameters are processed to establish the optimization objective function of the model predictive control parameters; the genetic algorithm is used to solve the optimization objective function, and the control parameters are continuously updated to obtain the optimal value of the flight state assessment equation, thereby obtaining the aircraft control parameters with the best implementation effect.

[0046] Step 5. After the best aircraft control parameters are passed to the various components of the drone, the IMU drone attitude monitor, which includes a three-axis gyroscope, a three-axis accelerometer, a three-axis geomagnetic sensor and a barometer, acquires and fuses multi-sensor data, including its three attitude angles (pitch, roll, and yaw) and stably tracks the expected attitude signal. Design an RTK positioning technology extension detector to detect whether the flight trajectory of the drone coincides with the predetermined trajectory. The feedback system is composed of a single-chip microcomputer as the main component, which transmits interference signals to the control system in real time and designs dynamic error feedback control.

[0047] The step 1 specifically comprises the following steps:

[0048] The accelerometer, ultrasonic sensor and gyroscope sensor acquire attitude data. The accelerometer acquires acceleration information, the ultrasonic sensor acquires the distance from the drone to the ground, and the gyroscope sensor detects the angle change of the drone. The multi-sensor data fusion is performed, including decision information fusion, real-time situation prediction, etc. The attitude algorithm is used to perform dynamic step optimization estimation on the drone, enhance the dynamic performance of the drone aircraft attitude solution, and thus realize the acquisition and solution of the drone's popular attitude.

[0049] The step 2 specifically comprises the following steps:

[0050] Introduce rotation matrix, Euler angle, and quaternion to establish an airborne NED coordinate system based on UAV flight. The schematic diagram of the coordinate system is as follows Figure 2 Taking the six-rotor UAV as an example, in the established coordinate system, the dynamic model equation of the six-rotor UAV based on the Newton-Euler equation is:

[0051]

[0052] is the acceleration in the x direction, is the acceleration in the y direction, is the acceleration in the z direction, where U1, U2, U3, U4 and Ω are:

[0053]

[0054] Among them, U1, U2, U3, and U4 are the vertical, roll, pitch, and yaw control quantities, respectively. x , I y , I z are the rotational inertia of the body when rotating around the x, y, and z axes, Jr is the rotational inertia of the propeller rotor around the motor shaft; L is the distance from the center of the propeller rotor to the center of the body; m is the mass of the body; g is the acceleration of gravity; φ, θ, ψ are the roll angle, pitch angle, and yaw angle of the body respectively; are the roll angular velocity, pitch angular velocity and yaw angular velocity of the aircraft respectively; are the roll angular velocity, pitch angular velocity and yaw angular velocity of the aircraft respectively; b and d are the lift coefficient and drag coefficient of the propeller rotor respectively; Ωi is the rotational speed of each propeller rotor.

[0055] The step 3 specifically comprises the following steps:

[0056] When the UAV flies in a straight line at a constant speed, it presents a stable flight state as a whole. The overall force analysis of the UAV and pipeline is as follows: Figure 3 As shown, according to Figure 3 Establish a balance equation between the sum of the lift force generated by the rotors of the drone, the pulling force of the pipeline on the drone, and the gravity of the drone itself:

[0057]

[0058] In the formula, F represents the sum of the upward lift generated by the rotation of each rotor, T represents the pulling force of the pipeline on the body, m is the mass of the spraying drone, g is the acceleration of gravity, α is the angle between the total lift F and the horizontal direction, and β is the pitch angle. The pipeline tension model based on the hyperbolic cosine function is applied to the pipeline tension analysis of the spraying drone, such as Figure 4 As shown, the component force matrix of pipeline tension is obtained:

[0059]

[0060] Where, T represents the pipeline tension, T x , T y , T zIt represents the component forces of tension T on the three coordinate axes, γ1 is the angle between the projection of tension T on plane O1X1Y1 and axis X1, and γ2 is the angle between tension T and plane O1X1Y1.

[0061] Then, the components of the pipeline tension coordinate axis in the x-axis, y-axis, and z-axis directions are finally obtained.

[0062] The step 4 specifically comprises the following steps:

[0063] According to the characteristics of the UAV control system, a model predictive controller is constructed to determine the control parameters that need to be optimized, namely the yaw angle η, roll angle σ and flight altitude H of the spraying UAV, and perform initialization parameter processing. The optimization objective function of the model predictive control parameters is established

[0064]

[0065] Where t is time, e(t) is defined as system error, u(t) is defined as output, w1, w2, w3 are weight values, and the sum is 1; t u Used to adjust the time. In addition, the value of the objective function is greater than or equal to 0, and the larger the calculated value, the closer the algorithm is to the optimal solution.

[0066] Genetic algorithm is used to solve the optimization objective function, further obtain the optimal value of the flight state assessment equation, and obtain the aircraft control parameters with the best implementation effect.

[0067] The step 5 specifically comprises the following steps:

[0068] After the optimized control data is passed to each component, the inertial measurement unit composed of a three-axis gyroscope, a three-axis accelerometer, a three-axis geomagnetic sensor and a barometer monitors the three attitude angles (pitch, roll and yaw) of the six-rotor drone during flight, stably tracks the expected attitude signals, and compiles these attitude signals into electronic signals through the data link and transmits them to the control system.

[0069] RTK (Real-time kinematic) positioning technology is used to determine the correct course of the six-rotor drone. During the operation of the drone, RTK continuously monitors whether the flight trajectory of the drone coincides with the predetermined trajectory. If there is a deviation, the deviation distance is converted into an electrical signal and transmitted to the control system.

[0070] The single-chip microcomputer in the control system is responsible for calculating the compensation angle and then compiling the compensation data into an electronic signal, which is transmitted to the drone's servo or motor. The motor or servo executes the command and completes the compensation action. After the IMU senses that the aircraft's attitude is stable again, it sends the real-time data to the single-chip microcomputer again, and the single-chip microcomputer stops the compensation signal. The drone flight control system forms a 10HZ internal loop, and the high-frequency adjustment and power distribution enable the drone to automatically correct its heading and attitude when encountering interference during flight.

[0071] A spraying drone stability control system according to an embodiment of the present invention can implement the above-mentioned spraying drone stability control method, including a controller and a position sensor, an attitude sensor, an altitude sensor, a weight sensor and a motor control module of each propeller rotor electrically connected to the controller. The controller adopts a single-chip microcomputer, and a computer program is stored in its storage medium. The computer program is designed to implement the above-mentioned spraying drone stability control method when executed. The spraying drone stability control system according to an embodiment of the present invention can control the spraying drone to remain stable during flight, effectively improving the spraying quality.

Claims

1. A spraying UAV stability control method, characterized in that: The following steps are involved: Step 1. Acquisition and solution of the spraying UAV attitude: Acquire attitude data and perform data fusion obtained by multiple sensors, including decision information fusion and real-time situation prediction, and use attitude algorithm to perform dynamic step length optimization estimation for the UAV; Step 2. Establishment of the dynamic model of the spraying UAV: ​​Establish an airborne NED coordinate system based on the flight of the UAV, introduce the rotation matrix, Euler angle, and quaternion; establish the dynamic model of the spraying UAV based on the Newton-Euler equation in the coordinate system; Step 3. Establish a pipeline tension model based on the hyperbolic cosine function; Step 4. Flight parameter optimization: construct a model predictive controller according to the characteristics of the UAV control system, take the yaw angle ψ, roll angle φ and flight altitude H of the spraying UAV as the flight control parameters, perform initialization parameter processing, and establish the optimization objective function of the model predictive control parameters; use the genetic algorithm to solve the optimization objective function, continuously update the control parameters, obtain the optimal value of the flight state assessment equation, and obtain the aircraft control parameters with the best implementation effect; Step 5. The aircraft control parameters with the best implementation effect are transmitted to various components of the UAV, including the IMU UAV attitude monitor with a three-axis gyroscope, a three-axis accelerometer, a three-axis geomagnetic sensor and a barometer to acquire and fuse multi-sensor data, including its three attitude angles, namely the pitch angle, the roll angle and the yaw angle, and stably track the expected attitude signal; design an RTK positioning technology extension detector to detect whether the flight trajectory of the UAV coincides with the predetermined trajectory; Including a feedback system with a single-chip microcomputer, which transmits interference signals to the control system in real time and designs dynamic error feedback control; The process of establishing the pipeline tension model described in step 3 includes: Establish a balance equation between the sum of the lift force generated by the rotors of the drone, the pulling force of the pipeline on the drone, and the gravity of the drone itself: In the formula, F represents the sum of the upward lift generated by the rotation of each rotor, T represents the pull of the pipeline on the fuselage, m is the mass of the spraying drone, g is the acceleration of gravity, α is the angle between the total lift F and the horizontal direction, and θ is the pitch angle. The pipeline tension model based on the hyperbolic cosine function is applied to the pipeline tension analysis of the spraying drone, and the component force matrix of the pipeline tension is obtained: Where, T represents the pipeline tension, T x , T y , T z represents the components of the tension T on the three coordinate axes, γ1 is the angle between the projection of the tension T on the plane O1X1Y1 and the axis X1, and γ2 is the angle between the tension T and the plane O1X1Y1; The optimization objective function of the model predictive control parameters described in step 4 is: Where t is time, e(t) is defined as system error, u(t) is defined as output, w1, w2, w3 are weight values, and the sum is 1; t u Used to adjust the time.

2. A spraying UAV stability control method according to claim 1, characterized in that: In step 2, the dynamic model of the spraying UAV based on the Newton-Euler equation established in the airborne NED coordinate system based on the UAV flight is: in, is the acceleration in the x direction, is the acceleration in the y direction, is the acceleration in the z direction, where U1, U2, U3, U4 and Ω are: Among them, U1, U2, U3, and U4 are the vertical, roll, pitch, and yaw control quantities, respectively. x , I y , I z are the moments of inertia of the body when it rotates around the x, y, and z axes, respectively, r is the moment of inertia of the propeller rotor around the motor shaft; L is the distance from the center of the propeller rotor to the center of the fuselage; m is the mass of the fuselage; g is the acceleration of gravity; φ, θ, ψ are the roll angle, pitch angle and yaw angle of the fuselage respectively; are the roll angular velocity, pitch angular velocity and yaw angular velocity of the aircraft respectively; are the roll acceleration, pitch acceleration and yaw acceleration of the aircraft respectively; b and d are the lift coefficient and drag coefficient of the propeller rotor respectively; Ω i is the rotation speed of each propeller blade.

3. A spraying UAV stability control method according to claim 1, characterized in that: The value of the objective function is at least not less than 0; and the larger the value is, the closer it is to the optimal solution.

4. A spraying drone stability control system, characterized in that: It includes a controller and a position sensor, an attitude sensor, an altitude sensor, a weight sensor and a motor control module of each propeller rotor electrically or communicatively connected to the controller; the controller adopts a single-chip microcomputer, and a computer program is stored in its storage medium. The computer program is designed to implement the spraying UAV stability control method as described in any one of claims 1 to 3 when executed.

5. A spraying UAV stability control system according to claim 4, characterized in that: The position sensor, attitude sensor, height sensor and weight sensor respectively adopt: RTK positioning system, acceleration sensor and barometer.

Images