A multi-rotor unmanned aerial vehicle ground target tracking control method and device

Abstract

The present invention relates to a multi-rotor unmanned aerial vehicle ground target tracking control method, comprising the following steps: calculating the ground target position, using the imaging principle of a camera and the positional relationship between the unmanned aerial vehicle and the ground target to solve the specific position coordinates of the ground target; multi-rotor unmanned aerial vehicle trajectory planning, using a method based on geometric constraints to calculate the expected flight trajectory of the multi-rotor unmanned aerial vehicle tracking the ground target; multi-rotor unmanned aerial vehicle trajectory tracking, designing a PID controller for the multi-rotor unmanned aerial vehicle tracking the expected flight trajectory, including the position, speed, attitude angle, and attitude angular velocity controller of the multi-rotor unmanned aerial vehicle; camera attitude control, using a sliding mode control method based on an extended state observer with strong anti-disturbance performance to control the camera's attitude angle, so that the ground target is always located at the center of the image. The method has relatively excellent anti-disturbance performance, and its robustness is stronger than that of a general conventional control system.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic control of unmanned aerial vehicles, and in particular relates to a method, device, electronic equipment and storage medium for tracking and controlling ground targets of a multi-rotor unmanned aerial vehicle. Background Art

[0002] Target recognition, positioning and tracking are the most commonly used applications of drones. After obtaining image information of the environment through the camera equipment, the drone can identify the target in the image, and the positioning, tracking and approaching of the target can complete tasks such as close reconnaissance, signal interference, and precision strikes. This is a very important means of intelligent unmanned combat in the military field that tends to be intelligent. Therefore, the target tracking and control technology of multi-rotor drones has important practical and research value. Target tracking includes four processes: target positioning after target recognition, drone trajectory planning, drone trajectory tracking control, and camera attitude control. In the actual environment, most targets are dynamically moving, and drones, cameras, and targets are all in a non-static state. The viewing angle limitation of the camera equipment will cause the target tracked by the drone to be easily lost. Therefore, how to keep the dynamic target always within the image range of the camera equipment is one of the difficulties in target tracking research. Summary of the invention

[0003] To solve the problem of ground target tracking for multi-rotor UAV equipped with camera, a ground target tracking method with high tracking accuracy based on geometric constraints and camera attitude adjustment is used. At the same time, a camera attitude control algorithm is designed using a method with strong anti-disturbance performance, so that the head of the multi-rotor UAV is always facing the ground target and the ground target is always located in the center of the image during the UAV tracking process.

[0004] The present invention provides a multi-rotor unmanned aerial vehicle ground target tracking control method, comprising the following steps:

[0005] Calculate the position of the ground target, and use the imaging principle of the camera and the positional relationship between the drone and the ground target to solve the specific position coordinates of the ground target;

[0006] Multi-rotor UAV trajectory planning, using a geometric constraint-based approach to calculate the desired flight trajectory of a multi-rotor UAV tracking a ground target;

[0007] Multi-rotor UAV trajectory tracking, design PID controller for multi-rotor UAV to track the expected flight trajectory, including position, speed, attitude angle, and attitude angular velocity controller of multi-rotor UAV;

[0008] The camera's attitude control uses a sliding mode control method based on an extended state observer with strong anti-disturbance performance to control the camera's attitude angle so that the ground target is always located in the center of the image.

[0009] Furthermore, the specific method for calculating the target position is as follows:

[0010] Establish the kinematic model of the entire system of drone, camera, and ground target.

[0011] Define the drone coordinate system: take the center of gravity of the multi-rotor drone as the origin b Oh, b O b The X-axis points to the direction of the nose of the drone in the symmetry plane. b O b The Z axis is perpendicular to the symmetry plane of the drone. b O b X down, then confirm by right hand rule b O b Y axis;

[0012] Define the camera coordinate system: take the camera center of gravity as the origin c Oh, c O c The X axis points to the direction of the lens in the camera's symmetry plane. c O c The Y axis is perpendicular to the camera in the symmetry plane. c O c X down, then confirm by right hand rule c O c Y axis; the camera is fixed under the multi-rotor drone;

[0013] Define the ground target coordinate system: take any fixed point on the ground as the origin s O, the take-off position of the drone can be selected as the origin, s O s The X-axis points in a certain direction in the horizontal plane. s O s The Z axis is perpendicular to the ground and downward, and then determined by the right-hand rule s O s Y axis;

[0014] From the geometric relationship, we can know that the three-dimensional position vector of the dynamic target in the ground target coordinate system is It can be expressed as follows

[0015] s p0= s p1+ s p2+ s p3 (1)

[0016] Among them, the vector is the position vector of the multi-rotor UAV in the ground target coordinate system. The position and attitude information of the UAV are directly obtained by the integrated navigation system of the UAV. is the distance from the center of mass of the drone to the optical center of the camera, which can be expressed as follows

[0017]

[0018] in is the offset between the camera coordinate system and the drone coordinate system. Since the camera is fixed under the multi-rotor drone, b p2 can be obtained in advance through calibration and is a known vector; is the transformation matrix between the UAV coordinate system and the ground target coordinate system, expressed as

[0019]

[0020] Among them, θ, φ, and ψ are the pitch angle, roll angle, and yaw angle of the multi-rotor drone, respectively;

[0021] vector Represents the distance from the camera to the target, which can be expressed as

[0022]

[0023] in is the coordinate representation of the ground target in the camera coordinate system, which is calculated using the image information obtained by the camera. is the transformation matrix from the camera coordinate system to the drone coordinate system, expressed as

[0024]

[0025] Where α is the horizontal rotation angle of the camera, and β is the pitch angle. These two angle information are measured by the encoder of the camera pan / tilt. Substituting equations (2) and (4) into equation (1), we get

[0026]

[0027] Calculate the coordinates of the target under the camera according to the imaging principle of the camera c p3=[ c x3 c y3 c z3] T , where the coordinates c x3, c y3, c z3 satisfies the following relationship

[0028]

[0029] Where f is the focal length of the camera, [u c v c ] Tis the image pixel position coordinate of the ground target in the camera image plane, where u c is the horizontal coordinate, v c is the vertical coordinate, which is obtained by image processing method based on the image of the camera; the position of the ground target is set to s p0=[x0 y0 0] T , then according to formula (6) and formula (7) we can get

[0030]

[0031] The right side of formula (8) contains known quantities measured or calculated by sensors, and the left side contains three unknown quantities x0, y0, c z3, are obtained by solving equation (8)

[0032]

[0033]

[0034]

[0035] Where R ij Representation Matrix The i-th row and j-th column element, p k Representation vector The kth element of , equations (10) and (11) can calculate the position of the ground target s p0=[x0 y0 0] T .

[0036] Furthermore, the specific method of multi-rotor UAV trajectory planning is as follows:

[0037] First, two constraints are set for the trajectory planning of the multi-rotor UAV.

