Holder control method and device for aerial spraying of unmanned aerial vehicle

Abstract

The invention discloses a holder control method and device for aerial spraying of an unmanned aerial vehicle. A nozzle and an attitude sensor are arranged on a holder, and the holder is controlled to rotate through a three-axis motor; the method comprises the following steps: collecting an Euler angle signal of a nozzle through an attitude sensor; inputting the Euler angle signal to a tracking differentiator for signal tracking and differential processing, and outputting an angle tracking differential signal; the real-time speed of the three-axis motor is detected; the linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal; calculating a system error according to the angle tracking differential signal and the speed observation differential signal, and inputting the system error to a linear error compensation controller; the linear error compensator calculates and outputs a speed target value of the three-axis motor according to the system error and the total disturbance signal; the rotating speed of the three-axis motor is controlled according to the speed target value; according to the method, the stability and reliability of high-altitude spraying of the nozzle can be improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a pan / tilt control method and device for high-altitude spraying of unmanned aerial vehicles. Background Art

[0002] Traditional high-altitude spray painting live operations mainly rely on human-vehicle collaboration, which is not only inefficient but also has high safety risks. Especially in areas where tree-line conflicts and bird damage are frequent, the maintenance and repair of high-fault lines are restricted by many factors, such as the difficulty of live vehicles to reach, the difficulty of live operations, and terrain and space limitations. In addition, traditional high-altitude spray painting live operations require complex safety measures, such as building safety protection facilities and setting up warning areas, which increase the complexity and cost of the operation. Therefore, this traditional power maintenance method can no longer meet the needs of modern power systems for efficiency, safety and environmental protection.

[0003] To solve the above problems, drone spraying technology came into being. This technology realizes the whole process of detection, spraying and acceptance through automated and intelligent operation, greatly shortens the closed-loop cycle of defect handling, and significantly improves work efficiency. For example, patent text CN220430506U discloses an adjustable spray motion device for spraying drones, including an adjustable device support frame and a power supply. The adjustable device support frame includes a spray gun adjustment control mechanism, a telescopic mechanism, and a spray gun 2D motion gimbal; the telescopic mechanism includes a telescopic rod and a telescopic rod positioning support rod; the spray gun 2D motion gimbal includes a telescopic rod gimbal fixing base, a 2D gimbal mechanism, and a gimbal spray gun fixing sleeve. The power supply is installed in the power supply box. Support beams are installed on both sides of the power supply box. The left side is the rear support beam and the right side is the front support beam. The spray gun adjustment control mechanism is installed on the lower side of the power supply box. The front and rear limit blocks of the slider are installed on the spray gun adjustment control mechanism. The front limit block of the slider is located on the right side and the rear limit block of the slider is located on the left side. The left limit switch of the slider is installed on the rear limit block of the slider, and the right limit switch of the slider is installed on the front limit block of the slider. According to the spatial position of the drone, the spray gun position and posture are automatically adjusted.

[0004] During spraying operations, drones are easily disturbed by wind and body shaking, causing shaking during the spraying process. For spraying of tiny parts such as insulation defects of equipment on power poles, shaking will lead to inaccurate spraying positions and low spraying reliability. Summary of the invention

[0005] The present invention provides a pan / tilt control method and device for high-altitude spraying of unmanned aerial vehicles, which can improve the reliability of high-altitude spraying of unmanned aerial vehicles.

[0006] A method for controlling a gimbal for high-altitude spraying of an unmanned aerial vehicle, wherein a nozzle and a posture sensor are arranged on the gimbal, and the gimbal is controlled to rotate by a three-axis motor;

[0007] The method comprises:

[0008] Collecting the Euler angle signal of the nozzle by the posture sensor;

[0009] Inputting the Euler angle signal into a tracking differentiator for signal tracking and differentiation processing, and outputting an angle tracking differential signal;

[0010] Detecting the real-time speed of the three-axis motor and inputting the real-time speed of the three-axis motor into a linear expansion observer;

[0011] The linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal;

[0012] Calculating a system error according to the angle tracking differential signal and the speed observation differential signal, and inputting the system error into a linear error compensation controller;

[0013] The linear error compensator calculates and outputs a speed target value of the three-axis motor according to the system error and the total disturbance signal;

[0014] The rotation speed of the three-axis motor is controlled according to the speed target value.

[0015] Further, the Euler angle signal of the nozzle includes a nutation angle signal, a precession angle signal and a rotation angle signal;

[0016] The three-axis motor is used to control the roll axis, yaw axis and pitch axis of the gimbal.

[0017] Further, the angle tracking differential signal includes a differential signal and a tracking signal of the Euler angle;

[0018] The Euler angle signal is input to a tracking differentiator for signal tracking and differentiation processing, and an angle tracking differential signal is output, including:

[0019] Calculate the fastest control comprehensive function according to the input Euler angle signal;

[0020] The differential signal of the Euler angle at the current moment is calculated according to the differential signal of the Euler angle output by the tracking differentiator in the previous round, the preset integration step and the fastest control comprehensive function;

[0021] The tracking signal of the Euler angle at the current moment is calculated according to the tracking signal of the Euler angle output by the last tracking differentiator, the preset integration step and the differential signal of the Euler angle output by the last tracking differentiator.

