A tethered unmanned aerial vehicle cooperative control method in an air-ground heterogeneous robot system

Abstract

The application discloses a tethered unmanned aerial vehicle cooperative control method in an air-ground heterogeneous robot system, which eliminates the interference of the tension of the cable of the tethered unmanned aerial vehicle on the tethered unmanned aerial vehicle after the tethered unmanned aerial vehicle is connected with an unmanned vehicle. In the attitude control process, a self-disturbance rejection control algorithm is used, the disturbance of the cable of the system on the tethered unmanned aerial vehicle is observed through an extended state observer, and a disturbance compensation amount is introduced in a nonlinear error feedback control rate, so that the unmanned aerial vehicle reaches an expected attitude by using simple error feedback control. In the attitude control scheme, inner loop self-disturbance rejection control and outer loop PID control are used, compared with cascade PID control, the algorithm greatly reduces the parameter adjustment difficulty, has smaller overshoot, shorter adjustment time and better robustness. PID control is used in the height, horizontal and heading control scheme, so that the control parameters are relatively independent, the parameter selection is relatively simple, and the PID control has better adaptability and stronger robustness, thereby achieving good effect.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) control technology, specifically relating to a collaborative control method for tethered UAVs in an air-to-ground heterogeneous robot system. Background Technology

[0002] Nuclear energy, as a clean, low-carbon, safe, and efficient new energy source, is an important component of China's energy system. With the vigorous development of nuclear energy in China, the maintenance of nuclear facilities, nuclear decommissioning, and nuclear emergency response have received increasing attention. Nuclear radiation scenarios are complex, and various types of radiation pose radiation hazards to the human body, which limits on-site human operations in nuclear environments. Research on nuclear robot technology to replace humans in entering hazardous nuclear radiation environments for inspection and response operations has become an indispensable part of China's nuclear safety field. Therefore, there is an urgent need to study nuclear robot motion planning and autonomous operation to replace remote operation of nuclear robots by humans.

[0003] Unmanned vehicles (UAVs) offer advantages such as large payload capacity and long endurance; however, due to limitations in their operational space, they suffer from narrow field of view, weak maneuverability, and low efficiency, making it difficult to independently achieve multi-dimensional environmental perception. Unmanned aerial vehicles (UAVs), on the other hand, possess advantages such as being unrestricted by terrain, having a wide field of view, strong maneuverability, and high efficiency, enabling them to independently conduct large-scale environmental perception; however, their small payload and short endurance limit their application in nuclear radiation environments. Collaborative operation between UAVs and UAVs can leverage the complementary strengths of both. Summary of the Invention

[0004] To address the aforementioned shortcomings in existing technologies, the tethered UAV collaborative control method in the air-ground heterogeneous robot system provided by this invention solves the problem that existing tethered UAVs and unmanned vehicles cannot complement each other's advantages when working in the same environment.

[0005] To achieve the above-mentioned objectives, the technical solution adopted by this invention is as follows: a cooperative control method for tethered unmanned aerial vehicles (UAVs) in an air-to-ground heterogeneous robot system, comprising the following steps:

[0006] S1. Construct an air-ground heterogeneous robot system consisting of tethered drones and unmanned vehicles, and model the unmanned vehicles and tethered drones based on this system respectively.

[0007] S2. Construct a collaborative control system for tethered drones and unmanned vehicles;

[0008] The cooperative control system includes an altitude controller, an attitude controller, a horizontal position controller, and a heading controller.

[0009] S3. Process the expected linear velocity and angular velocity of the unmanned vehicle using the unmanned vehicle model to obtain the expected horizontal position and expected yaw angle of the unmanned vehicle.

[0010] S4. The desired horizontal position is processed sequentially by the horizontal position controller and the attitude controller to obtain the attitude control parameters of the UAV; the desired yaw angle is processed by the heading controller to obtain the heading control parameters of the UAV; and the desired altitude of the UAV is processed by the altitude controller to obtain the altitude control parameters of the UAV.

[0011] S5. Substitute the attitude, heading, and altitude control parameters of the UAV into the UAV model to obtain the control parameters of the tethered UAV and achieve collaborative control.

[0012] The beneficial effects of this invention are as follows:

[0013] This invention establishes a reasonable and accurate kinematic and dynamic model for tethered unmanned aerial vehicles (UAVs) experiencing cable tension interference, and analyzes in detail the impact of cable tension on the position and attitude of the tethered UAV. Subsequently, it utilizes an active disturbance rejection control (ADRC) method, introducing an extended state observer to estimate the interference effect of cable tension on the tethered UAV in real time, and uses a nonlinear state error feedback control law to compensate for the disturbance, eliminating the influence of cable tension on the attitude stability of the tethered UAV. The beneficial effects include:

[0014] (1) The present invention eliminates the interference caused by the tension of the cable generated by the tethered drone after it is connected to the unmanned vehicle.

[0015] (2) In the attitude control scheme, the active disturbance rejection control algorithm is used to observe the disturbance generated by the cable of the extended state observer system on the tethered UAV, and the disturbance compensation amount is introduced into the nonlinear error feedback control law. Simple error feedback control is used to enable the UAV to reach the desired attitude.

[0016] (4) In the attitude control scheme, inner loop active disturbance rejection control and outer loop PID control are used. Compared with cascade PID control, this control algorithm greatly reduces the difficulty of parameter tuning, has a smaller overshoot, shorter adjustment time, and better robustness.

[0017] (5) PID control is used in altitude, level and heading control schemes to make each control parameter relatively independent, the parameter selection is relatively simple, and it has good adaptability and strong robustness, thus achieving good results. Attached Figure Description

[0018] Figure 1 The flowchart of the tethered UAV collaborative control method in the air-to-ground heterogeneous robot system provided by the present invention is shown.

[0019] Figure 2 A simplified diagram illustrating the operation of the air-ground heterogeneous robot system provided by this invention.

