A pan / tilt position control method, system and unmanned aerial vehicle

Abstract

The present invention discloses a pan / tilt azimuth control method, system, and unmanned aerial vehicle (UAV) comprising the following steps: the pan / tilt azimuth obtains pan / tilt control parameter information from sensors and pre-stage equipment; performs compensation calculations based on the pan / tilt control parameter information to obtain a compensated pan / tilt azimuth angle; obtains pan / tilt following parameters for controlling the pan / tilt motor; and generates and outputs pan / tilt motor control signals based on the pan / tilt motor's speed loop PID controller and position loop PID controller. The present invention accelerates pan / tilt azimuth angle response and following speed. Furthermore, through a one-way hysteresis variable parameter control algorithm, the method maintains the weak following characteristics of the pan / tilt control during steady-state conditions at small angles, while accelerating convergence at large angles, thereby improving the pan / tilt's small-angle following accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles (UAVs), and more particularly to a pan / tilt position control method and system, and an UAV. Background Art

[0002] In areas such as aircraft payloads and handheld gimbals, it is often desired that the gimbal camera's perspective be a first-person perspective during use, and that it be able to quickly follow and align in the correct direction without drift.

[0003] To achieve the above purpose, the existing technology mostly adopts the method of weakly following the gimbal's yaw angle to achieve it in front of the gimbal movement. That is, when the drone's heading rotates, the gimbal weakly follows the heading angle to converge the heading angle. However, there will be obvious lag in the image during this process. Moreover, after the gimbal camera is aimed at the target, the image is still moving due to weak following, resulting in obvious image drift. Especially for zoom camera gimbals, it greatly affects user control and experience.

[0004] At the same time, when the above-mentioned gimbal weak following control method converges the gimbal heading angle to a small angle, the image often drifts in the horizontal direction, resulting in a very poor user experience, especially for a large-magnification zoom gimbal. This phenomenon is mainly caused by the fact that the above-mentioned weak following control method takes too long to converge when converging the gimbal heading angle to a small angle. Summary of the Invention

[0005] The purpose of the present invention is to provide a gimbal azimuth control method, system and drone, which can accelerate the gimbal azimuth angle response and following speed, and at the same time, maintain the weak following characteristics of the gimbal control during the small-angle steady-state process through a one-way hysteresis variable parameter control algorithm, and accelerate the convergence characteristics at large angles to improve the accuracy of the gimbal's small-angle following.

[0006] In order to achieve these purposes and other advantages according to the present invention, a pan / tilt position control method is provided, which comprises the following steps:

[0007] The PTZ obtains PTZ control parameter information from sensors and front-end devices;

[0008] Perform compensation calculation according to the pan / tilt control parameter information to obtain a compensated pan / tilt azimuth angle;

[0009] Get the gimbal following parameters used to control the gimbal motors;

[0010] The pan-tilt motor speed loop PID controller and position loop PID controller generate and output pan-tilt motor control signals.

[0011] Preferably, the gimbal control parameter information includes one or more of the gimbal IMU data information, gimbal azimuth angle, azimuth control information output by the drone controller / handheld gimbal controller, heading speed, etc.

[0012] Preferably, the gimbal can be mounted on a drone or is a handheld gimbal.

[0013] Preferably, performing compensation calculation according to the pan / tilt control parameter information to obtain the compensated pan / tilt azimuth angle includes the following steps:

[0014] Obtain the compensated gimbal azimuth angle Err_Ture using the following formula:

[0015] Err_Ture=Err+PosH_K*Err_b

[0016] Among them, PosH_K is the position loop compensation parameter, which can be adjusted according to the actual tracking effect; Err is the current azimuth error angle of the gimbal, which can be obtained through the sensor; Err_b is the azimuth control information output by the drone controller / handheld gimbal controller.

[0017] Preferably, obtaining the pan-tilt following parameters for controlling the pan-tilt motor includes the following steps:

[0018] Calculate the absolute value of the gimbal position angle error abs_temp according to the formula abs_temp = fabs(target position A - position sensor feedback B);

[0019] Calculate the position loop compensation parameter PosH_K based on the absolute value of the gimbal position angle error abs_temp;