[0022] Further, the speed observation differential signal includes a speed observation signal and a speed differential signal;

[0023] The linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal, including:

[0024] Calculate the output total disturbance signal according to the real-time speed and the speed observation signal output by the previous round of linear expansion observer;

[0025] Calculate the current speed differential signal according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, the speed target value of the three-axis motor obtained by the previous round of calculation, and the total disturbance signal;

[0026] The current speed observation signal is calculated according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, and the speed differential signal output by the previous round of linear expansion observer.

[0027] Further, calculating the system error according to the angle tracking differential signal and the speed observation differential signal includes:

[0028] Calculating the actual state error according to the tracking signal of the Euler angle at the current moment and the current speed observation signal;

[0029] The differential error is calculated according to the differential signal of the Euler angle at the current moment and the differential signal of the current speed.

[0030] Furthermore, the linear error compensator calculates and outputs a speed target value of the three-axis motor according to the system error and the total disturbance signal, including:

[0031] Calculating the previous stage speed value according to the actual state error, differential error and corresponding gain coefficient;

[0032] The preceding stage speed value is compensated according to the total disturbance signal and a preset system coefficient to obtain the speed target value.

[0033] Furthermore, the speed target value is a ratio of a difference value calculated based on the previous stage speed value and the total disturbance signal to a preset system coefficient.

[0034] Further, controlling the rotation speed of the three-axis motor according to the speed target value includes:

[0035] Collecting current information and rotor angle information of the three-axis motor, performing Clark transformation and Park transformation according to the current information and rotor angle information, and obtaining d-axis current and q-axis current in a synchronous rotating coordinate system;

[0036] Performing differential processing based on the rotor angle information to obtain the rotor angular velocity;

[0037] Inputting the speed target value and the rotor angular velocity into a speed regulator for calculation, and outputting a current expected value;

[0038] Inputting the current expected value, the d-axis current and the q-axis current into the current regulator, and calculating and obtaining the d-axis voltage target value and the q-axis voltage target value;

[0039] Performing inverse Park transformation and inverse Clark transformation on the d-axis voltage target value and the q-axis voltage target value according to the rotor angle information to obtain three-phase expected voltage values ​​of the three-axis motor;

[0040] SVPWM modulation is performed according to the three-phase desired voltage values ​​to generate a control signal to control the rotation speed of the three-axis motor.

[0041] A pan / tilt control device for high-altitude spraying of unmanned aerial vehicles applied to the above method comprises:

[0042] An angle acquisition module, used for acquiring the Euler angle signal of the nozzle through the attitude sensor;

[0043] A tracking differentiator, used for performing signal tracking and differentiation processing according to the Euler angle signal, and outputting an angle tracking differential signal;

[0044] A speed detection module, used for detecting the real-time speed of the three-axis motor and inputting the real-time speed of the three-axis motor into a linear expansion observer;

[0045] A linear expansion observer is used to observe the real-time speed and output a speed observation differential signal and a total disturbance signal;

[0046] An error compensation module, used for calculating a system error according to the angle tracking differential signal and the speed observation differential signal, and inputting the system error into a linear error compensation controller;

[0047] A linear error compensator, used for calculating and outputting a speed target value of the three-axis motor according to the system error and the total disturbance signal;

[0048] The motor control module is used to control the rotation speed of the three-axis motor according to the speed target value.

[0049] Further, the Euler angle of the nozzle includes a nutation angle signal, a precession angle signal and a rotation angle signal;

[0050] The three-axis motor is used to control the roll axis, yaw axis and pitch axis of the gimbal.

[0051] Further, the angle tracking differential signal includes a differential signal and a tracking signal of the Euler angle;

[0052] The tracking differentiator performs signal tracking and differentiation processing according to the Euler angle and outputs an angle tracking differential signal, including:

[0053] Calculate the fastest control comprehensive function according to the input Euler angle signal;

[0054] The differential signal of the Euler angle at the current moment is calculated according to the differential signal of the Euler angle output by the tracking differentiator in the previous round, the preset integration step and the fastest control comprehensive function;

[0055] The tracking signal of the Euler angle at the current moment is calculated according to the tracking signal of the Euler angle output by the last tracking differentiator, the preset integration step and the differential signal of the Euler angle output by the last tracking differentiator.

[0056] Further, the speed observation differential signal includes a speed observation signal and a speed differential signal;

[0057] The linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal, including:

[0058] Calculate the output total disturbance signal according to the real-time speed and the speed observation signal output by the previous round of linear expansion observer;

[0059] Calculate the current speed differential signal according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, the speed target value of the three-axis motor obtained by the previous round of calculation, and the total disturbance signal;

[0060] The current speed observation signal is calculated according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, and the speed differential signal output by the previous round of linear expansion observer.

[0061] Furthermore, the error compensation module calculates the system error according to the angle tracking differential signal and the speed observation differential signal, including:

[0062] Calculating the actual state error according to the tracking signal of the Euler angle at the current moment and the current speed observation signal;

[0063] The differential error is calculated according to the differential signal of the Euler angle at the current moment and the differential signal of the current speed.