[0038] The first constraint: During the tracking process, the yaw angle of the multi-rotor drone needs to be constantly adjusted, that is, the horizontal rotation angle α of the camera is 0, so that the nose of the drone faces the target in real time;

[0039] Second constraint: The horizontal distance between the multirotor drone and the target should be close to the flight altitude;

[0040] Under these two constraints,

[0041] Assume that the position vector of the multi-rotor drone in the ground coordinate system at time k-1 is s p1 and the yaw angle is [x k-1 y k-1 z k-1 ψ k-1 ] T, whose values ​​are obtained by the inertial integrated navigation on the UAV; assuming that the position of the target at time k is (x 0,k y 0,k );

[0042] The frequent changes in the height of a multi-rotor drone are of little significance. At the same time, in order to facilitate the tracking and control of the drone, the flight height of the drone is designed to remain unchanged. Therefore, at time k,

[0043]

[0044] Where h is the pre-set flight altitude of the drone; the position of the target at time k is (x 0,k y 0,k ), according to the second constraint, the horizontal distance between the multi-rotor UAV and the target should be close to the flight altitude, that is,

[0045]

[0046] Therefore, at time k, the drone should be at the target position (x 0,k y 0,k ) is the center of the circle and the radius is h; on this circle, the current position of the drone (x k-1 y k-1 ) should be the point closest to (x 0,k y 0,k ) and (x k-1 y k-1 ) and the circumference, which is the expected position of the drone at time k. This point satisfies the following relationship

[0047]

[0048] The expected position of the UAV at time k can be solved by equations (12) and (13): for

[0049]

[0050]

[0051] According to the first constraint, the nose of the UAV needs to face the target in real time, and the expected yaw angle at time k is designed to be

[0052]

[0053] Combining equations (12), (15), (16), and (17), we can obtain the expected flight trajectory l of the multi-rotor drone at time k. ref =[x r y r zr ψ r ] T .

[0054] Furthermore, the specific method of multi-rotor drone trajectory tracking is as follows:

[0055] After obtaining the desired flight trajectory of the UAV, the PID control method is used to design the controller of the multi-rotor UAV, including position, speed, attitude angle, and attitude angular velocity controllers, so that the multi-rotor UAV can fly according to the desired trajectory;

[0056] First, establish the dynamic equation of the multi-rotor drone

[0057]

[0058] in s p1=[xyz] T is the position vector of the multi-rotor UAV in the ground coordinate system; ω x ,ω y ,ω z are the rolling angular velocity, pitch angular velocity, and yaw angular velocity of the UAV respectively; I x ,I y ,I z are the moments of inertia of the roll axis, pitch axis, and yaw axis respectively; K1, K2, and K3 are the air resistance related coefficients; U1, U2, U3, and U4 are the control quantities of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor drone respectively, and m is the total mass of the drone; represents the first-order derivative of , Represents the second-order derivative of , and the black dot “·” represents the or The corresponding symbols below; represents the first-order derivative of the observed value of , represents the observed value of the first-order derivative of ·, and the black dot “·” represents the or Corresponding symbols below; Position controller: The control law of position control is designed using the PID PI control method, as shown below

[0059]

[0060] in K is the expected speed of the UAV in the north, east and ground directions obtained by the position controller. ij (i=P, I, j=x, y, z) are the proportional and integral parameters of the three directions in the position controller, and the parameters are adjustable.

[0061] Speed ​​controller: The speed control law is designed using the PID P control method, as shown below:

[0062]

[0063] in v is the expected acceleration of the UAV in the north, east and ground directions obtained by the velocity controller; x 、v y 、v z K is the actual measured speed of the UAV in the north, east and ground directions; Vx , K Vy , K Vz It is the proportional parameter of the three directions in the speed controller, and the parameter is adjustable.

[0064] Then, according to the expected acceleration command, the longitudinal throttle control amount U1 and the expected pitch angle and roll angle are solved. The control forces in the three directions required inside the drone are as follows:

[0065]

[0066] According to the UAV dynamics equation (23),

[0067]

[0068] Combining equations (26) and (27), we can obtain the longitudinal throttle control value U1 and the desired pitch angle and roll angle respectively:

[0069]

[0070]

[0071]

[0072] Attitude angle controller: The desired attitude angle is obtained by equations (29), (30), and (17). The control law of attitude angle control is designed by using the PID P control method as follows:

[0073]

[0074] in is the desired roll, pitch, and yaw angular velocity; K ωθ , K ωφ , K ωψ It is the proportional parameter of the three postures in the attitude angle controller.

[0075] Attitude angular velocity controller: The PID control method is used to design the control law of attitude angular velocity control, as shown in the following formula

[0076]

[0077] in is the expected roll, pitch, and yaw acceleration; K i,j (i=P, I, D, j=ωφ, ωθ, ωψ) are the proportional, integral and differential parameters of the three attitudes in the attitude angular velocity controller.

[0078] So the control quantities U2, U3, and U4 of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor drone are

[0079]

[0080] The combined formulas (28), (32), and (33) are the control variables of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor UAV.

[0081] Furthermore, the specific method of the camera posture control is as follows:

[0082] Since the camera is prone to shaking when it is fixed on the drone, a sliding mode control method based on an extended state observer with strong anti-disturbance ability is used to control the movement of the motor; the camera is fixed on the gimbal under the multi-rotor drone, and the gimbal drives it to rotate horizontally and in pitch, and the horizontal and pitch rotations of the gimbal are controlled by two motors, so the attitude control of the camera is converted into the angle control of the motor; since the camera is prone to shaking when it is fixed on the drone, a sliding mode control method based on an extended state observer with strong anti-disturbance ability is used to control the movement of the motor.

[0083] Specifically, the method of controlling the motor for the pitch rotation of the camera using the sliding mode control method is as follows:

[0084] First, establish the dynamic equation of the motor as shown below

[0085]

[0086] where θ β ,y β The angle of the motor shaft that controls the pitch rotation is measured by the motor encoder; ω β is the motor shaft angular velocity, measured by the motor encoder; J β is the equivalent moment of inertia of the load and the motor shaft; d β (t) is the load torque T L , load friction torque T on the motor shaft f , the sum of internal and external disturbances Δ; K β is the motor torque coefficient; u β The control quantity of the motor, the small black dot on the symbol head Indicates the first-order derivative; the disturbance d β (t) Expand to the third state, let the state vector x β =[xβ1 x β2 x β3 ] T =[θ β ω β d β (t)] T , formula (34) is expanded to

[0087]

[0088] where b β =K β / J β are known parameters; the extended state observer is designed as

[0089]

[0090] in represents the observed value of state ·, and the black dot “·” represents the corresponding symbol under “^” in the formula; Represents the first-order derivative of the state observation; σ βi (i=1, 2, 3) are observation parameters, ε β is a high gain parameter, all parameters are adjustable and are positive real numbers. This observer can be used to implement the polynomial s 3 +σ β1 s 2 +σ β2 s+σ β3 When the Hurwitz condition is satisfied, when time t→∞, That is, the observed value can gradually approach the true value;