[0020] Figure 3 The body coordinate system provided by the present invention With global coordinate system A schematic diagram of coordinate transformation between them.

[0021] Figure 4 The structural block diagram of the collaborative control system provided by the present invention.

[0022] Figure 5 The height controller structure block diagram provided by the present invention.

[0023] Figure 6 The block diagram of the active disturbance rejection controller in the angular velocity control loop provided by the present invention.

[0024] Figure 7 The diagram shows the structure of the attitude controller provided by this invention.

[0025] Figure 8 A block diagram of the horizontal position controller provided by the present invention.

[0026] Figure 9 The diagram shows the structural block diagram of the heading controller provided by this invention. Detailed Implementation

[0027] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0028] This invention provides a cooperative control method for tethered unmanned aerial vehicles (UAVs) in an air-to-ground heterogeneous robot system, such as... Figure 1 As shown, it includes the following steps:

[0029] S1. Construct an air-ground heterogeneous robot system consisting of tethered drones and unmanned vehicles, and model the unmanned vehicles and tethered drones based on this system respectively.

[0030] S2. Construct a collaborative control system for tethered drones and unmanned vehicles;

[0031] The cooperative control system includes an altitude controller, an attitude controller, a horizontal position controller, and a heading controller.

[0032] S3. Process the expected linear velocity and angular velocity of the unmanned vehicle using the unmanned vehicle model to obtain the expected horizontal position and expected yaw angle of the unmanned vehicle.

[0033] S4. The desired horizontal position is processed sequentially by the horizontal position controller and the attitude controller to obtain the attitude control parameters of the UAV; the desired yaw angle is processed by the heading controller to obtain the heading control parameters of the UAV; and the desired altitude of the UAV is processed by the altitude controller to obtain the altitude control parameters of the UAV.

[0034] S5. Substitute the attitude, heading, and altitude control parameters of the UAV into the UAV model to obtain the control parameters of the tethered UAV and achieve collaborative control.

[0035] In step S1 of this embodiment of the invention, the air-to-ground heterogeneous robot system consists of a tethered drone, an unmanned vehicle, and a winding machine and a mobile power supply mounted on the unmanned vehicle, such as... Figure 2 As shown, the system is reasonably simplified, and three coordinate systems are established for the air-ground heterogeneous robot according to the right-hand rule: a global coordinate system fixed relative to the Earth, and a global coordinate system. Different vehicle coordinate systems are fixed relative to autonomous vehicles. and the fixed coordinate system of the tethered drone. Among them, the global coordinate system Following the North-East-Earth (NED) orientation, i.e., the global coordinate system o e x e o e y e With o e z e The axes point to the Earth's North Pole, the Earth's East Pole, and are perpendicular to the Earth's surface, pointing towards the Earth's center; vehicle coordinate system. With body coordinate system All follow the front-right-bottom (FRD) orientation, i.e., the vehicle coordinate system. With body coordinate system o g x g With o a x a The axes point towards the front of the unmanned vehicle and the front of the tethered drone, respectively. g y g With o a y a The axes point to the right of the unmanned vehicle and the tethered drone, respectively. g z g With o a z a The axes point from the top to the bottom of both the unmanned vehicle and the tethered drone. Meanwhile, the vehicle coordinate system... The origin o g With body coordinate system The origin o a They are located at the geometric centers of the unmanned vehicle and the tethered drone, respectively.

[0036] In step S1 of this embodiment of the invention, the unmanned vehicle modeling model is the kinematic equation of the unmanned vehicle. It is assumed that the geometric center of the unmanned vehicle coincides with the center of gravity, and the unmanned vehicle is in the global coordinate system. Chinese z e The linear velocity of the planar motion with a velocity of 0 is... g v and its direction is along the vehicle coordinate system o g x g The axis has an angular velocity of g The kinematic equations of the unmanned vehicle are: w.

[0037]

[0038] In the formula, e x g and e y g These represent the autonomous vehicle in the global coordinate system. Middle edge o e x e With o e y e The position of the axis, ψ g Indicates the autonomous vehicle in the global coordinate system The yaw angle in the formula. It can be seen from this formula that the yaw angle can be changed by... g v and g The size of w determines the autonomous vehicle's position in the global coordinate system. Chinese z e Arbitrary movement in a plane where = 0

[0039] In step S1 of this embodiment of the invention, the tethered UAV modeling model includes the kinematic model, dynamic model, and influence equation of the cable on the tethered UAV; wherein, the kinematic model includes position kinematic equation and attitude kinematic equation; the dynamic model includes position dynamic equation and attitude dynamic equation.

[0040] Specifically, using an X-shaped quadcopter as the tethered UAV in this embodiment, the following assumptions are made when modeling the UAV:

[0041] (1) The geometric center and the center of gravity of the tethered UAV coincide;

[0042] (2) The effect of the gyroscopic torque generated by the rotation of the motor and propeller on the tethered UAV is ignored;

[0043] (3) Ignore the impact of air resistance on tethered drones;

[0044] (4) The two ends of the cable act on the coordinate system of the tethered UAV body respectively. Ina p F Point and autonomous vehicle body coordinate system The origin o g And it remains in a straight position at all times;

[0045] (5) Due to airflow interference and the influence of robot motion on the cable, it is assumed that the magnitude of the cable tension F follows the average value F. m The standard deviation is F σ The Gaussian distribution.

[0046] In this embodiment, the kinematic equations for the position of the tethered UAV are as follows:

[0047] For tethered UAVs in the global coordinate system Kinematic analysis was performed to obtain the tethered UAV in the global coordinate system. The position kinematic equations are as follows:

[0048]

[0049] In the formula, e p a =[ e x a , e y a , e z a ] T and These are examples of tethered unmanned aerial vehicles in the global coordinate system. lower edge o e x e o e y e o e z e The position and linear velocity of the axis;

[0050] In this embodiment, the attitude kinematics equations for the tethered UAV are as follows:

[0051] Posture kinematic equations of a machine including a coordinate system To the global coordinate system rotation matrix and the attitude change rate of tethered drones With body angular velocity a The relational expression for w;

[0052] Specifically, in determining the rotation matrix During the process, the following unit vector is defined:

[0053]

[0054] In each coordinate system, the unit vector along each coordinate axis is equal to i1, i2, and i3, respectively. Specifically, in the global coordinate system... Down, along o e x e o e y e o e z e The unit vectors of the axes are represented as e1, e2, and e3, respectively; in the body coordinate system... Down, along o a x a o a y a o a z a The unit vectors of the axes are represented as a1, a2, and a3, respectively. In the two coordinate systems mentioned above, e i (i = 1, 2, 3), a i (i = 1, 2, 3) satisfy the following relations respectively:

[0055]

[0056] Body coordinate system With global coordinate system The universal rotational relationships between them can be established using the global coordinate system. Circling around its o in sequence e z e o e y e o e x e Obtained by rotating the three coordinate axes. Based on the above rotation relationship and the unit vector defined by formulas (3) and (4), define... Figure 3 The coordinate system shown:

[0057] First, the global coordinate system o e -e1e2e3 around the coordinate axis o e e3 rotation angle ψ a Obtain the intermediate coordinate system o e -k1k2k3. At this time, the coordinate system o e -k1k2k3 to coordinate system o e The rotation matrix R(ψ) of -e1e2e3 a )for:

[0058]

[0059] Furthermore, coordinate system o e -k1k2k3 around the coordinate axis o e k2 rotation angle θ a Obtain coordinate system oe -n1n2n3. At this time, coordinate system o e -n1n2n3 to coordinate system o e The rotation matrix R(θ) of -k1k2k3 a )for:

[0060]

[0061] Finally, coordinate system o e -n1n2n3 around the coordinate axis o e n1 Rotation angle φ a Obtain the body coordinate system o a -a1a2a3. At this time, the body coordinate system o a -a1a2a3 to coordinate system o e The rotation matrix R(φ) of -n1n2n3 a )for:

[0062]

[0063] Furthermore, the body coordinate system To the global coordinate system rotation matrix for:

[0064]

[0065] In the formula, φ a θ a and ψ a These are the roll angle, pitch angle, and yaw angle, respectively, R(φ) a ), R(θ) a ), R(ψ a ) are the rotation matrices for roll, pitch, and yaw angles, respectively. * c * and t * These represent the trigonometric functions sin(*), cos(*), and tan(*), respectively.

[0066] Determining the attitude change rate of a tethered drone With body angular velocity a During the process of w:

[0067] To make the tethered UAV in the body coordinate system lower edge o a x a o a y a o a z a The angular velocity of the axis is according to Figure 3The rotational relationship shown indicates the angular velocity of the tethered drone. a w and attitude change rate The relationship is shown in formula (9):

[0068]

[0069] in a k3 a n2 represents the coordinates of k3 and n2 in the body coordinate system. The representation in the formulas (3)-(6) is given by:

[0070]

[0071] in Substituting the aforementioned equations and formulas (3) and (10) into formula (9), we can obtain the angular velocity of the tethered UAV body. a w and attitude change rate Relationship:

[0072]

[0073] This leads to the attitude change rate of the tethered UAV. With body angular velocity a The relation for w is:

[0074]