[0064] Furthermore, the linear error compensator calculates and outputs a speed target value of the three-axis motor according to the system error and the total disturbance signal, including:

[0065] Calculating the previous stage speed value according to the actual state error, differential error and corresponding gain coefficient;

[0066] The preceding stage speed value is compensated according to the total disturbance signal and a preset system coefficient to obtain the speed target value.

[0067] Furthermore, the speed target value is a ratio of a difference value calculated based on the previous stage speed value and the total disturbance signal to a preset system coefficient.

[0068] Further, the motor control module controls the rotation speed of the three-axis motor according to the speed target value, including:

[0069] Collecting current information and rotor angle information of the three-axis motor, performing Clark transformation and Park transformation according to the current information and rotor angle information, and obtaining d-axis current and q-axis current in a synchronous rotating coordinate system;

[0070] Performing differential processing based on the rotor angle information to obtain the rotor angular velocity;

[0071] Inputting the speed target value and the rotor angular velocity into a speed regulator for calculation, and outputting a current expected value;

[0072] Inputting the current expected value, the d-axis current and the q-axis current into the current regulator, and calculating and obtaining the d-axis voltage target value and the q-axis voltage target value;

[0073] Performing inverse Park transformation and inverse Clark transformation on the d-axis voltage target value and the q-axis voltage target value according to the rotor angle information to obtain three-phase expected voltage values ​​of the three-axis motor;

[0074] SVPWM modulation is performed according to the three-phase desired voltage values ​​to generate a control signal to control the rotation speed of the three-axis motor.

[0075] An electronic device comprises a processor and a storage device, wherein the storage device stores a plurality of instructions, and the processor is used to read the instructions and execute the above method.

[0076] The pan / tilt control method and device for high-altitude spraying of unmanned aerial vehicles provided by the present invention have at least the following beneficial effects:

[0077] (1) Collecting the Euler angle signal of the nozzle and the real-time speed of the three-axis motor, tracking the Euler angle signal through the tracking differentiator, observing the real-time speed through the linear extension observer, calculating and outputting the speed target value of the three-axis motor through the linear error compensator, and controlling the three-axis motor based on the speed target value. The coordination of the tracking differentiator, the linear extension observer and the linear error compensator can quickly and accurately control and adjust the speed of the three-axis motor based on the Euler angle of the nozzle, thereby quickly adjusting the angles of the gimbal roll axis, yaw axis and pitch axis, overcoming the nozzle shaking problem caused by unstable flight to a certain extent, and ensuring the accuracy and reliability of nozzle spraying;

[0078] (2) The tracking differentiator is used to track the Euler angle signal, which has good noise resistance and can effectively avoid the error accumulation and signal oscillation problems introduced by noise;

[0079] (3) A linear extended observer is used with real-time speed for observation. By online observing unknown disturbances and system dynamic characteristics, the controller output is adjusted in real time to achieve the purpose of precise control. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] Figure 1 The present invention provides a flow chart of an embodiment of a method for controlling a pan / tilt system for high-altitude spraying of an unmanned aerial vehicle.

[0081] Figure 2 A control block diagram of the pan / tilt control method for high-altitude spraying of unmanned aerial vehicles provided by the present invention.

[0082] Figure 3 The present invention provides a flow chart of an embodiment of tracking differentiator signal processing in the pan / tilt control method for high-altitude spraying of unmanned aerial vehicles.

[0083] Figure 4 The present invention provides a flowchart of an embodiment of linear expansion observer signal processing in the pan-tilt control method for high-altitude spraying of unmanned aerial vehicles.

[0084] Figure 5 The present invention provides a flowchart of an embodiment of error calculation in the pan / tilt control method for high-altitude spraying of unmanned aerial vehicles.

[0085] Figure 6 The present invention provides a flowchart of an embodiment of linear error compensator signal processing in the pan / tilt control method for high-altitude spraying of unmanned aerial vehicles provided by the present invention.

[0086] Figure 7 A control block diagram of an embodiment of motor control in a pan / tilt control method for high-altitude spraying of an unmanned aerial vehicle provided by the present invention.

[0087] Figure 8The present invention provides a flow chart of an embodiment of motor control in a pan / tilt control method for high-altitude spraying of an unmanned aerial vehicle.

[0088] Fig. 9 The present invention is a flow chart of an embodiment of a pan / tilt control device for high-altitude spraying of unmanned aerial vehicles provided by the present invention. DETAILED DESCRIPTION

[0089] In order to better understand the above technical solution, the above technical solution will be described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0090] refer to Figure 1 , in some embodiments, a gimbal control method for high-altitude spraying of a drone is provided, wherein a nozzle and a posture sensor are provided on the gimbal, and the gimbal is controlled to rotate by a three-axis motor;

[0091] The method comprises:

[0092] S1, collecting the Euler angle signal of the nozzle through the posture sensor;

[0093] S2, inputting the Euler angle signal into a tracking differentiator for signal tracking and differentiation processing, and outputting an angle tracking differential signal;

[0094] S3, detecting the real-time speed of the three-axis motor, and inputting the real-time speed of the three-axis motor into a linear expansion observer;

[0095] S4, the linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal;

[0096] S5, calculating a system error according to the angle tracking differential signal and the speed observation differential signal, and inputting the system error into a linear error compensation controller;