[0091] Then design the sliding mode controller, set the control error in The desired camera pitch angle (45°) is converted into the desired motor shaft rotation angle. For the controlled object (34), the sliding surface is designed as

[0092]

[0093] Where c>0; the sliding mode controller based on the extended state observer is designed as

[0094]

[0095] where k β is the feedback parameter, which is a positive real number; the Lyapunov function of the sliding mode control is taken as Combining equations (35)-(38), we can find their differentials:

[0096]

[0097]

[0098] Where Δ max is a positive real number and satisfies represents the state observation error, that is The black dot “·” represents the corresponding symbol below the “~” in the formula, and the small black dot above the symbol Indicates the first-order derivative, with two small black dots on the symbol head represents the second-order derivative; from formula (39), we can get Then there is

[0099]

[0100] Therefore, when t→∞, The convergence speed depends on the feedback parameter k β and high gain parameter ε β ; Equation (36) and Equation (38) are the final extended state observer and control law;

[0101] Similarly, the motor that controls the horizontal rotation of the camera uses the sliding mode control method to control its motion in the same way as the motor that controls the pitch rotation of the camera. The extended state observer and control law are designed as follows:

[0102]

[0103]

[0104] where x α =[x α1 x α2 x α3 ] T =[θ α ω α d α (t)] T is the state vector, θ α ,y α To control the horizontal rotation angle of the motor shaft, it can be measured by the motor encoder; d β (t) is the sum of load torque, load friction torque on the motor shaft, and internal and external disturbances; b α =K α / J α is the ratio of the motor rotation coefficient to the moment of inertia, which is a known quantity; u α is the control quantity of the motor; σ αi (i=1, 2, 3) are observation parameters, ε α is a high gain parameter, all parameters are adjustable and are positive real numbers; k α is the feedback parameter, which is a positive real number; The desired camera horizontal rotation angle (0°) is converted into the desired motor shaft angle; adjustable parameter c α >0; control error Sliding surface represents the first-order derivative of , Represents the second-order derivative of , and the black dot “·” represents the or The corresponding symbols below; represents the first-order derivative of the observed value of , represents the observed value of the first-order derivative of ·, and the black dot “·” represents the or The corresponding symbol below, the small black dot above the symbol Indicates the first-order derivative, with two small black dots on the symbol head It means to find the second-order derivative.

[0105] The present invention also provides a multi-rotor UAV ground target tracking control device, comprising:

[0106] The ground target position calculation module is used to solve the specific position coordinates of the ground target by using the imaging principle of the camera and the position relationship between the UAV and the ground target;

[0107] The multi-rotor UAV trajectory planning module is used to calculate the expected flight trajectory of the multi-rotor UAV tracking the ground target using a geometric constraint-based method;

[0108] The multi-rotor UAV trajectory tracking module is used to track the desired flight trajectory of the multi-rotor UAV and design a PID controller, including the position, speed, attitude angle, and attitude angular velocity controller of the multi-rotor UAV;

[0109] The camera's attitude control module is used to control the camera's attitude angle using a sliding mode control method based on an extended state observer with strong anti-disturbance performance, so that the ground target is always located in the center of the image.

[0110] The present invention also provides an electronic device, comprising: a processor;

[0111] A memory, communicatively connected to the processor;

[0112] At least one program is stored in the memory and configured to be executed by the processor, and the at least one program is configured to: implement the multi-rotor unmanned aerial vehicle ground target tracking control method.

[0113] The present invention also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by an electronic device, the multi-rotor unmanned aerial vehicle ground target tracking control method is implemented.

[0114] The multi-rotor unmanned aerial vehicle ground target tracking control method and device of the present invention aims at the problem that the target is easily lost when the multi-rotor unmanned aerial vehicle equipped with a camera tracks the ground target. First, the specific position coordinates of the ground target are solved by using the imaging principle of the camera and the positional relationship between the unmanned aerial vehicle and the target. Then, a method based on geometric constraints is used to calculate the expected flight trajectory of the multi-rotor unmanned aerial vehicle tracking the ground target. Subsequently, a PID controller is designed to solve the problem of the quad-rotor unmanned aerial vehicle tracking the expected flight trajectory, including position, speed, attitude angle, and attitude angular velocity controllers. Finally, a sliding mode control method based on an extended state observer with strong anti-disturbance performance is used to control the attitude angle of the camera, so that the target is always located at the center of the image, thereby improving the stability and robustness of the system.

[0115] In summary, compared with the prior art, the present invention has the following beneficial effects:

[0116] (1) The present invention uses the imaging principle of the camera and the positional relationship between the drone and the target to solve the specific position coordinates of the ground target, without the need to carry other instruments and equipment to measure the target position coordinates;

[0117] (2) The present invention uses a method based on geometric constraints to calculate the expected flight trajectory of a quadrotor drone tracking a ground target. The calculation method is simple, easy to operate, and has high real-time performance.

[0118] (3) The present invention uses the PID method to design the position, speed, attitude angle, and attitude angular velocity controllers of the quadrotor drone, which can enable the drone to accurately track the desired flight trajectory and ensure that the drone will not lose the target;

[0119] (4) The present invention designs a sliding mode controller based on an extended state observer to control the posture of the camera. This method designs the control law by observing the disturbance value to compensate for the impact of the disturbance on the system. Therefore, this method has excellent anti-disturbance performance and its robustness is stronger than that of general conventional control systems.

[0120] (5) The Lyapunov method is used to prove the convergence and stability of the camera control system, and no oscillation or divergence will occur. BRIEF DESCRIPTION OF THE DRAWINGS

[0121] Figure 1 It is a schematic diagram of the process of the present invention;

[0122] Figure 2 A coordinate relationship diagram of the quadrotor aircraft, camera, and ground target of the present invention;

[0123] Figure 3 The figure is a diagram showing the relationship between the positions of the quadrotor aircraft and the target from a top-down perspective of the present invention;

[0124] Figure 4 is a diagram showing the relationship between the positions of the quadrotor aircraft and the target from a side view perspective of the present invention;

[0125] Figure 5 A schematic diagram of the desired trajectory planning of the UAV of the present invention;

[0126] Figure 6 It is a block diagram of the desired trajectory tracking of the quadrotor drone of the present invention. DETAILED DESCRIPTION

[0127] like Figure 1 As shown, the present invention provides a ground target tracking control method for a multi-rotor UAV, specifically a quad-rotor UAV, comprising the following steps:

[0128] Calculate the position of the ground target, and use the imaging principle of the camera and the positional relationship between the drone and the ground target to solve the specific position coordinates of the ground target;

[0129] Before performing trajectory planning and tracking and camera attitude control on a quadrotor drone, the position information of the ground target must be determined. First, a kinematic model of the entire system of drone, camera, and ground target is established.

[0130] Define the drone coordinate system: take the center of gravity of the multi-rotor drone as the origin b Oh, b O b The X-axis points to the direction of the nose of the drone in the symmetry plane. b O b The Z axis is perpendicular to the symmetry plane of the drone. b O b X down, then confirm by right hand rule b O b Y axis;