[0075] In the formula, φ a θ a and ψ a These are the roll angle, pitch angle, and yaw angle, respectively, R(φ) a ), R(θ) a ), ψ(ψ a These are the rotation matrices for roll, pitch, and yaw angles, respectively. These are the rates of change of roll angle, pitch angle, and yaw angle, respectively. These are the coordinates of the tethered UAV in the body coordinate system. lower edge o a x a o a y a o a z a angular velocity of the axis, s * c * and t * These represent the trigonometric functions sin(*), cos(*), and tan(*), respectively.

[0076] In this embodiment, the position dynamics equations for the tethered unmanned aerial vehicle are as follows:

[0077] Let the angular velocity of the four propellers of the tethered UAV be... The lift T generated when a single propeller rotates i The size of (i = 1, 2, 3, 4) is:

[0078]

[0079] Among them, C t >0 represents the lift constant, the value of which depends on the air density, propeller radius, number of blades, and blade chord length. In this case, the magnitude of the total lift T generated by the propeller rotation is:

[0080]

[0081] Based on the point of action of the cable on the tethered drone in the body coordinate system The position in the middle a p F =[ a x F , a y F , a z F ] T and the body coordinate system of tethered drones In the global coordinate system posture in With position e p a This allows us to obtain the point of action of the cable on the tethered drone in the global coordinate system. The position in the middle e p F =[ e x F , e y F , e z F ] T Represented as:

[0082]

[0083] Based on the point of application of the cable on the tethered drone and the unmanned vehicle (vehicle coordinate system) The origin o g In the global coordinate system The positions in the middle are respectively e p F and e p h =[ e x g , e y g ,0]T One end of the cable can be obtained ( e p F ) to the other end ( e p g The relative position of )

[0084]

[0085] In the global coordinate system The tension F in the cable is decomposed into its components, which are parallel to the global coordinate system. China e x e o e y e o e z e The three components of the axis e F x , e F y and e F z And their sizes are as follows:

[0086]

[0087] According to Newton's second law, the position dynamics equations of the tethered UAV can be obtained as follows:

[0088]

[0089] In the formula, m is the mass of the drone. Let be the velocity derivative of the UAV in the global coordinate system. e F x , e F y and e F z In the global coordinate system The tension F in the cable is decomposed into components parallel to the global coordinate system. China e x e o e y e o e z e The three components of the axis are: G is gravity, and T is the total lift generated when the propeller of the tethered UAV rotates.

[0090] In this embodiment, the attitude dynamics equations for the tethered UAV are as follows:

[0091] Assuming the wheelbase of the tethered UAV is 2d, then the lift T of the tethered UAV in the body coordinate system... lower edge o a xa and o a y a The rolling torque τ generated by the shaft x and pitching moment τ y They are respectively:

[0092]

[0093] The anti-torque M exerted on the tethered drone when the propeller rotates i (i = 1, 2, 3, 4) in the body coordinate system The following can be represented as:

[0094]

[0095] Among them, M i The direction of (i = 1, 2, 3, 4) is opposite to the direction of propeller rotation, and C m With C t Similarly, its value depends on air density, propeller radius, number of blades, and blade chord length. The total anti-torque τ generated by the four motors of the tethered drone on the tethered drone is then... z Size in body coordinate system The following can be represented as:

[0096] τ z =M1-M2+M3-M4 (21)

[0097] According to the cable tension F = [ e F x , e F y , e F z ] T and its body coordinate system The point of action of the tethered drone a p F =[ a x F , a y F , a z F ] T We can obtain the torque τ′ generated by the cable tension F on the tethered UAV in the body coordinate system. The representation in:

[0098]

[0099] in, World coordinate system Transform to body coordinate system The rotation matrix has the following specific values:

[0100]

[0101] According to Euler's formula, we can obtain the coordinates in the body coordinate system. Attitude dynamics equations for tethered unmanned aerial vehicles:

[0102]

[0103] Where J = diag[J xx J yy J zz Let τ be the moment of inertia of the tethered unmanned aerial vehicle (UAV), where τ = [τ]. x ,τ y ,τ z ] T .

[0104] Given a constant, full-rank matrix M, find its inverse matrix Minverse. -1 Formula (25) can be transformed as follows;

[0105]

[0106] Given a constant, full-rank matrix M, find its inverse matrix Minverse. -1 Equation (25) can be transformed into the attitude dynamics equations shown below:

[0107]

[0108] In the formula, and Let ω represent the angular velocities required for the four propellers of the tethered drone, M be a constant full-rank matrix, and τ be the angular velocities required for the propellers of the tethered drone. x τ y The lift T of the tethered UAV in the body coordinate system are respectively lower edge o a x a and o a y a The rolling torque τ generated by the shaft x and pitching moment τ y , τ z The total counter-torque generated by the four motors of the tethered drone on the tethered drone;

[0109] Using formula (26), the square of the angular velocity required by the four propellers of the tethered UAV can be calculated based on the desired lift and torque of the tethered UAV. Achieve power distribution for tethered drones.

[0110] In this embodiment, the equation for the influence of cables on tethered drones is as follows:

[0111] Because the cable is typically installed near the geometric center of the tethered drone, that is... a p F ≈[0,0,0] T Therefore, formula (15) can be rewritten as formula (27):

[0112] e p F ≈ e p a (27)

[0113] Substituting formula (27) into formula (16) yields formula (28):

[0114]

[0115] During the coordinated control of the tethered drone and the unmanned vehicle, as the tethered drone stably tracks the unmanned vehicle, the relative positions Δx and Δy of the tethered drone and the unmanned vehicle on the horizontal plane approach zero, and their relative height Δz approaches the desired height of the tethered drone. At this point, formula (17) becomes formula (29):

[0116]

[0117] Meanwhile, the roll angle φ of the tethered drone a With pitch angle θ a It gradually stabilizes at a small angle, approaching zero. At this point, in formula (23)... The result is shown in formula (30):

[0118]

[0119] Furthermore, substituting equations (29) and (30) into equation (22) and expanding them in detail, we arrive at the equation for the influence of the cable on the tethered UAV, i.e., the equation for the body coordinate system. The torque τ generated by the tension of the cable on the tethered drone ′ a Its expression is:

[0120]

[0121] In the formula, τ ′ x τ ′ y and τ z ′ The tension of the cable on the tethered UAV in the body coordinate system. o a x a oa y a o a z a The torque generated by the shaft a y F and a x F The tension F of the cable is applied to the tethered UAV in the body coordinate system at o. a y a and o a x a The coordinates of the axis.

[0122] According to formulas (29) and (18), as the tethered UAV stably tracks the unmanned vehicle, the tension F of the cable mainly affects the tethered UAV in the global coordinate system. lower edge o e z e Shaft generates force e F z Its function directly affects the position of tethered UAVs in the global coordinate system. lower edge o e z e linear acceleration of the axis This, in turn, affects the tethered drone in the global coordinate system. Middle edge o e z e linear velocity of the shaft and height e z a This will not affect the stability of the tethered UAV's attitude. As can be seen from formula (31), as the tethered UAV stably tracks the unmanned vehicle, the tension F of the cable mainly affects the tethered UAV's attitude in the body coordinate system. Middle edge o a x a and o a y a The shaft generates a torque τ ′ x and τ ′ y Its function.

[0123] Furthermore, according to formula (24), the torque τ ′ x and τ ′ y This will directly affect the tethered drone's position in the body coordinate system. lower edge o a x a and o a y a angular acceleration of the axis and This, in turn, affects the tethered drone's coordinate system. lower edge o a x a and o a y a angular velocity of the axis and Ultimately affecting tethered drones in the global coordinate system Stability of attitude.

[0124] In step S2 of this embodiment, based on the foregoing analysis, it can be seen that the influence of the cable tension F on the attitude stability of the tethered UAV is mainly due to the force it generates along the body coordinate system. o a x a and o a y a The torque τ on the shaft ′ x and τ ′ y Directly affects the tethered UAV along the body coordinate system o a x a and o a y a angular acceleration of the axis and This is caused by the cable tension F. Therefore, to eliminate the interference of the cable tension F on the attitude stability of the tethered UAV, the control system of the tethered UAV in the body coordinate system can be targeted. lower edge o a x a and o a y a The angular velocity loop of the shaft is designed with an active disturbance rejection control algorithm to estimate the disturbance caused by the cable tension F to the tethered UAV in real time and compensate for the disturbance. For other sub-control systems of the tethered UAV control system, a proportional-integral-derivative (PID) control algorithm is used. Based on this, this embodiment is designed as follows: Figure 4 The cooperative control system of the tethered UAV and unmanned vehicle shown includes an altitude controller, attitude controller, horizontal position controller and heading controller, all of which are cascade control structures composed of inner loop controllers and outer loop controllers.

[0125] exist Figure 4 middle, g v # and g w # Represent the vehicle's coordinate system. Middle edge o g x g The desired linear velocity of the axis and along o g z g The desired angular velocity of the axis. Figure 4In the cooperative control system shown, the unmanned vehicle is positioned in the global coordinate system. Middle edge o e x e With o e y e Axis position e x g and e y g As the desired input to the horizontal position controller of the tethered UAV, it enables positional coordination between the tethered UAV and the unmanned vehicle; simultaneously, it uses the tethered UAV in the global coordinate system... yaw angle ψ g As the desired input to the yaw controller of a tethered drone, it enables the coordination of the tethered drone's heading with that of an unmanned vehicle.

[0126] In this embodiment, to avoid the drawback of conventional differentiators being sensitive to high-frequency noise in the cooperative control system, a PID controller in the complex s-domain is used. For any of the altitude controller, attitude controller, horizontal position controller, and heading controller, the expression for the corresponding PID controller is:

[0127]

[0128] In the formula, R * (s), X * (s) represent the system's expected value r. * With the current state x * The Laplace transform, E * (s) and U * (s) represent the expected r * With feedback x * error e * and the control quantity u output by the controller * Laplace transform, K *,P K *,I and K *,D These are the proportional coefficient, integral coefficient, and derivative coefficient in the PID controller, respectively, and N is the filter coefficient in the low-pass filter.

[0129] When the integral coefficient K *,I Or differential coefficient K *,D When the integral coefficient K is zero, the PID controller degenerates into a PD controller and a PI controller; when the integral coefficient K is zero, the PID controller degenerates into a PD controller and a PI controller. *,I With differential coefficient K *,D When both are zero, the PID controller degenerates into a P controller.

[0130] In this embodiment, the inner loop controller and outer loop controller of the tethered UAV's altitude controller are a P-controller and a PID controller, respectively, which respectively realize the tethered UAV's altitude in the global coordinate system. lower edge o e z e The control block diagrams for shaft height speed control and height position control are as follows: Figure 5 As shown:

[0131] Where T # The desired lift of the tethered UAV calculated by the altitude controller will be T # Substituting into formula (26), the square of the rotational speed required for the four propellers of the tethered UAV to provide this lift can be calculated, thus achieving power distribution. Furthermore, Figure 5 The adjustable parameters of the P controller and the PID controller are respectively With K z,P K z,I K z,D .

[0132] In this embodiment, the attitude controller includes a roll cascade controller and a pitch cascade controller, which respectively control the roll angle and pitch angle of the tethered UAV.

[0133] The inner loop controllers of both the roll cascade controller and the pitch cascade controller are active disturbance rejection controllers, while the outer loop controllers are PID controllers. The inner loop controllers control the tethered UAV in the body coordinate system. Middle edge o a x a With o a y a The angular velocity of the axis is controlled, and the tension F of the cable is estimated and compensated in real time for the tethered UAV in the body coordinate system. lower edge o a x a With o a y a The disturbance generated by the axis; the outer loop controller realizes the control of the roll and pitch angles of the tethered UAV, and outputs the desired roll and pitch rates of the tethered UAV.

[0134] Based on the aforementioned UAV modeling model, formula (24) is expanded in detail, and the following is ignored. From this, we can obtain formula (33):

[0135]

[0136] For tethered UAVs in the body coordinate system lower edge o a x a In the angular velocity control loop of the shaft, the following equation is established:

[0137]

[0138] Where, x x,1 and x x,2 For tethered UAVs in the body coordinate system lower edge o a x a The state variables in the angular velocity control loop of the shaft; d x The external disturbance to this loop, generated by the cable tension T, is unknown; f x This is an internal disturbance of the loop, because the tethered UAV is in the body coordinate system. lower edge o a y a With o a z a angular velocity of the axis and It can be measured in real time using a gyroscope, therefore it is known; u x b is the control output of this loop. x The compensation factor is an adjustable parameter. Based on formulas (33) and (34), the coordinates of the tethered UAV in the body coordinate system can be obtained. lower edge o a x a State equation (35) in the angular velocity control loop of the shaft:

[0139]

[0140] This embodiment focuses on tethered unmanned aerial vehicles (UAVs) in the body coordinate system. lower edge o a x a With o a y a The active disturbance rejection controller designed in the shaft angular velocity control loop consists of two parts: an extended state observer and a nonlinear state error feedback control law.

[0141] According to the state equation shown in formula (35), the designed tethered UAV is in the body coordinate system. lower edge o a x a The extended state observer (ESO) of the active disturbance rejection controller in the angular velocity control loop of the shaft is shown in Equation (36):

[0142]

[0143] In the formula, z x,1 With z x,2 For each ESO, the state variable x x,1 With x x,2 The estimated value, and These are the derivatives of the estimated values, e x To extend the state observer for state variable x x,1 The estimation error, For tethered UAVs along the body coordinate system o a x a angular velocity, β x,1 β x,2 and δ x,1 For adjustable parameters, fal(*) is a specific function in the active disturbance rejection control algorithm, f x For the internal disturbance of this loop, b x For along o a x a Axis compensation factor, u x For the output of the angular velocity control loop along o a x a The control parameters of the axis;

[0144] fal(*) is a specific function in the active disturbance rejection control algorithm, and its specific expression is shown in formula (37):

[0145]

[0146] Tethered UAV in body coordinate system lower edge o a x a The expression for the nonlinear state error feedback control law of the shaft is:

[0147]

[0148] In the formula, α x With δ x,2 ε is an adjustable parameter. x For state variable x x,1 The error, For along o a x a The desired angular velocity of the axis, u ′ x The control quantity u is the final output of the active disturbance rejection controller. x The derivative;

[0149] Similar to the analysis above, for tethered UAVs in the body coordinate system lower edge o a y a In the angular velocity control loop of the shaft, the equation shown in formula (39) is established:

[0150]

[0151] Where, x y,1 With xy,2 For tethered UAVs in the body coordinate system lower edge o a y a The state variable d in the angular velocity control loop of the shaft y For external unknown disturbances, f y For known internal perturbations, u y b is the control quantity output by this loop. y These are adjustable parameters. Based on formulas (33) and (39), the coordinates of the tethered UAV in the body coordinate system can be obtained. lower edge o a y a State equation (40) in the angular velocity control loop of the shaft:

[0152]

[0153] According to formula (40), the coordinate system of the tethered UAV is obtained. lower edge o a y a The expression for the axis's expanded state observer is:

[0154]

[0155] In the formula, z y,1 With z y,2 For each of the extended state observers, the state variable x is... y,1 With x y,2 The estimated value, and For each ESO, the state variable x y,1 With x y,2 The estimated value, e y To extend the state observer for state variable x y,1 The estimation error, For tethered UAVs in the body coordinate system lower edge o a y a angular velocity, β y,1 β y,2 and δ y,1 All are adjustable parameters, f y Internal known disturbance, b y For along o a y a The compensation factor of the axis, u y For the output of the angular velocity control loop along o a y a＝ The control parameters of the axis;

[0156] According to formula (40), the coordinate system of the tethered UAV is obtained. lower edge oa The expression for the nonlinear state error feedback control law along the y-axis is:

[0157]

[0158] In the formula, α y With δ y,2 ε is an adjustable parameter. y For state variable x y,1 The error, For along o a y a The desired angular velocity of the axis, u ′ y The control quantity u is the final output of the active disturbance rejection controller. y The derivative of .

[0159] In this embodiment, the tethered UAV is considered in the body coordinate system. lower edge o a x a o a y a The block diagrams of the active disturbance rejection controllers designed for the angular velocity control loops of the shafts are as follows: Figure 6 As shown:

[0160] exist Figure 6 In the equation, i = x, y, The desired angular velocity is calculated by the external controller in the roll and pitch cascade controller.

[0161] The outer loop controller of the attitude controller for the tethered UAV is a PID controller, which controls the roll and pitch angles of the tethered UAV. Its output is the desired roll and pitch rates of the tethered UAV. Assume the tethered UAV is in a global coordinate system. The roll angle φ a With pitch angle θ a For small angles, formula (12) can be rewritten as:

[0162]

[0163] According to formula (43), the outer loop output of the attitude controller can be directly used as the desired input of the inner loop controller. Based on this, this paper designs... Figure 7 The attitude controller shown; in Figure 7 In this case, i = φ, θ For tethered UAVs in the global coordinate system The desired roll angle and desired pitch angle are respectively represented by the parameters K in the PID controller. a,P K a,I K a,D .

[0164] In this embodiment of the invention, the inner loop controller and outer loop controller of the horizontal position controller are respectively a PD controller and a PID controller, which respectively realize the control of the tethered UAV along the body coordinate system. China a x a and o a y a Axis linear velocity control and tethered UAVs along the global coordinate system China e x e and o e y e Axis position control.

[0165] Specifically, for an external PID controller, its output is the tethered UAV moving along the global coordinate system. China e x e With o e y e Desired linear velocity and The desired input for the internal PD controller is the tethered UAV in the body coordinate system. Middle edge o a x a With o a y a linear velocity of the shaft and It can be calculated using the coordinate transformation shown in formula (44):

[0166]

[0167] Consider another body coordinate system Its coordinate system with the body coordinate system They have the same origin, but different coordinate systems. In the global coordinate system The roll angle φ′ a With pitch angle θ′ a Both are zero, and their yaw angle ψ′ a With body coordinate system In the global coordinate system yaw angle ψ a Equal. In the body coordinate system. Middle edge o′ a x′ a and o′ a y′ a By decomposing the lift T of the tethered UAV about the axis, we can obtain formula (45):

[0168]

[0169] Assuming roll angle φ a With yaw angle θ a Since all angles are small, substituting formula (46) into formula (45) and performing a simple transformation yields:

[0170]

[0171] The output of the internal PD controller of the horizontal position controller is the tethered UAV in the body coordinate system. lower edge o a x a and o a y a Desired linear acceleration of the axis and By transforming formula (47) and reconsidering the product of coefficient m / T and internal PD parameters as parameters of the internal PD controller, the desired roll angle of the tethered UAV in the global coordinate system can be obtained. and desired pitch angle

[0172] The linear velocity signal output by a tethered UAV is its position in the global coordinate system. linear velocity in e v a The feedback signal input to the internal PD controller of the horizontal position controller is the tethered UAV's position in the body coordinate system. The linear velocity of the lower a v a It can be obtained through formula (48):

[0173]

[0174] Based on the above analysis, this embodiment designs as follows: Figure 8 The horizontal position controller, wherein e i # (i = x, y) represents the tethered UAV in the global coordinate system. Middle edge o e x e With o e y e The desired position of the axis. Furthermore, to prevent the desired angle output by the horizontal position controller from being too large and affecting the stability of the tethered UAV's attitude, this paper limits the output of the internal PD controller of the horizontal position controller, with a limiting range of [-π / 9, π / 9]. Figure 8 In the horizontal position controller shown, the parameters of the PD controller are as follows: The parameters of the PID controller are: K p,P K p,I K p,D .

[0175] In this embodiment, the inner loop controller and outer loop controller of the heading controller are a P-controller and a PID controller, respectively, which respectively control the tethered UAV in the global coordinate system. Yaw angle control and tethered UAVs in the body coordinate system Middle edge o a z a Angular velocity control of the shaft.

[0176] The outer loop controller output of the heading controller is the yaw rate, which, according to formula (43), can be used as the desired input of the inner loop controller. The control block diagram of the heading controller is as follows: Figure 9 As shown, where For the desired heading angle, the parameters of the P controller and the PID controller are respectively... K ψ,P K ψ,I With K ψ,D .

[0177] In step S3 of this embodiment of the invention, after taking the desired linear velocity and velocity of the autonomous vehicle as inputs to the autonomous vehicle model, the autonomous vehicle in the global coordinate system is obtained. Middle edge o e x e With o e y e The position of the axis and the autonomous vehicle in the global coordinate system The yaw angle is used as the expected horizontal position and expected yaw angle of the UAV.

[0178] In step S4 of this embodiment of the invention, the desired horizontal position of the UAV is used as the input to the horizontal position controller to obtain the desired pitch angle and desired roll angle, and then the attitude controller obtains the position of the tethered UAV in the body coordinate system. lower edge o a x a and o a y a The desired roll and pitch moments of the axis are used as attitude control parameters for the UAV.

[0179] The desired yaw angle of the UAV is used as the input to the heading controller to obtain the desired total anti-torque generated by the four motors of the tethered UAV. As a heading control parameter for drones.

[0180] The desired altitude of the drone is used as the input to the altitude controller to obtain the desired lift T of the tethered drone. # This serves as the altitude control parameter for the drone.

[0181] In step S5 of this embodiment of the invention, the desired roll moment, pitch force, and desired lift T are... # and expected total anti-torque Substituting these values ​​into the drone model, we obtain the squares of the angular velocities required for the four propellers of the tethered drone, thus realizing the power distribution of the tethered drone.

[0182] Specific embodiments have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

[0183] Those skilled in the art will recognize that the embodiments described herein are intended to help the reader understand the principles of the invention, and should be understood that the scope of protection of the invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific modifications and combinations based on the technical teachings disclosed in this invention without departing from the spirit of the invention, and these modifications and combinations are still within the scope of protection of this invention.

Claims

1. A cooperative control method for tethered unmanned aerial vehicles (UAVs) in an air-to-ground heterogeneous robot system, characterized in that, Includes the following steps: S1. Construct an air-ground heterogeneous robot system consisting of tethered drones and unmanned vehicles, and model the unmanned vehicles and tethered drones based on this system respectively; S2. Construct a collaborative control system for tethered drones and unmanned vehicles; The cooperative control system includes an altitude controller, an attitude controller, a horizontal position controller, and a heading controller. The altitude controller, attitude controller, horizontal position controller, and heading controller in the cooperative control system are all cascade control structures composed of inner loop controllers and outer loop controllers. The inner loop controller and the outer loop controller of the height controller are P controller and PID controller respectively, which respectively realize the tethered unmanned aerial vehicle in global coordinate system Lower edge Height speed control and height position control of the shaft The attitude controller includes a roll cascade controller and a pitch cascade controller, which respectively control the roll angle and pitch angle of the tethered UAV. The inner loop controllers of both the roll cascade controller and the pitch cascade controller are active disturbance rejection controllers, while the outer loop controllers are PID controllers. The inner loop controllers control the tethered UAV in the body coordinate system. Middle and The angular velocity of the shaft is controlled, while the tension of the cable is estimated and compensated in real time. For tethered UAVs in the body coordinate system lower edge and The disturbance generated by the axis; the outer loop controller realizes the control of the roll and pitch angles of the tethered UAV, and outputs the desired roll and pitch rates of the tethered UAV; The inner and outer loop controllers of the horizontal position controller are a PD controller and a PID controller, respectively, which respectively control the tethered UAV along the body coordinate system. middle and Axis linear velocity control and tethered UAVs along the global coordinate system middle and Axis position control; The heading controller comprises an inner loop controller (P-controller) and an outer loop controller (PID-controller), which respectively control the tethered UAV in the global coordinate system. Yaw angle control and tethered UAVs in the body coordinate system Middle Angular velocity control of the shaft; S3. Process the expected linear velocity and angular velocity of the unmanned vehicle using the unmanned vehicle model to obtain the expected horizontal position and expected yaw angle of the unmanned vehicle. S4. The desired horizontal position is processed sequentially by the horizontal position controller and the attitude controller to obtain the attitude control parameters of the UAV; the desired yaw angle is processed by the heading controller to obtain the heading control parameters of the UAV; and the desired altitude of the UAV is processed by the altitude controller to obtain the altitude control parameters of the UAV. S5. Substitute the attitude, heading, and altitude control parameters of the UAV into the UAV model to obtain the control parameters of the tethered UAV and achieve collaborative control.

2. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 1, characterized in that, In step S1, the air-ground heterogeneous robot system includes three coordinate systems, namely a global coordinate system fixed relative to the Earth. Different vehicle coordinate systems are fixed relative to the unmanned vehicles. and the body coordinate system of the tethered drone that is fixed. ; The unmanned vehicle modeling model is the kinematic equation of the unmanned vehicle; The tethered UAV modeling model includes the kinematic model, dynamic model, and influence equations of the cable on the tethered UAV; wherein, the kinematic model includes position kinematic equations and attitude kinematic equations; the dynamic model includes position dynamic equations and attitude dynamic equations.

3. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 2, characterized in that, When the autonomous vehicle is in the global coordinate system middle The planar motion has a linear velocity of . And the direction is along the vehicle coordinate system of The axis has an angular velocity of At that time, the kinematic equations of the driverless car are: In the formula, and These represent the autonomous vehicle in the global coordinate system. Middle and The position of the axis Indicates the autonomous vehicle in the global coordinate system The yaw angle in the middle.

4. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 3, characterized in that, For tethered UAVs in the global coordinate system Kinematic analysis was performed to obtain the tethered UAV in the global coordinate system. The position kinematic equations are as follows: In the formula, and These are the tethered UAVs in the global coordinate system. lower edge , , The position and linear velocity of the axis; The kinematic equations of attitude include the body coordinate system To the global coordinate system rotation matrix and the attitude change rate of tethered drones With body angular velocity Relationship; Wherein, rotation matrix for: In the formula, , and These are roll angle, pitch angle, and yaw angle, respectively. These are the rotation matrices for roll, pitch, and yaw angles, respectively. , and They represent trigonometric functions respectively. , as well as ; Rate of attitude change of tethered unmanned aerial vehicles With body angular velocity The relation is: In the formula, , and These are roll angle, pitch angle, and yaw angle, respectively. These are the rotation matrices for roll, pitch, and yaw angles, respectively. , These are the rates of change of roll angle, pitch angle, and yaw angle, respectively. , These are the coordinates of the tethered UAV in the body coordinate system. lower edge , , angular velocity of the axis, , and They represent trigonometric functions respectively. , as well as ; The position dynamic equation is: In the formula, For the quality of drones, Let be the velocity derivative of the UAV in the global coordinate system. , and In the global coordinate system The pulling force on the cable The decomposition results in coordinates parallel to the global coordinate system. middle , , The three components of the axis, For gravity, The total lift generated when the propeller of a tethered drone rotates; The attitude dynamics equation is the relationship between the tension and torque of a tethered UAV: In the formula, , , and These are the angular velocities required for the four propellers of a tethered drone. A constant and full-rank matrix. , The lift of tethered drones In the body coordinate system lower edge and Rolling torque generated by the shaft and pitch moment , The total counter-torque generated by the four motors of the tethered drone on the tethered drone; The equation for the influence of cables on tethered UAVs is given by the body coordinate system. The torque generated by the tension of the cable on the tethered drone Its expression is: In the formula, , and The tension of the cable on the tethered UAV in the body coordinate system. of The torque generated by the shaft and The tensile force of the cable In the body coordinate system, the tethered UAV is... and The coordinates of the axis.

5. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 1, characterized in that, In the aforementioned coordinated control system, for any one of the altitude controller, attitude controller, horizontal position controller, and heading controller, the expression for its corresponding PID controller is: In the formula, , respectively the expectations of the system Compared with the current state Laplace transform, and respectively the expected Feedback error and the control quantity output by the controller Laplace transform, , as well as These are the proportional coefficient, integral coefficient, and derivative coefficient in a PID controller. These are the filter coefficients in a low-pass filter; When the integral coefficient or differential coefficients When the integral coefficient is zero, the PID controller degenerates into a PD controller and a PI controller; when the integral coefficient is zero... With differential coefficients When both are zero, the PID controller degenerates into a P controller.

6. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 5, characterized in that, In the attitude controller: The active disturbance rejection controller includes the tethered UAV in the body coordinate system. lower edge and The axis-wide extended state observer and nonlinear state error feedback control law; among them, the tethered UAV in the body coordinate system lower edge The expression for the axis's expanded state observer is: In the formula, and ESO pairs of state variables and The estimated value, and These are the derivatives of the estimated values, To extend the state observer to the state variables The estimation error, For tethered UAVs along the body coordinate system angular velocity, , as well as It is an adjustable parameter. This is a specific function in the active disturbance rejection control algorithm. The internal disturbance of the angular velocity control loop along the x-axis of a tethered unmanned aerial vehicle (UAV) in the body coordinate system. For along Axis compensation factor, For the output of the angular velocity control loop The control parameters of the axis; Tethered UAV in body coordinate system lower edge The expression for the nonlinear state error feedback control law of the shaft is: In the formula, and It is an adjustable parameter. State variables The error, For along The desired angular velocity of the axis, The control quantity that is the final output of the active disturbance rejection controller. The derivative; Tethered UAV in body coordinate system lower edge The expression for the axis's expanded state observer is: In the formula, and For each of the extended state observers, the state variables are... and The estimated value, and ESO pairs of state variables and The estimated value, To extend the state observer to the state variables The estimation error, For tethered UAVs in the body coordinate system lower edge angular velocity, , as well as All parameters are adjustable. Internal known disturbances For along The compensation factor of the shaft. For the output of the angular velocity control loop The control parameters of the axis; Tethered UAV in body coordinate system lower edge The expression for the nonlinear state error feedback control law of the shaft is: In the formula, and It is an adjustable parameter. State variables The error, For along The desired angular velocity of the axis, The control quantity that is the final output of the active disturbance rejection controller. The derivative of .

7. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 6, characterized in that, In step S3, the desired linear velocity and velocity of the autonomous vehicle are used as inputs to the autonomous vehicle model to obtain the autonomous vehicle's position in the global coordinate system. Middle and The position of the axis and the autonomous vehicle in the global coordinate system The yaw angle is used as the expected horizontal position and expected yaw angle of the UAV.

8. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 7, characterized in that, In step S4, the desired horizontal position of the UAV is used as the input to the horizontal position controller to obtain the desired pitch angle and desired roll angle, and then the attitude controller obtains the position of the tethered UAV in the body coordinate system. lower edge and The desired roll and pitch moments of the axis are used as attitude control parameters for the UAV. The desired yaw angle of the UAV is used as the input to the heading controller to obtain the desired total anti-torque generated by the four motors of the tethered UAV. , as the heading control parameter for the drone; The desired altitude of the drone is used as the input to the altitude controller to obtain the desired lift of the tethered drone. This serves as the altitude control parameter for the drone.

9. The tethered UAV cooperative control method in the air-to-ground heterogeneous robot system according to claim 8, characterized in that, In step S5, the desired roll torque, pitch force, and desired lift are... and expected total anti-torque Substituting these values ​​into the drone model, we obtain the squares of the angular velocities required for the four propellers of the tethered drone, thus realizing the power distribution of the tethered drone.

Images