[0097] S6, the linear error compensator calculates and outputs a speed target value of the three-axis motor according to the system error and the total disturbance signal;

[0098] S7. Control the rotation speed of the three-axis motor according to the speed target value.

[0099] Specifically, in step S1, the Euler angle signal of the nozzle includes a nutation angle signal, a precession angle signal and a rotation angle signal;

[0100] The three-axis motor is used to control the roll axis, yaw axis and pitch axis of the gimbal.

[0101] In some embodiments, the three-axis motor includes a first motor, a second motor and a third motor, the first motor is used to control the roll axis of the gimbal, the second motor is used to control the yaw axis of the gimbal, and the third motor is used to control the pitch axis of the gimbal.

[0102] Specifically, the nutation angle is input to the tracking differentiator for signal tracking and differentiation processing, and a nutation angle tracking differential signal is output. The real-time speed of the first motor is detected and input to the linear extension observer. The linear extension observer calculates and outputs the speed differential signal and the total disturbance signal of the first motor according to the real-time speed of the first motor. The first system error is calculated according to the nutation angle tracking differential signal and the speed observation differential signal of the first motor, and the first system error is input to the linear error compensation controller. The linear error compensator calculates and outputs the target value of the first motor speed according to the first system error and the total disturbance signal.

[0103] The precession angle is input to the tracking differentiator for signal tracking and differentiation processing, and a precession angle tracking differential signal is output. The real-time speed of the second motor is detected and input to the linear extension observer. The linear extension observer calculates and outputs the speed observation differential signal and the total disturbance signal of the second motor according to the real-time speed of the second motor. The second system error is calculated according to the precession angle tracking differential signal and the speed observation differential signal of the second motor, and the second system error is input to the linear error compensation controller. The linear error compensator calculates and outputs the target value of the second motor speed according to the second system error and the total disturbance signal.

[0104] The rotation angle is input to the tracking differentiator for signal tracking and differentiation processing, and the rotation angle tracking differential signal is output. The real-time speed of the third motor is detected and input to the linear extension observer. The linear extension observer calculates and outputs the speed observation differential signal and the total disturbance signal of the third motor according to the real-time speed of the third motor. The third system error is calculated according to the rotation angle tracking differential signal and the speed observation differential signal of the third motor, and the third system error is input to the linear error compensation controller. The linear error compensator calculates and outputs the target value of the speed of the third motor according to the third system error and the total disturbance signal.

[0105] In an embodiment of the present invention, the gimbal is controlled collaboratively by a tracking differentiator, a linear expansion observer, and a linear error compensation controller. The nutation angle, precession angle, and rotation angle of the Euler angle are respectively input into the tracking differentiator, and the roll axis, yaw axis, and pitch axis of the gimbal are respectively controlled by calculating the output control signals.

[0106] Further, refer to Figure 2 and Figure 3 In step S2, the angle tracking differential signal includes a differential signal of the Euler angle and a tracking signal;

[0107] The Euler angle signal is input to a tracking differentiator for signal tracking and differentiation processing, and an angle tracking differential signal is output, including:

[0108] S21, calculating the fastest control comprehensive function according to the input Euler angle signal;

[0109] S22, obtaining the differential signal of the Euler angle at the current moment according to the differential signal of the Euler angle output by the tracking differentiator in the previous round, the preset integration step size and the fastest control comprehensive function;

[0110] S23, calculating and obtaining the tracking signal of the Euler angle at the current moment according to the tracking signal of the Euler angle output by the last tracking differentiator, the preset integration step, and the differential signal of the Euler angle output by the last tracking differentiator.

[0111] Specifically, the Tracking Differentiator (TD) aims to solve the impact of drastic changes in the input signal on the control system and provide the controller with accurate information on the signal change rate. Compared with the traditional differentiator, the tracking differentiator has better noise resistance and can effectively avoid the error accumulation and signal oscillation problems introduced by noise. Its expression is as follows:

[0112]

[0113] Where v1(k+1) represents the tracking signal of the Euler angle at the current moment, v1(k) represents the tracking signal of the Euler angle obtained in the previous round of calculation, h represents the integration step, v2(k+1) represents the differential signal of the Euler angle at the current moment, v2(k) represents the differential signal of the Euler angle obtained in the previous round of calculation, h0 is the filtering parameter, and f h is the fastest control comprehensive function, v(k) represents the current input Euler angle signal, and r is the parameter for adjusting the tracking speed.

[0114] Specifically, in step S21, the fastest control comprehensive function is a function of the difference between the tracking signal v1(k) of the Euler angle obtained in the previous round of calculation and the currently input Euler angle signal v(k), and the differential signal v2(k) of the Euler angle obtained in the previous round of calculation. The fastest control comprehensive function f is calculated based on the input Euler angle signal v(k). h , as shown in formula (3).

[0115] In step S22, according to the differential signal v2(k) of the Euler angle obtained in the previous round, the set integration step h and the fastest control comprehensive function f h , calculate and obtain the differential signal v2(k+1) of the Euler angle at the current moment, as shown in formula (2).