[0131] Define the camera coordinate system: take the camera center of gravity as the origin c Oh, c O c The X axis points to the direction of the lens in the camera's symmetry plane. c O c The Y axis is perpendicular to the camera in the symmetry plane. c O c X down, then confirm by right hand rule c O c Y axis; the camera is fixed under the multi-rotor drone;

[0132] Define the ground target coordinate system: take any fixed point on the ground as the origin s O, the take-off position of the drone can be selected as the origin, s Os The X-axis points in a certain direction in the horizontal plane. s O s The Z axis is perpendicular to the ground and downward, and then determined by the right-hand rule s O s Y axis;

[0133] The coordinate systems of drones, cameras, and ground targets are as follows: Figure 2 As shown, the one marked with s in the upper left corner is the ground coordinate system, the one marked with b in the upper left corner is the UAV body coordinate system, and the one marked with c in the upper left corner is the camera coordinate system. The camera is fixedly connected to the bottom of the quadrotor UAV.

[0134] From the geometric relationship, we can know that the three-dimensional position vector of the dynamic target in the ground target coordinate system is It can be expressed as follows

[0135] s p0= s p1+ s p2+ s p3 (1)

[0136] Among them, the vector is the position vector of the multi-rotor UAV in the ground target coordinate system. The position and attitude information of the UAV are directly obtained by the integrated navigation system of the UAV. is the distance from the center of mass of the drone to the optical center of the camera, which can be expressed as follows

[0137]

[0138] in is the offset between the camera coordinate system and the drone coordinate system. Since the camera is fixed under the multi-rotor drone, b p2 can be obtained in advance through calibration and is a known vector; is the transformation matrix between the UAV coordinate system and the ground target coordinate system, expressed as

[0139]

[0140] Among them, θ, φ, and ψ are the pitch angle, roll angle, and yaw angle of the multi-rotor drone, respectively;

[0141] vector Represents the distance from the camera to the target, which can be expressed as

[0142]

[0143] in is the coordinate representation of the ground target in the camera coordinate system, which is calculated using the image information obtained by the camera. is the transformation matrix from the camera coordinate system to the drone coordinate system, expressed as

[0144]

[0145] Where α is the horizontal rotation angle of the camera, and β is the pitch angle. These two angle information are measured by the encoder of the camera pan / tilt. Substituting equations (2) and (4) into equation (1), we get

[0146]

[0147] Calculate the coordinates of the target under the camera according to the imaging principle of the camera c p3=[ c x3 c y3 c z3] T , where the coordinates c x3, c y3, c z3 satisfies the following relationship

[0148]

[0149] Where f is the focal length of the camera, [u c v c ] T is the image pixel position coordinate of the ground target in the camera image plane, where u c is the horizontal coordinate, v c is the vertical coordinate, which is obtained by image processing method based on the image of the camera; the position of the ground target is set to s p0=[x0 y0 0] T , then according to formula (6) and formula (7) we can get

[0150]

[0151] The right side of formula (8) contains known quantities measured or calculated by sensors, and the left side contains three unknown quantities x0, y0, c z3, are obtained by solving equation (8)

[0152]

[0153]

[0154]

[0155] Where R ij Representation Matrix The i-th row and j-th column element, p k Representation vector The kth element of , equations (10) and (11) can calculate the position of the ground target s p0=[x0 y0 0] T .

[0156] Multi-rotor UAV trajectory planning, using a geometric constraint-based approach to calculate the desired flight trajectory of a multi-rotor UAV tracking a ground target;

[0157] First, we analyze the angle and position relationship between the drone, camera, and target from a bird's-eye view. The simplified bird's-eye view is as follows: Figure 3 The yaw angle ψ is the direction of the drone head relative to the ground coordinate system. s The angle of the X axis, the horizontal rotation angle α is the angle between the camera optical axis and the direction of the drone head. s XO- s The line segment between the projection (x, y) on the Y plane and the target coordinate (x0, y0) is denoted as d, then d is s The angle ψ of the X axis d for

[0158]

[0159] The length of line segment d is the horizontal distance between the drone and the ground target.

[0160]

[0161] Depend on Figure 3 It can be seen that when the camera is facing the ground target, the yaw angle ψ of the drone, the horizontal rotation angle α of the camera, and ψ d There is the following relationship

[0162] ψ d =ψ+α (14-1)

[0163] When the yaw angle of the drone ψ=ψ d That is, when the horizontal rotation angle of the camera α = 0, the drone head and the camera can face the target. Since the horizontal rotation angle α ranges from -180° to 180°, when the drone head and the camera face the target, that is, α = 0, if the dynamic ground target suddenly deviates from the center of the image, the camera has enough angle margin to adjust the horizontal rotation angle to bring the dynamic target back to the center of the image, avoiding the loss of the dynamic target.

[0164] Then, the angle and position relationship between the drone, camera, and target are analyzed from a side view. The simplified side view is shown in the figure below. Figure 4 The camera's pitch angle β is the angle between the camera's optical axis and s XO- sThe angle of the Y plane, z is the flight altitude of the drone. When the camera is facing the target, the relationship between the pitch angle β and the horizontal distance ||d|| is

[0165]

[0166] The pitch angle β ranges from 0 to 90°. When β = 45°, if a dynamic ground target suddenly deviates from the center of the image, the camera will have enough angle margin to adjust the pitch angle to bring the dynamic target back to the center of the image, thus avoiding the loss of the dynamic target. In order to make β = 45°, the horizontal distance ||d|| and the flight altitude z must satisfy

[0167] ||d||=z (16-1)

[0168] Based on the above analysis, we can get the following two constraints:

[0169] The first constraint: During the tracking process, the yaw angle of the quadrotor drone needs to be constantly adjusted, that is, the horizontal rotation angle α of the camera is 0, so that the nose of the drone faces the target in real time;

[0170] Second constraint: The horizontal distance between the quadrotor drone and the target should be close to the flight altitude.

[0171] After obtaining the constraints, a method based on geometric constraints can be used to calculate the expected flight trajectory of the quadrotor drone. ref =[x r y r z r ψ r ] T .

[0172] Under these two constraints,

[0173] Assume that the position vector of the multi-rotor drone in the ground coordinate system at time k-1 is s p1 and the yaw angle is [x k-1 y k-1 z k-1 ψ k-1 ] T , whose values ​​are obtained by the inertial integrated navigation on the UAV; assuming that the position of the target at time k is (x 0,k y 0,k );

[0174] The frequent changes in the height of a multi-rotor drone are of little significance. At the same time, in order to facilitate the tracking and control of the drone, the flight height of the drone is designed to remain unchanged. Therefore, at time k,

[0175]

[0176] Where h is the pre-set flight altitude of the drone; the position of the target at time k is (x 0,k y 0,k ), according to the second constraint, the horizontal distance between the multi-rotor UAV and the target should be close to the flight altitude, that is,

[0177]