[0020] Compare the position loop compensation parameter PosH_K with the gimbal following parameter NLSEF_K, and

[0021] If PosH_K>NLSEF_K, the value of the gimbal following parameter NLSEF_K satisfies NLSEF_K=PosH_K;

[0022] If PosH_K≤NLSEF_K, and the absolute value of the current gimbal position angle error abs_temp≥0.1, then NLSEF_K(T+1)=(0.97+0.0027*NLSEF_K(T))*NLSEF_K(T)+(0.03-0.0027*NLSEF_K(T))*PosH_K(T), where,

[0023] T is the variable per unit time, representing the current moment, and T+1 represents the next moment;

[0024] If PosH_K ≤ NLSEF_K and the absolute value of the current pan-tilt position angle error abs_temp < 0.1, then the value of the pan-tilt following parameter NLSEF_K satisfies NLSEF_K = PosH_K.

[0025] Preferably, calculating the position loop compensation parameter PosH_K according to the absolute value of the pan-tilt position angle error abs_temp includes the following steps:

[0026] When abs_temp < X[0], PosH_K = Y[0];

[0027] When abs_temp ≥ X[n], PosH_K = Y[n];

[0028] When X[0] < abs_temp < X[n], perform one-dimensional interpolation calculation, which includes the following steps:

[0029] When abs_temp ∈ [X[0], X[1]],

[0030] Then PosH_K = ((Y[1] - Y[0]) / (X[1] - X[0])) * (abs_temp - X[0]) + Y[0];

[0031] When abs_temp ∈ [X[1], X[2]],

[0032] Then PosH_K = ((Y[2] - Y[1]) / (X[2] - X[1])) * (abs_temp - X[1]) + Y[1];

[0033] When abs_temp ∈ [X[2], X[3]],

[0034] PosH_K = ((Y[3] - Y[2]) / (X[3] - X[2])) * (abs_temp - X[2]) + Y[2]; ...

[0036] When abs_temp ∈ [X[n - 1], X[n]],

[0037] Then PosH_K = ((Y[n] - Y[n - 1]) / (X[n] - X[n - 1])) * (abs_temp - X[n - 1]) + Y[n - 1];

[0038] Where, n is a positive integer, X[0], X[1], X[2], X[3]... X[n] are the error arrays of the absolute value abs_temp of the pan-tilt position angle error, Y[0], Y[1], Y[2], Y[3]... Y[n] are the coefficient arrays, X[0], X[1], X[2], X[3]... X[n], Y[0], Y[1], Y[2], Y[3]... Y[n] are all constants, and X[0] < X[1] < X[2] < X[3]... X[n], Y[0] < Y[1] < Y[2] < Y[3]... Y[n], and at the same time, Y[0], Y[1], Y[2], Y[3]... Y[n] are all greater than 0.

[0039] Preferably, generating and outputting the pan-tilt motor control signal according to the pan-tilt motor speed loop PID controller and the position loop PID controller includes the following steps:

[0040] The position loop PID controller outputs the output Output of the position loop PID controller according to the following formula:

[0041]

[0042] Where, Kp = NLSEF_K, Ki and Kd are the relevant parameters of the position loop PID controller.

[0043] Preferably, generating and outputting the pan-tilt motor control signal according to the pan-tilt motor speed loop PID controller and the position loop PID controller includes the following steps:

[0044] The speed loop PID controller outputs the output Output' of the speed loop PID controller according to the following formula:

[0045]

[0046] Where, Kp', Ki' and Kd' are the relevant parameters of the speed loop PID controller; Err' is the deviation value between the output Output of the position loop PID controller and the azimuth angular velocity obtained by the azimuth speed sensor inside the pan-tilt.

[0047] A pan-tilt control system is also provided, which includes:

[0048] A pan-tilt position feed-forward compensator, which is used to perform compensation calculation according to the pan-tilt control parameter information to obtain the compensated pan-tilt azimuth angle;

[0049] A pan-tilt unidirectional hysteresis variable parameter controller, which is used to obtain the pan-tilt following parameter for controlling the pan-tilt motor;

[0050] A speed loop PID controller, which is used to output the output of the position loop PID controller;