[0116] In step S23, the tracking signal v1(k+1) of the Euler angle at the current moment is calculated based on the tracking signal v1(k) of the Euler angle obtained in the previous round, the set integration step h, and the differential signal v2(k) of the Euler angle obtained in the previous round, as shown in formula (1).

[0117] In some embodiments, in step S2, the Euler angle signal is input into a tracking differentiator for signal tracking and differentiation processing, that is, the nutation angle, precession angle and rotation angle are respectively input into the tracking differentiator for signal tracking and differentiation processing, and the corresponding angle tracking differentiation signal is output.

[0118] Furthermore, in step S3, the real-time speed of the three-axis motor is detected and input into the linear expansion observer. Specifically, the real-time speeds of the three-axis motor in three control directions are detected and input into the linear expansion observer respectively.

[0119] The Linear Extended State Observer (LESO) is a special form of the Extended State Observer (ESO) for state estimation and disturbance compensation of dynamic systems. The core purpose of LESO is to compensate for the nonlinearity and uncertainty of the system by observing the state of the system and unknown disturbances in real time, thereby improving the robustness and control performance of the system. LESO is mainly used to actively suppress the impact of external disturbances and uncertainties on the system, especially in some complex systems that are difficult to model. It achieves the purpose of precise control by online observation of unknown disturbances and system dynamic characteristics and adjusting the output of the controller in real time.

[0120] refer to Figure 2 and Figure 4 , in step S4, the speed observation differential signal includes a speed observation signal and a speed differential signal;

[0121] The linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal, including:

[0122] S41, calculating and outputting a total disturbance signal according to the real-time speed and the speed observation signal output by the previous round of linear expansion observer;

[0123] S42, calculating a current speed differential signal according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, the speed target value of the three-axis motor obtained by the previous round of calculation, and the total disturbance signal;

[0124] S43, obtaining a current speed observation signal according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, and the speed differential signal output by the previous round of linear expansion observer.

[0125] Specifically, the mathematical expression of the linear expansion observer is as follows:

[0126]

[0127] in, represents the speed observation signal output by the previous round of linear expansion observer, represents the velocity differential signal output by the previous round of linear expansion observer, is the total output disturbance of the previous round of linear expansion observer output, Indicates the current speed observation signal, Indicates the current speed differential signal, is the total disturbance signal currently output, β1, β2, β3 are the speed observation gain coefficient, the differential observation gain coefficient and the total disturbance observation gain coefficient respectively. The tracking effect of the state variable can be controlled by adjusting the three gain coefficients, u' is the speed target value of the three-axis motor obtained by the previous round of calculation, and b0 is the system coefficient.

[0128] Specifically, in step S41, according to the real-time speed y and the speed observation signal output by the previous round of linear expansion observer Calculate the difference, multiply it by the total disturbance observation gain coefficient β3, and calculate the total disturbance signal As shown in formula (6).

[0129] In step S42, according to the real-time speed y and the speed observation signal output by the previous round of linear expansion observer Total disturbance signal The system coefficient b0, the speed target value u' of the three-axis motor obtained in the previous round of calculation, and the differential observation gain coefficient β2 are used to calculate the current speed differential signal As shown in formula (5).

[0130] In step S43, according to the real-time speed y and the speed observation signal output by the previous round of linear expansion observer Represents the velocity differential signal output by the previous linear expansion observer And the speed observation gain coefficient β1, calculate the current speed observation signal As shown in formula (4).

[0131] Specifically, the signal processing of the real-time speed in step S4 is to process the real-time speeds in three control directions of the three-axis motors respectively.

[0132] Further, refer to Figure 5 In step S5, the system error is calculated according to the angle tracking differential signal and the speed observation differential signal, including:

[0133] S51, calculating the actual state error according to the tracking signal of the Euler angle at the current moment and the speed observation signal;

[0134] S52, calculating a differential error according to the differential signal of the Euler angle at the current moment and the differential signal of the velocity.

[0135] Specifically, in step S51, the actual state error is calculated by the following formula:

[0136]

[0137] Among them, e1 represents the actual state error, v1(k+1) represents the tracking signal of the Euler angle at the current moment, Indicates the current speed observation signal.

[0138] Further, in step S52, the differential error is calculated by the following formula:

[0139]

[0140] Among them, e2 represents the differential error, v2(k+1) represents the differential signal of the Euler angle at the current moment, Indicates the current speed differential signal.

[0141] In step S5, the system error is input into a linear error compensation controller. The linear error compensation controller (LESF) is a controller based on the feedback control principle, which is mainly used for error compensation and adjustment in dynamic systems. The core idea is to measure the error of the system (i.e., the deviation between the actual output of the system and the expected output), and use the error value to feed back to the controller for adjustment, so as to achieve the purpose of precise control.

[0142] Further, refer to Figure 6 In step S6, the linear error compensator calculates and outputs the speed target value of the three-axis motor according to the system error and the total disturbance signal, including:

[0143] S61, calculating the previous stage speed value according to the actual state error, differential error and corresponding gain coefficient;

[0144] S62. Compensate the previous stage speed value according to the total disturbance signal and a preset system coefficient to obtain the speed target value.