[0178] Therefore, at time k, the drone should be at the target position (x 0,k y 0,k ) is the center of the circle and the radius is h; on this circle, the current position of the drone (x k-1 y k-1 ) should be the point closest to (x 0,k y 0,k ) and (x k-1 y k-1 ) and the circumference, which is the expected position of the drone at time k. This point satisfies the following relationship

[0179]

[0180] The expected position of the UAV at time k can be solved by equations (12) and (13): for

[0181]

[0182]

[0183] According to the first constraint, the nose of the UAV needs to face the target in real time, and the expected yaw angle at time k is designed to be

[0184]

[0185] Combining equations (12), (15), (16), and (17), we can obtain the expected flight trajectory l of the multi-rotor drone at time k. ref =[x r y r z r ψ r ] T .

[0186] Multi-rotor UAV trajectory tracking, design PID controller for multi-rotor UAV to track the expected flight trajectory, including position, speed, attitude angle, and attitude angular velocity controller of multi-rotor UAV;

[0187] The specific method of multi-rotor UAV trajectory tracking is as follows:

[0188] After obtaining the desired flight trajectory of the UAV, the PID control method is used to design the controller of the multi-rotor UAV, including position, speed, attitude angle, and attitude angular velocity controllers, so that the multi-rotor UAV can fly according to the desired trajectory;

[0189] First, establish the dynamic equation of the multi-rotor drone

[0190]

[0191] in s p1=[xyz] T is the position vector of the multi-rotor UAV in the ground coordinate system; ω x ,ω y ,ω z are the rolling angular velocity, pitch angular velocity, and yaw angular velocity of the UAV respectively; I x ,I y ,I z are the moments of inertia of the roll axis, pitch axis, and yaw axis respectively; K1, K2, and K3 are the air resistance related coefficients; U1, U2, U3, and U4 are the control quantities of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor drone respectively, and m is the total mass of the drone; represents the first-order derivative of , Represents the second-order derivative of , and the black dot “·” represents the or The corresponding symbols below; represents the first-order derivative of the observed value of , represents the observed value of the first-order derivative of ·, and the black dot “·” represents the or The corresponding symbols below;

[0192] Position controller: The control law of position control is designed using the PID PI control method, as shown below:

[0193]

[0194] in K is the expected speed of the UAV in the north, east and ground directions obtained by the position controller. ij (i=P, I, j=x, y, z) are the proportional and integral parameters of the three directions in the position controller, and the parameters are adjustable.

[0195] Speed ​​controller: The speed control law is designed using the PID P control method, as shown below:

[0196]

[0197] in v is the expected acceleration of the UAV in the north, east and ground directions obtained by the velocity controller; x 、v y 、v z K is the actual measured speed of the UAV in the north, east and ground directions; Vx , K Vy , K Vz It is the proportional parameter of the three directions in the speed controller, and the parameter is adjustable.

[0198] Then, according to the expected acceleration command, the longitudinal throttle control amount U1 and the expected pitch angle and roll angle are solved. The control forces in the three directions required inside the drone are as follows:

[0199]

[0200] According to the UAV dynamics equation (23),

[0201]

[0202] Combining equations (26) and (27), we can obtain the longitudinal throttle control value U1 and the desired pitch angle and roll angle respectively:

[0203]

[0204]

[0205]

[0206] Attitude angle controller: The desired attitude angle is obtained by equations (29), (30), and (17). The control law of attitude angle control is designed by using the PID P control method as follows:

[0207]

[0208] in is the desired roll, pitch, and yaw angular velocity; K ωθ , K ωφ , K ωψ It is the proportional parameter of the three postures in the attitude angle controller.

[0209] Attitude angular velocity controller: The PID control method is used to design the control law of attitude angular velocity control, as shown in the following formula

[0210]

[0211] in is the expected roll, pitch, and yaw acceleration; K i,j (i=P, I, D, j=ωφ, ωθ, ωψ) are the proportional, integral and differential parameters of the three attitudes in the attitude angular velocity controller.

[0212] So the control quantities U2, U3, and U4 of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor drone are

[0213]

[0214] The combined formulas (28), (32), and (33) are the control variables of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor UAV.

[0215] The camera attitude control uses the sliding mode control method based on the extended state observer with strong anti-disturbance performance to control the camera attitude angle so that the ground target is always located in the center of the image;

[0216] The specific method of the camera posture control is as follows:

[0217] From the analysis of the trajectory planning steps of the multi-rotor drone, it can be seen that the horizontal rotation angle α of the camera needs to be controlled at 0°, and the pitch angle β needs to be controlled at about 45°, so that the target can always be located in the center of the image when the drone tracks a dynamic target. Since the camera is prone to shaking when it is fixed to the drone, a sliding mode control method based on an extended state observer with strong anti-disturbance capability is used to control the movement of the motor; the camera is fixed to the gimbal under the multi-rotor drone, and the gimbal drives it to rotate horizontally and in pitch, and the horizontal and pitch rotations of the gimbal are controlled by two motors, so the attitude control of the camera is converted into the angle control of the motor; since the camera is prone to shaking when it is fixed to the drone, a sliding mode control method based on an extended state observer with strong anti-disturbance capability is used to control the movement of the motor.

[0218] Specifically, the method of controlling the motor for the pitch rotation of the camera using the sliding mode control method is as follows:

[0219] First, establish the dynamic equation of the motor as shown below

[0220]

[0221] where θ β ,y β The angle of the motor shaft that controls the pitch rotation is measured by the motor encoder; ω β is the motor shaft angular velocity, measured by the motor encoder; J β is the equivalent moment of inertia of the load and the motor shaft; d β (t) is the load torque T L , load friction torque T on the motor shaft f , the sum of internal and external disturbances Δ; K β is the motor torque coefficient; u β The control quantity of the motor, the small black dot on the symbol head Indicates the first-order derivative; the disturbance d β (t) Expand to the third state, let the state vector x β =[x β1 x β2 x β3 ] T =[θ β ω β d β (t)] T , formula (34) is expanded to

[0222]

[0223] where b β =K β / J β are known parameters; the extended state observer is designed as

[0224]

[0225] in represents the observed value of state ·, and the black dot “·” represents the corresponding symbol under “^” in the formula; Represents the first-order derivative of the state observation; σ βi (i=1, 2, 3) are observation parameters, ε β is a high gain parameter, all parameters are adjustable and are positive real numbers. This observer can be used to implement the polynomial s 3 +σ β1 s 2 +σ β2 s+σ β3 When the Hurwitz condition is satisfied, when time t→∞, That is, the observed value can gradually approach the true value;

[0226] Then design the sliding mode controller, set the control error in The desired camera pitch angle (45°) is converted into the desired motor shaft rotation angle. For the controlled object (34), the sliding surface is designed as

[0227]

[0228] Where c>0; the sliding mode controller based on the extended state observer is designed as

[0229]

[0230] where k β is the feedback parameter, which is a positive real number; the Lyapunov function of the sliding mode control is taken as Combining equations (35)-(38), we can find their differentials:

[0231]