[0051] The position loop PID controller is used to output the output of the speed loop PID controller.

[0052] A drone is also provided, which includes the above-mentioned gimbal control system.

[0053] The present invention includes at least the following beneficial effects: the present invention can perform compensation calculation on the gimbal azimuth angle according to the gimbal control parameter information, realize the advanced prediction of the current motion state of the gimbal, and accelerate the gimbal azimuth angle response and following speed; at the same time, through the one-way hysteresis variable parameter control algorithm, it can realize the weak following characteristics of the gimbal control in the small-angle steady-state process, and accelerate the convergence characteristics at large angles, so as to improve the accuracy of the gimbal small-angle following, and further solve the problem of the image drifting in the horizontal direction. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 The figure is a flow chart of the steps of the pan-tilt position control method of the present invention.

[0055] Figure 2 This is the curve of the change of the pan / tilt following parameter NLSEF_K over time when PosH_K changes from 10 to 0.2 in the present invention.

[0056] Figure 3 It is a structural diagram of the pan-tilt position control system in the present invention. DETAILED DESCRIPTION

[0057] The present invention will be described in further detail below in conjunction with the accompanying drawings so that those skilled in the art can implement the invention with reference to the description.

[0058] In the description of the present invention, the terms "horizontal", "longitudinal", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside" and the like to indicate orientations or positional relationships are based on the orientations or positional relationships shown in the accompanying drawings and are only for the convenience of describing the present invention and simplifying the description. They do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation, and therefore should not be understood as limiting the present invention.

[0059] Example 1:

[0060] like Figure 1 As shown, this embodiment provides a pan / tilt position control method, which includes the following steps:

[0061] S1. After the PTZ is powered on and initialized, it obtains PTZ control parameter information from sensors and front-end devices.

[0062] In this embodiment, the gimbal can be mounted on a drone or the gimbal is a handheld gimbal; the front-end device includes a drone and / or a drone remote controller; the gimbal control parameter information includes one or several of gimbal IMU data information, gimbal azimuth angle, azimuth control information output by the drone controller / handheld gimbal controller (this azimuth control information can be quantified, such as the rocker amount of the drone remote controller / handheld gimbal remote controller, and its range is -128 to 128. When it is greater than 0, the drone turns right, and when it is less than 0, the drone turns left. The larger the absolute value of the value, the faster the drone rotates, and the smaller the value, the slower the drone rotates), heading speed, etc.;

[0063] S2. Perform compensation calculation according to the gimbal control parameter information to obtain the compensated gimbal azimuth angle, so as to achieve the advanced prediction of the current motion state of the gimbal and accelerate the speed of the gimbal azimuth angle response and following;

[0064] Step S2 includes the following steps:

[0065] Obtain the compensated gimbal azimuth angle Err_Ture according to the following formula:

[0066] Err_Ture = Err + PosH_K * Err_b

[0067] Where, PosH_K is the position loop compensation parameter, which can be adjusted according to the actual tracking effect; Err is the current azimuth error angle of the gimbal, which can be obtained by a sensor; Err_b is the azimuth control information output by the drone controller / handheld gimbal controller;

[0068] S3. Obtain the gimbal following parameters for controlling the gimbal motor, and it specifically includes the following steps:

[0069] S331. Calculate the absolute value of the gimbal position angle error abs_temp according to the formula abs_temp = fabs(target position A - position sensor feedback B); where, fabs represents taking the absolute value, the target position A is the position where the user expects to adjust the gimbal angle, such as 0° in the due front of the drone; the position sensor feedback B includes the current gimbal angle position obtained by the gimbal angle sensor, such as 30°, then at this time the absolute value of the gimbal position angle error abs_temp = fabs(0° - 30°) = 30°;

[0070] S332. Calculate the position loop compensation parameter PosH_K according to the absolute value of the gimbal position angle error abs_temp, and it specifically includes the following steps:

[0071] When abs_temp < X[0], PosH_K = Y[0];

[0072] When abs_temp ≥ X[n], PosH_K = Y[n];

[0073] When X[0] < abs_temp < X[n], perform one-dimensional interpolation calculation, which includes the following steps:

[0074] When abs_temp ∈ [X[0], X[1]],

[0075] Then PosH_K = ((Y[1] - Y[0]) / (X[1] - X[0])) * (abs_temp - X[0]) + Y[0];

[0076] When abs_temp ∈ [X[1], X[2]],

[0077] Then PosH_K = ((Y[2] - Y[1]) / (X[2] - X[1])) * (abs_temp - X[1]) + Y[1];

[0078] When abs_temp ∈ [X[2], X[3]],

[0079] PosH_K = ((Y[3] - Y[2]) / (X[3] - X[2])) * (abs_temp - X[2]) + Y[2]; ...

[0081] When abs_temp ∈ [X[n - 1], X[n]],

[0082] Then PosH_K = ((Y[n] - Y[n - 1]) / (X[n] - X[n - 1])) * (abs_temp - X[n - 1]) + Y[n - 1];

[0083] Where n is a positive integer, X[0], X[1], X[2], X[3]... X[n] is an error array of the absolute value of the pan-tilt position angle error abs_temp, Y[0], Y[1], Y[2], Y[3]... Y[n] is a coefficient array, X[0], X[1], X[2], X[3]... X[n], Y[0], Y[1], Y[2], Y[3]... Y[n] are all set constants, which can be adjusted according to the actual situation, and X[0] < X[1] < X[2] < X[3]... X[n], Y[0] < Y[1] < Y[2] < Y[3]... Y[n], and at the same time Y[0], Y[1], Y[2], Y[3]... Y[n] are all greater than 0;

[0084] S333. Compare the position loop compensation parameter PosH_K with the pan-tilt following parameter NLSEF_K, and

[0085] If PosH_K>NLSEF_K, the value of the gimbal following parameter NLSEF_K satisfies NLSEF_K=PosH_K;

[0086] If PosH_K≤NLSEF_K, and the absolute value of the current gimbal position angle error abs_temp≥0.1, then NLSEF_K(T+1)=(0.97+0.0027*NLSEF_K(T))*NLSEF_K(T)+(0.03-0.0027*NLSEF_K(T))*PosH_K(T), where,

[0087] T is the variable per unit time, representing the current moment, and T+1 represents the next moment;

[0088] If PosH_K≤NLSEF_K, and the absolute value of the current gimbal position angle error abs_temp<0.1, the value of the gimbal following parameter NLSEF_K satisfies NLSEF_K=PosH_K;

[0089] And S4, generating and outputting a pan-tilt motor control signal according to the pan-tilt motor speed loop PID controller and the position loop PID controller;

[0090] The specific steps include:

[0091] The position loop PID controller outputs the output value of the position loop PID controller according to the following formula:

[0092]

[0093] Among them, Kp=NLSEF_K, Ki and Kd are the parameters related to the position loop PID controller;

[0094] The speed loop PID controller outputs the speed loop PID controller output value Output' according to the following formula:

[0095]

[0096] Among them, Kp', Ki', and Kd' are the parameters related to the speed loop PID controller; Err' is the deviation between the output of the position loop PID controller and the azimuth angular velocity obtained by the azimuth velocity sensor inside the gimbal;