[0145] Specifically, the current speed observation signal output by the linear expansion observer is Current speed differential signal The actual state error e1 and the differential error e2 are calculated with the tracking signal v1(k+1) of the Euler angle at the current moment output by the tracking differentiator and the differential signal v2(k+1) of the Euler angle at the current moment. The actual state error e1 and the differential error e2 are input to the linear error compensator, and the output front-stage speed value u0 is calculated and output. The speed target value u is calculated according to the output total disturbance output by the linear extended observer and the front-stage speed value u0.

[0146] Specifically, in step S61, the previous stage speed value is calculated according to the following formula:

[0147] u0=b1·e1+b2·e2; (9)

[0148] Among them, u0 represents the previous stage speed value, e1 represents the actual state error, e2 represents the differential error, and b1 and b2 are gain coefficients.

[0149] In step S62, the speed target value is calculated by the following formula:

[0150]

[0151] Among them, u is the speed target value, u0 represents the previous stage speed value, represents the total output disturbance, and b0 represents the system coefficient.

[0152] Specifically, refer to Figure 2 , input the collected Euler angle signal v into the tracking differentiator TD, calculate and output the differential signal v of the Euler angle at the current moment 2( k+1) and the tracking signal v1(k+1) of the current Euler angle; the real-time speed y of the three-axis motor is input to the linear extended observer LESO, and the current speed observation signal is output after calculation. Current speed differential signal and the total disturbance signal After that, observe the signal according to the current speed Current speed differential signal The differential signal v of the Euler angle at the current moment 2( k+1) and the tracking signal v1(k+1) of the current Euler angle, the actual state error e1 and the differential error e2 are calculated, and the actual state error e1 and the differential error e2 are input to the linear error compensation controller LESF, and the previous stage speed value u0 is calculated. According to the previous stage speed value u0 and the total disturbance signal Calculate and obtain the speed target value u.

[0153] Specifically, refer to Figure 7 and Figure 8 In step S7, the rotation speed of the three-axis motor is controlled according to the speed target value, including:

[0154] S71, collecting current information and rotor angle information of the three-axis motor, performing Clark transformation and Park transformation according to the current information and rotor angle information, and obtaining a d-axis current and a q-axis current in a synchronous rotating coordinate system;

[0155] S72, performing differential processing based on the rotor angle information to obtain a rotor angular velocity;

[0156] S73, inputting the speed target value and the rotor angular velocity into a speed regulator for calculation, and outputting a current expected value;

[0157] S74, inputting the expected current value, the d-axis current and the q-axis current into the current regulator, and calculating and obtaining the d-axis voltage target value and the q-axis voltage target value;

[0158] S75, performing inverse Park transformation and inverse Clark transformation on the d-axis voltage target value and the q-axis voltage target value according to the rotor angle information to obtain three-phase expected voltage values ​​of the three-axis motor;

[0159] S76, performing SVPWM modulation according to the three-phase desired voltage values ​​to generate a control signal to control the rotation speed of the three-axis motor.

[0160] refer to Figure 7 In this embodiment, the three-axis motor is controlled based on the FOC motor control algorithm, including a speed control loop based on a speed regulator and a current control loop based on a current regulator. The specific method is: the rotor angle θ and current I of the three-axis motor are collected by an encoder. abc , after Clark transformation and Park transformation, the d-axis current Id and q-axis current Iq in the synchronous rotating coordinate system are obtained; at the same time, the rotor angle θ is differentiated to obtain the rotor angular velocity, and the obtained speed target value u and the rotor angular velocity are input into the speed regulator for calculation, and the expected current value is output. The expected current value is the expected current value of the q-axis, and the expected current value of the d-axis is 0. The obtained expected current value of the q-axis, the d-axis current Id and the q-axis current Iq are input into the current regulator, and the d-axis voltage target value Vd and the q-axis voltage target value Vq are calculated; according to the rotor angle θ, the d-axis voltage target value Vd and the q-axis voltage target value Vq are subjected to inverse Park transformation and inverse Clark transformation to obtain the three-phase expected voltage values ​​Va, Vb and Vc of the three-axis motor, and finally updated to the PWM output of each phase through the SVM mechanism, and finally acted on the motor by the three-phase inverter bridge.

[0161] refer to Fig. 9 In some embodiments, a pan / tilt control device for high-altitude spraying of an unmanned aerial vehicle applied to the above method is provided, comprising:

[0162] Angle acquisition module 201, used for acquiring the Euler angle signal of the nozzle through the posture sensor;

[0163] A tracking differentiator 202, used to perform signal tracking and differentiation processing according to the Euler angle signal, and output an angle tracking differential signal;

[0164] A speed detection module 203 is used to detect the real-time speed of the three-axis motor and input the real-time speed of the three-axis motor into a linear expansion observer;

[0165] A linear expansion observer 204, used for observing according to the real-time speed, and outputting a speed observation differential signal and a total disturbance signal;

[0166] An error compensation module 205, used for calculating a system error according to the angle tracking differential signal and the speed observation differential signal, and inputting the system error into a linear error compensation controller;

[0167] A linear error compensator 206, configured to calculate and output a speed target value of the three-axis motor according to the system error and the total disturbance signal;

[0168] The motor control module 207 is used to control the rotation speed of the three-axis motor according to the speed target value.