[0232]

[0233] Where Δ max is a positive real number and satisfies represents the state observation error, that is The black dot “·” represents the corresponding symbol below the “~” in the formula, and the small black dot above the symbol Indicates the first-order derivative, with two small black dots on the symbol head represents the second-order derivative; from formula (39), we can get Then there is

[0234]

[0235] Therefore, when t→∞, The convergence speed depends on the feedback parameter k β and high gain parameter ε β ; Equation (36) and Equation (38) are the final extended state observer and control law;

[0236] Similarly, the motor that controls the horizontal rotation of the camera uses the sliding mode control method to control its motion in the same way as the motor that controls the pitch rotation of the camera. The extended state observer and control law are designed as follows:

[0237]

[0238]

[0239] where x α =[x α1 x α2 x α3 ] T =[θ α ω α d α (t)] T is the state vector, θ α ,y α To control the horizontal rotation angle of the motor shaft, it can be measured by the motor encoder; d β (t) is the sum of load torque, load friction torque on the motor shaft, and internal and external disturbances; b α =K α / J α is the ratio of the motor rotation coefficient to the moment of inertia, which is a known quantity; u α is the control quantity of the motor; σ αi(i=1, 2, 3) are observation parameters, ε α is a high gain parameter, all parameters are adjustable and are positive real numbers; k α is the feedback parameter, which is a positive real number; The desired camera horizontal rotation angle (0°) is converted into the desired motor shaft angle; adjustable parameter c α >0; control error Sliding surface The design process is the same as that of the pitch angle control, so it will not be repeated. By controlling the two attitude angles of the camera according to the camera attitude control steps, the target can always be located in the center of the image.

[0240] represents the first-order derivative of , Represents the second-order derivative of , and the black dot “·” represents the or The corresponding symbols below; represents the first-order derivative of the observed value of , represents the observed value of the first-order derivative of ·, and the black dot “·” represents the or The corresponding symbol below, the small black dot above the symbol Indicates the first-order derivative, with two small black dots on the symbol head It means to find the second-order derivative.

[0241] An embodiment of the present invention further provides a multi-rotor UAV ground target tracking control device, comprising:

[0242] The ground target position calculation module is used to solve the specific position coordinates of the ground target by using the imaging principle of the camera and the position relationship between the UAV and the ground target;

[0243] The multi-rotor UAV trajectory planning module is used to calculate the expected flight trajectory of the multi-rotor UAV tracking the ground target using a geometric constraint-based method;

[0244] The multi-rotor UAV trajectory tracking module is used to track the desired flight trajectory of the multi-rotor UAV and design a PID controller, including the position, speed, attitude angle, and attitude angular velocity controller of the multi-rotor UAV;

[0245] The camera's attitude control module is used to control the camera's attitude angle using a sliding mode control method based on an extended state observer with strong anti-disturbance performance, so that the ground target is always located in the center of the image.

[0246] An embodiment of the present invention further provides an electronic device, comprising: a processor;

[0247] A memory, communicatively connected to the processor;

[0248] At least one program is stored in the memory and configured to be executed by the processor, and the at least one program is configured to: implement the multi-rotor unmanned aerial vehicle ground target tracking control method.

[0249] An embodiment of the present invention also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by an electronic device, the multi-rotor unmanned aerial vehicle ground target tracking control method is implemented.

Claims

1. A multi-rotor UAV ground target tracking control method, characterized in that The steps include: Calculate the position of the ground target, and use the imaging principle of the camera and the positional relationship between the drone and the ground target to solve the specific position coordinates of the ground target; Multi-rotor UAV trajectory planning, using a geometric constraint-based approach to calculate the desired flight trajectory of a multi-rotor UAV tracking a ground target; Multi-rotor UAV trajectory tracking, design PID controller for multi-rotor UAV to track the expected flight trajectory, including position, speed, attitude angle, and attitude angular velocity controller of multi-rotor UAV; The camera attitude control uses the sliding mode control method based on the extended state observer with strong anti-disturbance performance to control the camera attitude angle so that the ground target is always located in the center of the image; The specific method of the camera posture control is as follows: Since the camera is prone to shaking when it is fixed on the drone, a sliding mode control method based on an extended state observer with strong anti-disturbance capability is used to control the movement of the motor; the camera is fixed on the gimbal under the multi-rotor drone, and the gimbal drives it to rotate horizontally and in pitch, and the horizontal and pitch rotations of the gimbal are controlled by two motors, so the attitude control of the camera is converted into the angle control of the motor; Specifically, the method of controlling the motor for the pitch rotation of the camera using the sliding mode control method is as follows: First, establish the dynamic equation of the motor as shown below where θ β ,y β The angle of the motor shaft to control the pitch rotation is measured by the motor encoder; ω β is the motor shaft angular velocity, measured by the motor encoder; J β is the equivalent moment of inertia of the load and the motor shaft; d β (t) is the load torque T L , load friction torque T on the motor shaft f , the sum of internal and external disturbances Δ; K β is the motor torque coefficient; u β is the control quantity of the motor. The small black dot "·" on the symbol indicates the first-order derivative. β (t) Expand to the third state, let the state vector x β =[x β1 x β2 x β3 ] T =[θ β ω β d β (t)] T , formula (34) is expanded to where b β =K β / J β are known parameters; the extended state observer is designed as in Represents the observed value of state ·, and the black dot "·" represents the corresponding symbol under "^" in the formula; Represents the first-order derivative of the state observation; σ βi , i = 1, 2, 3 are observation parameters, ε β is a high gain parameter, all parameters are adjustable and are positive real numbers; this observer is used to implement the polynomial s 3 +σ β1 s 2 +σ β2 s+σ β3 When the Hurwitz condition is satisfied, when time t→∞, That is, the observed value gradually approaches the true value; Then design the sliding mode controller, set the control error in The desired camera pitch angle of 45° is converted into the desired motor shaft rotation angle; for the controlled object formula (34), the sliding surface is designed as Where c>0; the sliding mode controller based on the extended state observer is designed as where k β is the feedback parameter, which is a positive real number; the Lyapunov function of the sliding mode control is taken as Combining equations (35)-(38), we can find their differentials: where Δ max is a positive real number and satisfies represents the state observation error, that is The black dot "·" represents the corresponding symbol below "~" in the formula, the small black dot ".." above the symbol indicates the first-order derivative, and the two small black dots ".." above the symbol indicate the second-order derivative. From formula (39), we can get Then there is Therefore, when t→∞, The convergence speed depends on the feedback parameter k β and high gain parameter ε β ; Equation (36) and Equation (38) are the final extended state observer and control law; Similarly, the motor that controls the horizontal rotation of the camera uses the sliding mode control method to control its motion in the same way as the motor that controls the pitch rotation of the camera. The extended state observer and control law are designed as follows: where x α =[x α1 x α2 x α3 ] T =[θ α ω α d α (t)] T is the state vector, θ α ,y α To control the horizontal rotation angle of the motor shaft, measured by the motor encoder; d α (t) is the sum of load torque, load friction torque on the motor shaft, and internal and external disturbances; b α =K α / J α is the ratio of the motor rotation coefficient to the moment of inertia, which is a known quantity; u α is the control quantity of the motor; σ αi =1, 2, 3 are observation parameters, ε α is a high gain parameter, all parameters are adjustable and are positive real numbers; k α is the feedback parameter, which is a positive real number; The desired camera horizontal rotation angle 0° is converted into the desired motor shaft angle; adjustable parameter c α >0; control error Sliding surface represents the first-order derivative of ●, Represents the second-order derivative of ·, and the black dot "·" represents the corresponding symbol under "·" or "··" in the formula; represents the first-order derivative of the observed value of , represents the observed value of the first-order derivative of ·, and the black dot "·" represents the or The corresponding symbols below, the small black dot "·" on the symbol head indicates the first-order derivative, and the two small black dots ".." on the symbol head indicate the second-order derivative.