[0097] The gimbal motor adjusts its rotational position based on the output of the position loop PID controller (Output) and its speed based on the output of the speed loop PID controller (Output') to adjust the gimbal to the target position, such as adjusting it so that the gimbal camera's view is facing directly in front of the drone's flight path.

[0098] Figure 2 That is, it shows the change trend of the gimbal following parameter NLSEF_K over time when PosH_K changes from 10 to 0.2. It can be seen that when the absolute value of the gimbal position angle error abs_temp increases, NLSEF_K can immediately increase to the required value. When the absolute value of the gimbal position angle error abs_temp decreases, NLSEF_K decays over time and slowly decreases to the required value, thereby realizing a unidirectional hysteresis of NLSEF_K in the direction of decreasing the absolute value of the gimbal position angle error abs_temp, so that the weak following characteristics of the gimbal control can be maintained in the steady-state process of small angles, and the convergence characteristics are accelerated at large angles, so that the control process converges faster, thereby improving the accuracy of the gimbal small-angle following and further solving the problem of the image drifting in the horizontal direction.

[0099] Example 2:

[0100] This embodiment provides a pan-tilt control system for implementing the pan-tilt control method in embodiment 1, such as Figure 3 As shown, it includes:

[0101] A pan / tilt position feedforward compensator 1 is configured to perform compensation calculations based on pan / tilt control parameter information to obtain a compensated pan / tilt azimuth angle. The specific steps are the same as those in step S2.

[0102] A pan-tilt unidirectional hysteresis variable parameter controller 2 is used to obtain pan-tilt following parameters for controlling the pan-tilt motor, and its specific steps are the same as step S3;

[0103] Speed ​​loop PID controller 3, which is used to output the output of the position loop PID controller;

[0104] Position loop PID controller 4, which is used to output the output value Output' of the speed loop PID controller;

[0105] The steps for obtaining the output value Output of the position loop PID controller and the output value Output′ of the speed loop PID controller are the same as those in step S4.

[0106] Example 3:

[0107] This embodiment provides a drone, which includes the pan-tilt control system in Example 2.

[0108] In summary, the present invention can perform compensation calculation on the gimbal azimuth angle according to the gimbal control parameter information, realize the advance prediction of the current motion state of the gimbal, and accelerate the gimbal azimuth angle response and following speed; at the same time, the one-way hysteresis variable parameter control algorithm is used to realize the one-way hysteresis of NLSEF_K in the direction of reducing the absolute value abs_temp of the gimbal position angle error, so that the weak following characteristic of the gimbal control can be maintained in the small-angle steady-state process, and the convergence characteristic is accelerated at large angles, so as to improve the accuracy of the gimbal small-angle following, and further solve the problem of the image drifting in the horizontal direction.

[0109] It should be noted that the technical features in the above-mentioned Examples 1-2 can be combined in any way, and the technical solutions formed by the combination all fall within the scope of protection of this application. Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the description and implementation methods. They can be applied to various fields suitable for the present invention. For those familiar with the art, additional modifications can be easily implemented. Therefore, without departing from the general concept defined by the claims and their equivalents, the present invention is not limited to the specific details and drawings shown and described herein.

Claims

1. A pan-tilt position control method, characterized in that: It includes the following steps: The pan-tilt obtains pan-tilt control parameter information from sensors and front-end devices; Perform compensation calculation according to the pan-tilt control parameter information to obtain the compensated pan-tilt azimuth angle; Obtain the pan-tilt following parameter for controlling the pan-tilt motor; Generate and output a pan-tilt motor control signal according to the pan-tilt motor speed loop PID controller and position loop PID controller; Performing compensation calculation according to the pan-tilt control parameter information to obtain the compensated pan-tilt azimuth angle includes the following steps: Obtain the compensated pan-tilt azimuth angle Err_Ture according to the following formula: Err_Ture = Err + PosH_K * Err_b Where, PosH_K is the position loop compensation parameter, which can be adjusted according to the actual tracking effect; Err is the current azimuth error angle of the pan-tilt, which can be obtained through sensors; Err_b is the azimuth control information output by the UAV controller / handheld pan-tilt controller; Obtaining the pan-tilt following parameter for controlling the pan-tilt motor includes the following steps: Calculate the absolute value of the pan-tilt position angle error abs_temp according to the formula abs_temp = fabs(target position A - position sensor feedback B); Calculate the position loop compensation parameter PosH_K according to the absolute value of the pan-tilt position angle error abs_temp; Compare the position loop compensation parameter PosH_K with the pan-tilt following parameter NLSEF_K, and If PosH_K > NLSEF_K, the value of the pan-tilt following parameter NLSEF_K satisfies NLSEF_K = PosH_K; If PosH_K ≤ NLSEF_K, and the absolute value of the current pan-tilt position angle error abs_temp ≥ 0.1, then NLSEF_K(T + 1) = (0.97 + 0.0027 * NLSEF_K(T)) * NLSEF_K(T) + (0.03 - 0027 * NLSEF_K(T)) * PosH_K(T), where, T is a variable of unit time, representing the current moment, and T + 1 represents the next moment; If PosH_K ≤ NLSEF_K, and the absolute value of the current pan-tilt position angle error abs_temp < 0.1, then the value of the pan-tilt following parameter NLSEF_K satisfies NLSEF_K = PosH_K.

2. The pan / tilt position control method according to claim 1, wherein: The pan-tilt control parameter information includes one or several of pan-tilt IMU data information, pan-tilt azimuth angle, azimuth control information output by the UAV controller / handheld pan-tilt controller, heading speed, etc.

3. The pan / tilt position control method according to claim 1, wherein: The pan-tilt can be mounted on a UAV or the pan-tilt is a handheld pan-tilt.

4. The pan / tilt position control method according to claim 1, wherein: Calculating the position loop compensation parameter PosH_K according to the absolute value of the pan-tilt position angle error abs_temp includes the following steps: [[ID=二十一]]When abs_temp < X[0], PosH_K = Y[0]; [[ID=二十二]]When abs_temp ≥ X[n], PosH_K = Y[n]; [[ID=二十三]]When X[0] < abs_temp < X[n], perform one-dimensional interpolation calculation, which includes the following steps: [[ID=二十四]]When abs_temp ∈ [X[0], X[1]], Then PosH_K = ((Y[1] - Y[0]) / (X[1] - X[0])) * (abs_temp - X[0]) + Y[0]; When abs_temp ∈ [X[1], X[2]], Then PosH_K = ((Y[2] - Y[1]) / (X[2] - X[1])) * (abs_temp - X[1]) + Y[1]; When abs_temp ∈ [X[2], X[3]], PosH_K = ((Y[3] - Y[2]) / (X[3] - X[2])) * (abs_temp - X[2]) + Y[2]; ... When abs_temp ∈ [X[n - 1], X[n]], Then PosH_K = ((Y[n] - Y[n - 1]) / (X[n] - X[n - 1])) * (abs_temp - X[n - 1]) + Y[n - 1]; Where n is a positive integer, X[0], X[1], X[2], X[3]... X[n] is an error array of the absolute value abs_temp of the pan - tilt position angle error, Y[0], Y[1], Y[2], Y[3]... Y[n] is a coefficient array, X[0], X[1], X[2], X[3]... X[n], Y[0], Y[1], Y[2], Y[3]... Y[n] are all constants, and X[0] < X[1] < X[2] < X[3]... X[n], Y[0] < Y[1] < Y[2] < Y[3]... Y[n], and at the same time Y[0], Y[1], Y[2], Y[3]... Y[n] are all greater than 0.

5. The pan / tilt position control method according to claim 1, wherein: Generating and outputting a pan - tilt motor control signal according to the pan - tilt motor speed - loop PID controller and the position - loop PID controller includes the following steps: The position - loop PID controller outputs the output quantity Output of the position - loop PID controller according to the following formula: Where, Kp = NLSEF_K, Ki and Kd are parameters related to the position - loop PID controller.

6. The pan / tilt position control method according to claim 5, wherein: Generating and outputting a pan - tilt motor control signal according to the pan - tilt motor speed - loop PID controller and the position - loop PID controller includes the following steps: The speed - loop PID controller outputs the output quantity Output' of the speed - loop PID controller according to the following formula: Where, Kp', Ki', Kd' are parameters related to the speed - loop PID controller; Err' is the deviation value between the output quantity Output of the position - loop PID controller and the azimuth angular velocity obtained by the azimuth angular velocity sensor inside the pan - tilt.

7. A pan-tilt control system for implementing the pan-tilt position control method described in any one of embodiments 1-6, characterized in that: Including: A pan - tilt position feed - forward compensator, which is used to perform compensation calculations according to the pan - tilt control parameter information to obtain the compensated pan - tilt azimuth angle; A pan - tilt unidirectional hysteresis variable - parameter controller, which is used to obtain the pan - tilt following parameters for controlling the pan - tilt motor; A speed - loop PID controller, which is used to output the output quantity of the position - loop PID controller; A position - loop PID controller, which is used to output the output quantity of the speed - loop PID controller.

8. A drone, characterized in that: Including the pan - tilt control system according to claim 7.

Images