[0169] Further, the Euler angle of the nozzle includes a nutation angle signal, a precession angle signal and a rotation angle signal;

[0170] The three-axis motor is used to control the roll axis, yaw axis and pitch axis of the gimbal.

[0171] Further, the angle tracking differential signal includes a differential signal and a tracking signal of the Euler angle;

[0172] The tracking differentiator 202 performs signal tracking and differentiation processing according to the Euler angle, and outputs an angle tracking differential signal, including:

[0173] Calculate the fastest control comprehensive function according to the input Euler angle signal;

[0174] The differential signal of the Euler angle at the current moment is calculated according to the differential signal of the Euler angle output by the tracking differentiator in the previous round, the preset integration step and the fastest control comprehensive function;

[0175] The tracking signal of the Euler angle at the current moment is calculated according to the tracking signal of the Euler angle output by the last tracking differentiator, the preset integration step and the differential signal of the Euler angle output by the last tracking differentiator.

[0176] Further, the speed observation differential signal includes a speed observation signal and a speed differential signal;

[0177] The linear expansion observer 204 observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal, including:

[0178] Calculate the output total disturbance signal according to the real-time speed and the speed observation signal output by the previous round of linear expansion observer;

[0179] Calculate the current speed differential signal according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, the speed target value of the three-axis motor obtained by the previous round of calculation, and the total disturbance signal;

[0180] The current speed observation signal is calculated according to the real-time speed, the speed observation signal output by the previous round of linear expansion observer, and the speed differential signal output by the previous round of linear expansion observer.

[0181] Furthermore, the error compensation module 205 calculates the system error according to the angle tracking differential signal and the speed observation differential signal, including:

[0182] Calculating the actual state error according to the tracking signal of the Euler angle at the current moment and the current speed observation signal;

[0183] The differential error is calculated according to the differential signal of the Euler angle at the current moment and the differential signal of the current speed.

[0184] Furthermore, the linear error compensator 206 calculates and outputs the target speed value of the three-axis motor according to the system error and the total disturbance signal, including:

[0185] Calculating the previous stage speed value according to the actual state error, differential error and corresponding gain coefficient;

[0186] The preceding stage speed value is compensated according to the total disturbance signal and a preset system coefficient to obtain the speed target value.

[0187] Furthermore, the speed target value is a ratio of a difference value calculated based on the previous stage speed value and the total disturbance signal to a preset system coefficient.

[0188] Furthermore, the motor control module 207 controls the rotation speed of the three-axis motor according to the speed target value, including:

[0189] Collecting current information and rotor angle information of the three-axis motor, performing Clark transformation and Park transformation according to the current information and rotor angle information, and obtaining d-axis current and q-axis current in a synchronous rotating coordinate system;

[0190] Performing differential processing based on the rotor angle information to obtain the rotor angular velocity;

[0191] Inputting the speed target value and the rotor angular velocity into a speed regulator for calculation, and outputting a current expected value;

[0192] Inputting the current expected value, the d-axis current and the q-axis current into the current regulator, and calculating and obtaining the d-axis voltage target value and the q-axis voltage target value;

[0193] Performing inverse Park transformation and inverse Clark transformation on the d-axis voltage target value and the q-axis voltage target value according to the rotor angle information to obtain three-phase expected voltage values ​​of the three-axis motor;

[0194] SVPWM modulation is performed according to the three-phase desired voltage values ​​to generate a control signal to control the rotation speed of the three-axis motor.

[0195] In some embodiments, an electronic device is also provided, including a processor and a storage device, wherein the storage device stores a plurality of instructions, and the processor is used to read the instructions and execute the above method.

[0196] The above-mentioned embodiments provide a method and device for controlling a pan / tilt system for high-altitude spraying of a UAV, which have at least the following beneficial effects:

[0197] (1) Collecting the Euler angle signal of the nozzle and the real-time speed of the three-axis motor, tracking the Euler angle signal through the tracking differentiator, observing the real-time speed through the linear extension observer, calculating and outputting the speed target value of the three-axis motor through the linear error compensator, and controlling the three-axis motor based on the speed target value. The coordination of the tracking differentiator, the linear extension observer and the linear error compensator can quickly and accurately control and adjust the speed of the three-axis motor based on the Euler angle of the nozzle, thereby quickly adjusting the angles of the gimbal roll axis, yaw axis and pitch axis, overcoming the nozzle shaking problem caused by unstable flight to a certain extent, and ensuring the accuracy and reliability of nozzle spraying;

[0198] (2) The tracking differentiator is used to track the Euler angle signal, which has good noise resistance and can effectively avoid the error accumulation and signal oscillation problems introduced by noise;

[0199] (3) A linear extended observer is used with real-time speed for observation. By online observing unknown disturbances and system dynamic characteristics, the controller output is adjusted in real time to achieve the purpose of precise control.