2. The multi-rotor UAV ground target tracking control method according to claim 1, characterized in that: The specific method for calculating the ground target position is as follows: Establish the kinematic model of the entire system of drone, camera, and ground target. Define the drone coordinate system: take the center of gravity of the multi-rotor drone as the origin b Oh, b O b The X-axis points to the direction of the nose of the drone in the symmetry plane. b O b The Z axis is perpendicular to the symmetry plane of the drone. b O b X down, then confirm by right hand rule b O b Y axis; Define the camera coordinate system: take the camera center of gravity as the origin c Oh, c O c The X axis points to the direction of the lens in the camera's symmetry plane. c O c The Y axis is perpendicular to the camera in the symmetry plane. c O c X down, then confirm by right hand rule c O c Y axis; the camera is fixed under the multi-rotor drone; Define the ground target coordinate system: take any fixed point on the ground as the origin s O, select the take-off position of the drone as the origin, let s O s The X-axis points in a certain direction in the horizontal plane. s O s The Z axis is perpendicular to the ground and downward, and then determined by the right-hand rule s O s Y axis; From the geometric relationship, we can know that the three-dimensional position vector of the dynamic target in the ground target coordinate system is It is expressed as follows s p0= s p1+ s p2+ s p3 (1) Among them, the vector is the position vector of the multi-rotor UAV in the ground target coordinate system. The position and attitude information of the UAV are directly obtained by the integrated navigation system of the UAV. is the distance from the center of mass of the drone to the optical center of the camera, expressed by the following formula in is the offset between the camera coordinate system and the drone coordinate system. Since the camera is fixed under the multi-rotor drone, b p2 is obtained in advance through calibration and is a known vector; is the transformation matrix between the UAV coordinate system and the ground target coordinate system, expressed as Among them, θ, φ, and ψ are the pitch angle, roll angle, and yaw angle of the multi-rotor drone, respectively; vector Represents the distance from the camera to the target, expressed as in is the coordinate representation of the ground target in the camera coordinate system, which is calculated using the image information obtained by the camera. is the transformation matrix from the camera coordinate system to the drone coordinate system, expressed as Where α is the horizontal rotation angle of the camera, and β is the pitch angle. These two angle information are measured by the encoder of the camera pan / tilt. Substituting equations (2) and (4) into equation (1), we get Calculate the coordinates of the target under the camera according to the imaging principle of the camera c p3=[ c x3 c y3 c z3] T , where the coordinates c x3, c y3, c z3 satisfies the following relationship Where f is the focal length of the camera, [u c v c ] T is the image pixel position coordinate of the ground target in the camera image plane, where u c is the horizontal coordinate, v c is the vertical coordinate, which is obtained by image processing method based on the image of the camera; the position of the ground target is set to s p0=[x0 y0 0] T , then according to formula (6) and formula (7) we can get The right side of formula (8) contains known quantities measured or calculated by sensors, and the left side contains three unknown quantities x0, y0, c z3, are obtained by solving equation (8) Where R ij Representation Matrix The i-th row and j-th column element, p k Representation vector The kth element of , the position of the ground target is calculated by equations (10) and (11) s p0=[x0 y0 0] T .

3. The multi-rotor UAV ground target tracking control method according to claim 2, characterized in that: The specific method of multi-rotor UAV trajectory planning is as follows: First, two constraints are set for the trajectory planning of the multi-rotor UAV. The first constraint: During the tracking process, the yaw angle of the multi-rotor drone needs to be constantly adjusted, that is, the horizontal rotation angle α of the camera is 0, so that the nose of the drone faces the target in real time; Second constraint: The horizontal distance between the multirotor drone and the target should be close to the flight altitude; Under these two constraints, Assume that the position vector of the multi-rotor drone in the ground target coordinate system at time k-1 is s p1 and the yaw angle is [x k-1 y k-1 z k-1 ψ k-1 ] T , whose values ​​are obtained by the inertial integrated navigation on the UAV; assuming that the position of the target at time k is (x 0,k y 0,k ); The frequent changes in the height of a multi-rotor drone are of little significance. At the same time, in order to facilitate the tracking and control of the drone, the flight height of the drone is designed to remain unchanged. Therefore, at time k, Where h is the pre-set flight altitude of the drone; the position of the target at time k is (x 0,k y 0,k ), according to the second constraint, the horizontal distance between the multi-rotor UAV and the target should be close to the flight altitude, that is, Therefore, at time k, the drone should be at the target position (x 0,k y 0,k ) is the center of the circle and the radius is h; on this circle, the current position of the drone (x k-1 y k-1 ) should be the point closest to (x 0,k y 0,k ) and (x k-1 y k-1 ) and the circumference, which is the expected position of the drone at time k. The intersection satisfies the following relationship The expected position of the UAV at time k can be solved by equations (12) and (13): for According to the first constraint, the nose of the UAV needs to face the target in real time, and the expected yaw angle at time k is designed to be Combining equations (12), (15), (16), and (17), we can obtain the expected flight trajectory l of the multi-rotor UAV at time k: ref =[x r y r z r ψ r ] T .

4. The multi-rotor UAV ground target tracking control method according to claim 3 is characterized in that: The specific method of multi-rotor UAV trajectory tracking is as follows: After obtaining the desired flight trajectory of the UAV, the PID control method is used to design the controller of the multi-rotor UAV, including position, speed, attitude angle, and attitude angular velocity controllers, so that the multi-rotor UAV can fly according to the desired trajectory; First, establish the dynamic equation of the multi-rotor drone in s p1=[xyz] T is the position vector of the multi-rotor UAV in the ground coordinate system; ω x ,ω y ,ω z are the rolling angular velocity, pitch angular velocity, and yaw angular velocity of the UAV respectively; I x ,I y ,I z are the moments of inertia of the roll axis, pitch axis, and yaw axis respectively; K1, K2, and K3 are the air resistance related coefficients; U1, U2, U3, and U4 are the control quantities of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor drone respectively, and m is the total mass of the drone; represents the first-order derivative of , Represents the second-order derivative of ·, and the black dot "·" represents the corresponding symbol under "·" or "··" in the formula; Position controller: The PI control method is used to design the control law of position control, as shown in the following formula: in K is the expected speed of the UAV in the north, east and ground directions obtained by the position controller; ij , i=P, I, j=x, y, z are the proportional and integral parameters of the three directions in the position controller, and the parameters are adjustable; Speed ​​controller: The P control method is used to design the control law of speed control, as shown in the following formula: in v is the expected acceleration of the UAV in the north, east and ground directions obtained by the velocity controller; x 、v y 、v z K is the actual measured speed of the UAV in the north, east and ground directions; Vx , K Vy , K Vz It is the proportional parameter of the three directions in the speed controller, and the parameter is adjustable; Then, according to the expected acceleration command, the longitudinal throttle control amount U1 and the expected pitch angle and roll angle are solved. The control forces in the three directions required inside the drone are as follows: According to the UAV dynamics equation (23), Combining equations (26) and (27), we can obtain the longitudinal throttle control value U1 and the desired pitch angle and roll angle respectively: Attitude angle controller: The desired attitude angle is obtained by equations (29), (30), and (17). The control law of attitude angle control designed by P control method is as follows: in is the desired roll, pitch, and yaw angular velocity; K ωθ , K ωφ , K ωψ is the proportional parameter of the three postures in the attitude angle controller; Attitude angular velocity controller: The PID control method is used to design the control law of attitude angular velocity control, as shown in the following formula in is the expected roll, pitch, and yaw acceleration; K i，j , i=P, I, D, j=ωφ, ωθ, ωψ are the proportional, integral and differential parameters of the three attitudes in the attitude angular velocity controller; The control values ​​of the roll angle, pitch angle, and yaw angle of the multi-rotor drone are U2, U3, and U4 respectively. The combined formulas (28), (32), and (33) are the control variables of the throttle, roll angle, pitch angle, and yaw angle of the multi-rotor UAV.

5. A multi-rotor UAV ground target tracking control device, characterized in that: include: The ground target position calculation module is used to solve the specific position coordinates of the ground target by using the imaging principle of the camera and the position relationship between the UAV and the ground target; The multi-rotor UAV trajectory planning module is used to calculate the expected flight trajectory of the multi-rotor UAV tracking the ground target using a geometric constraint-based method; The multi-rotor UAV trajectory tracking module is used to track the desired flight trajectory of the multi-rotor UAV and design a PID controller, including the position, speed, attitude angle, and attitude angular velocity controller of the multi-rotor UAV; The camera attitude control module is used to control the camera attitude angle using a sliding mode control method based on an extended state observer with strong anti-disturbance performance, so that the ground target is always located in the center of the image; The specific method of camera posture control is as follows: Since the camera is prone to shaking when it is fixed on the drone, a sliding mode control method based on an extended state observer with strong anti-disturbance capability is used to control the movement of the motor; the camera is fixed on the gimbal under the multi-rotor drone, and the gimbal drives it to rotate horizontally and in pitch, and the horizontal and pitch rotations of the gimbal are controlled by two motors, so the attitude control of the camera is converted into the angle control of the motor; Specifically, the method of controlling the motor for the pitch rotation of the camera using the sliding mode control method is as follows: First, establish the dynamic equation of the motor as shown below where θ β ,y β The angle of the motor shaft to control the pitch rotation is measured by the motor encoder; ω β is the motor shaft angular velocity, measured by the motor encoder; J β is the equivalent moment of inertia of the load and the motor shaft; d β (t) is the load torque T L , load friction torque T on the motor shaft f , the sum of internal and external disturbances Δ; K β is the motor torque coefficient; u β is the control quantity of the motor. The small black dot "·" on the symbol indicates the first-order derivative. β (t) Expand to the third state, let the state vector x β =[x β1 x β2 x β3 ] T =[θ β ω β d β (t)] T , formula (34) is expanded to where b β =K β / J β are known parameters; the extended state observer is designed as in Represents the observed value of state ·, and the black dot "·" represents the corresponding symbol under "^" in the formula; Represents the first-order derivative of the state observation; σ βi , i = 1, 2, 3 are observation parameters, ε β is a high gain parameter, all parameters are adjustable and are positive real numbers; this observer is used to implement the polynomial s 3 +σ β1 s 2 +σ β2 s+σ β3 When the Hurwitz condition is satisfied, when time t→∞, That is, the observed value gradually approaches the true value; Then design the sliding mode controller, set the control error in The desired camera pitch angle of 45° is converted into the desired motor shaft rotation angle; for the controlled object formula (34), the sliding surface is designed as Where c>0; the sliding mode controller based on the extended state observer is designed as where k β is the feedback parameter, which is a positive real number; the Lyapunov function of the sliding mode control is taken as Combining equations (35)-(38), we can find their differentials: Where Δ max is a positive real number and satisfies represents the state observation error, that is The black dot "·" represents the corresponding symbol below "~" in the formula, the small black dot ".." above the symbol indicates the first-order derivative, and the two small black dots ".." above the symbol indicate the second-order derivative. From formula (39), we can get Then there is Therefore, when t→∞, The convergence speed depends on the feedback parameter k β and high gain parameter ε β ; Equation (36) and Equation (38) are the final extended state observer and control law; Similarly, the motor that controls the horizontal rotation of the camera uses the sliding mode control method to control its motion in the same way as the motor that controls the pitch rotation of the camera. The extended state observer and control law are designed as follows: where x α =[x α1 x α2 x α3 ] T =[θ α ω α d α (t)] T is the state vector, θ α ,y α To control the horizontal rotation angle of the motor shaft, measured by the motor encoder; d ɑ (t) is the sum of load torque, load friction torque on the motor shaft, and internal and external disturbances; b α =K α / J α is the ratio of the motor rotation coefficient to the moment of inertia, which is a known quantity; u α is the control quantity of the motor; σ αi , i = 1, 2, 3 are observation parameters, ε α is a high gain parameter, all parameters are adjustable and are positive real numbers; k α is the feedback parameter, which is a positive real number; The desired camera horizontal rotation angle 0° is converted into the desired motor shaft angle; adjustable parameter c α >0; control error Sliding surface represents the first-order derivative of ●, Represents the second-order derivative of ·, and the black dot "·" represents the corresponding symbol under "·" or "··" in the formula; represents the first-order derivative of the observed value of , represents the observed value of the first-order derivative of ·, and the black dot "·" represents the or The corresponding symbols below, the small black dot "·" on the symbol head indicates the first-order derivative, and the two small black dots ".." on the symbol head indicate the second-order derivative.

6. An electronic device, characterized in that: include: processor; A memory, communicatively connected to the processor; At least one program is stored in the memory and configured to be executed by the processor, and the at least one program is configured to: implement the multi-rotor unmanned aerial vehicle ground target tracking control method as described in any one of claims 1-4.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by an electronic device, the multi-rotor unmanned aerial vehicle ground target tracking control method as described in any one of claims 1 to 4 is implemented.

Images