[0200] Although preferred embodiments of the present invention have been described, additional changes and modifications may be made to these embodiments by those skilled in the art once the basic inventive concepts are known. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention. Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A pan / tilt control method for high-altitude spraying of unmanned aerial vehicles, characterized in that: The platform is provided with a nozzle and a posture sensor, and the platform is controlled to rotate by a three-axis motor; The method comprises: Collecting the Euler angle signal of the nozzle by the posture sensor; Inputting the Euler angle signal into a tracking differentiator for signal tracking and differentiation processing, and outputting an angle tracking differential signal; Detecting the real-time speed of the three-axis motor and inputting the real-time speed of the three-axis motor into a linear expansion observer; The linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal; Calculating a system error according to the angle tracking differential signal and the speed observation differential signal, and inputting the system error into a linear error compensation controller; The linear error compensator calculates and outputs a speed target value of the three-axis motor according to the system error and the total disturbance signal; The rotation speed of the three-axis motor is controlled according to the speed target value.

2. The method according to claim 1, characterized in that: The Euler angle signal of the nozzle includes a nutation angle signal, a precession angle signal and a rotation angle signal; The three-axis motor is used to control the roll axis, yaw axis and pitch axis of the gimbal.

3. The method according to claim 1, characterized in that The angle tracking differential signal includes a differential signal of the Euler angle and a tracking signal; The Euler angle signal is input to a tracking differentiator for signal tracking and differentiation processing, and an angle tracking differential signal is output, including: Calculate the fastest control comprehensive function according to the input Euler angle signal; The differential signal of the Euler angle at the current moment is calculated according to the differential signal of the Euler angle output by the tracking differentiator in the previous round, the preset integration step and the fastest control comprehensive function; The tracking signal of the Euler angle at the current moment is calculated according to the tracking signal of the Euler angle output by the last tracking differentiator, the preset integration step and the differential signal of the Euler angle output by the last tracking differentiator.

4. The method according to claim 3, characterized in that The speed observation differential signal includes a speed observation signal and a speed differential signal; The linear expansion observer observes the real-time speed and outputs a speed observation differential signal and a total disturbance signal, including: Calculate the output total disturbance signal according to the real-time speed and the speed observation signal output by the previous round of linear expansion observer; The current speed differential signal is calculated based on the real-time speed, the speed observation signal output by the previous round of linear expansion observer, the speed target value of the three-axis motor obtained by the previous round of calculation, and the total disturbance signal; A current speed observation signal is obtained according to the real-time speed, a speed observation signal output by a previous round of linear expansion observer, and a speed differential signal output by the previous round of linear expansion observer.

5. The method according to claim 4, characterized in that Calculating a system error according to the angle tracking differential signal and the speed observation differential signal includes: Calculating the actual state error according to the tracking signal of the Euler angle at the current moment and the current speed observation signal; The differential error is calculated according to the differential signal of the Euler angle at the current moment and the differential signal of the current speed.

6. The method according to claim 5, characterized in that The linear error compensator calculates and outputs a speed target value of the three-axis motor according to the system error and the total disturbance signal, including: Calculating the previous stage speed value according to the actual state error, differential error and corresponding gain coefficient; The preceding stage speed value is compensated according to the total disturbance signal and a preset system coefficient to obtain the speed target value.

7. The method according to claim 6, characterized in that The speed target value is a ratio of a difference value calculated based on the previous stage speed value and the total disturbance signal to a preset system coefficient.

8. The method according to claim 7, characterized in that Controlling the rotation speed of the three-axis motor according to the speed target value includes: Collecting current information and rotor angle information of the three-axis motor, performing Clark transformation and Park transformation according to the current information and rotor angle information, and obtaining d-axis current and q-axis current in a synchronous rotating coordinate system; Performing differential processing based on the rotor angle information to obtain the rotor angular velocity; Inputting the speed target value and the rotor angular velocity into a speed regulator for calculation, and outputting a current expected value; Inputting the current expected value, the d-axis current and the q-axis current into the current regulator, and calculating and obtaining the d-axis voltage target value and the q-axis voltage target value; Performing inverse Park transformation and inverse Clark transformation on the d-axis voltage target value and the q-axis voltage target value according to the rotor angle information to obtain three-phase expected voltage values ​​of the three-axis motor; SVPWM modulation is performed according to the three-phase desired voltage values ​​to generate a control signal to control the rotation speed of the three-axis motor.

9. A pan / tilt control device for high-altitude spraying of unmanned aerial vehicles applied to the method as claimed in any one of claims 1 to 8, characterized in that: include: An angle acquisition module, used for acquiring the Euler angle signal of the nozzle through the attitude sensor; A tracking differentiator, used for performing signal tracking and differentiation processing according to the Euler angle signal, and outputting an angle tracking differential signal; A speed detection module, used for detecting the real-time speed of the three-axis motor and inputting the real-time speed of the three-axis motor into a linear expansion observer; A linear expansion observer is used to observe the real-time speed and output a speed observation differential signal and a total disturbance signal; An error compensation module, used for calculating a system error according to the angle tracking differential signal and the speed observation differential signal, and inputting the system error into a linear error compensation controller; A linear error compensator, used for calculating and outputting a speed target value of the three-axis motor according to the system error and the total disturbance signal; The motor control module is used to control the rotation speed of the three-axis motor according to the speed target value.

10. An electronic device, characterized in that: The method comprises a processor and a storage device, wherein the storage device stores a plurality of instructions, and the processor is used to read the instructions and execute the method according to any one of claims 1 to 8.

Images