A trajectory tracking control method for high-speed maneuvering flight of a multi-machine hoisting system

Abstract

The present invention discloses a trajectory tracking control method for high-speed maneuvering flight of a multi-machine hoisting system. The method establishes a dynamic model of a multi-machine hoisting system taking into account the time-varying tension of the rope, and constructs a differential flat mapping to calculate the high-order state / control input of the hoisted quadrotor as reference information for its position and attitude control algorithm. The position dynamics of the quadrotor are abstracted as a second-order integral system subject to Lipschitz disturbance, and a new terminal sliding surface and a fixed-time integral controller are proposed to realize external force estimation and ensure that the system error converges within a fixed time. Based on the reference information calculated by the flat mapping, a fixed-time position controller and an attitude geometry controller of the multi-machine hoisting system are further designed. Based on the propeller speed measurement, the external torque of the rope acting on the quadrotor is estimated and compensated, and an expected throttle calculation method based on closed-loop speed control is designed to improve the speed control accuracy and thus improve the trajectory tracking control accuracy.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle tracking control, and in particular relates to a trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system. Background Art

[0002] Multi-rotor UAVs (UAVs for short) have the advantages of low cost, easy maintenance, vertical take-off and landing, and strong maneuverability, and are widely used in military and civilian fields. The existing UAV logistics solution is to transport heavy objects by attaching them to UAVs. However, since the heavy objects need to be stored in a regular space, the attachment solution will limit the configuration of the heavy objects. In addition, the attachment solution will change the overall dynamic parameters of the system, so the robustness of the system control is extremely high. Another solution is to tie the heavy objects to the UAV through a rope (called a single-machine lifting system). The single-machine lifting system is suitable for the transportation of larger and more complex heavy objects. However, due to the limitation of the load capacity of a single UAV, the single-machine lifting system solution has a limited scope of application for the weight of heavy objects. Therefore, the solution of using a multi-machine lifting system composed of multiple UAVs to transport heavy objects has attracted widespread attention at home and abroad. The multi-machine lifting system improves the scope of application for the weight of heavy objects. The load capacity of the system can be increased by increasing the number of UAVs in the multi-machine lifting system, without the need to design UAV platforms with different load capacities due to different heavy object handling requirements. Therefore, the multi-machine lifting system is more flexible.

[0003] The motion control module of the multi-machine lifting system is one of the most critical modules. The characteristics of motion coupling between heavy objects and drones in the multi-machine lifting system put forward two clear requirements for the motion control algorithm of the multi-machine lifting system. a) Each drone in the multi-machine lifting system is affected by the tension of the rope, and the force is at the same order of magnitude as the weight of the drone. The existence of external forces / external torques requires the drone to accurately estimate and compensate for the external forces and torques caused by the tension of the rope. b) The tracking error of the multi-machine lifting system affects the safety of the system. The control error of the drone is often large when performing maneuvering flight at high speed and large attitude. The existence of control errors causes the actual tracking trajectory of the system to deviate from the planned trajectory, causing the drone control to oscillate, and then cause the drones in the system to collide, causing safety hazards. Therefore, it is necessary to reduce the control error of the system when executing high-speed maneuvering trajectories.

[0004] The existing control scheme is a control scheme based on the closed loop of the weight position as the core feature, and the characteristic of this scheme is the existence of a centralized force distribution link. The closed loop of the weight position is based on the measurement of the weight position, so the controller given by this scheme depends on the position and speed tracking error of the weight. In addition, there are countless possible force distribution schemes required for the movement of heavy objects in a multi-machine lifting system. Such schemes generally distribute the force required for the control of the heavy object movement to each drone according to the principle of minimum force norm. However, the force distribution link is centralized. After each drone obtains the force allocated to itself, it controls the relative position between itself and the weight to provide the force required by the weight. The disadvantage of this scheme is that the system delay is relatively large, and the force distribution result needs to be distributed to each drone through the communication link, which causes communication delay. In addition, the use of the minimum force norm distribution scheme cannot take into account the obstacle avoidance requirements of the drone in the air. After the force distribution is completed, the desired target position of the drone is uniquely determined, and this method is difficult to apply in an obstacle-dense environment. More importantly, after the drone obtains the force distribution result, it needs to adjust its relative position with the weight and change the direction of the rope to provide the required force. The posture adjustment process of the drone will also cause a certain delay in the force applied to the weight. Therefore, this solution is not suitable for high-speed maneuvering flight of a multi-machine hoisting system.

[0005] For the high-speed maneuvering flight of multi-machine hoisting systems, the following problems have not been solved: 1. There is no method to calculate the control feedforward signal for the multi-machine hoisting system affected by rope tension. The existing trajectory tracking does not have a precise feedforward control signal, which restricts the high-speed maneuvering flight of the system; 2. For the second-order integral system, how to construct a fixed-time controller using only the position error and velocity error signals without relying on the observer, to estimate and compensate for the rope tension, and realize the third-order sliding mode, that is, the position error, velocity error and the derivative of the velocity error converge to zero within a fixed time period; 3. The existing control scheme with the closed-loop position of heavy objects as the core feature has a centralized calculation link and a large delay. There is still a lack of distributed low-latency control schemes with drones as the core feature suitable for the high-speed maneuvering flight of multi-machine hoisting systems at home and abroad. Summary of the invention

[0006] In view of the shortcomings of the prior art, the present invention proposes a trajectory tracking control method for high-speed maneuverable flight of a multi-machine hoisting system. The control method can meet the high-precision control requirements of fully autonomous high-speed maneuverable flight of the multi-machine hoisting system, consider the influence of wind resistance on the UAVs during high-speed flight and the time-varying tension of the hoisting ropes, reduce the tracking control accuracy of the system's high-speed maneuverable flight, and improve the high-speed maneuverable flight capability of the multi-machine hoisting system.

[0007] The technical solution of the present invention is as follows:

[0008] The technical solution of the present invention is as follows: According to a first aspect of the present invention, a trajectory tracking control method for high-speed maneuvering flight of a multi-machine hoisting system is provided, comprising the following steps: (1) dynamic modeling of the UAV of the multi-machine hoisting system based on the time-varying external force of the rope;

[0009] (2) Based on the motion trajectory of the weight, describe the trajectory of the force and force vector acting on each rope. Based on the differential flatness property of the drone, calculate the reference state information of the drone, namely, the reference thrust, reference angular velocity, and reference angular acceleration.

[0010] (3) The position dynamics of the quadrotor of the UAV is abstracted as a second-order integral system subject to Lipschitz perturbation. For the second-order integral system subject to Lipschitz perturbation, two continuously differentiable fractional-order functions are defined, and the terminal sliding surface and fixed-time integral controller are designed.

[0011] (4) Based on the fixed-time integral controller designed in step (3), a fixed-time position controller of the UAV under the influence of rope tension is designed according to the reference velocity and reference acceleration, and the desired attitude, desired angular velocity and desired thrust of the UAV are further solved;

[0012] (5) Based on the measurement of the propeller speed of the quadrotor, the external torque of the rope acting on the quadrotor is estimated; and based on the current measured attitude, angular rate, desired attitude, and desired angular velocity, an exponentially stable attitude tracking geometry controller is designed; and the external torque caused by the rope is compensated, and the control torque required for the attitude tracking of the quadrotor is calculated by the dynamic model;

[0013] (6) Calculate the desired throttle based on closed-loop speed control, that is, map the desired thrust and control torque obtained in steps (4)-(5) to the desired speed required for control through the mixing matrix based on the thrust coefficient and torque coefficient of the propeller, and design a closed-loop speed controller to control the propeller speed tracking error caused by voltage changes during high-speed maneuvering flight. Fit the change relationship between the propeller throttle and the speed based on experimental data, and then map the closed-loop speed expectation of the propeller to the desired throttle required for the four propellers.

[0014] Furthermore, the step (1) is specifically as follows:

[0015] The dynamic modeling of the UAV in the multi-machine hanging system is carried out, that is, the single hanging quadrotor in the system is modeled to obtain the dynamic model, and its expression is as follows:

[0016] Where m, J are the mass of the quadrotor and its moment of inertia around the center of mass, p, v are the position and velocity in the world coordinate system, R is the rotation matrix between the body axis system and the world coordinate system, ω is the angular velocity, is the antisymmetric matrix corresponding to the angular velocity; f and τ are thrust and control torque respectively; F is the tension applied by the rope on the body, q is the vector direction of the rope, which points from the weight to the drone, and is determined by the pitch angle θ and heading angle of the rope. The only confirmed expression is as follows:

[0017] Where Fq is the pulling force of the rope on the quadrotor, e3 = [0, 0, 1]

[0018] Furthermore, the step (2) is specifically as follows: since the suspended UAV has a differentially flat property, its state quantity and control quantity can be uniquely determined by the flat output and its finite-order derivative; since there is a kinematic constraint between the UAV and the weight when the rope is tensioned, the position of the weight and the vector of the rope need to be used to indirectly represent the position of the UAV to eliminate the kinematic constraint between the UAV and the weight. First, let p L represents the position of the weight, ψ represents the heading angle of the drone, l represents the length of the rope connected between the drone and the weight, and q represents the vector direction of the rope. The expression of the drone position p is as follows:

[0019] p=p L +lq;

[0020] Select p L ,ψ,F,θ, is the flat output variable of the UAV; for the reference position p of the UAV r , reference speed v r , reference acceleration a r , reference acceleration j r , using the flat output variable p L ,θ, And its higher-order derivatives are uniquely determined by continuous differentiation of the above formula: Where a is the acceleration of the drone, a L is the acceleration of the mass;

[0021] The reference attitude, reference angular velocity, reference angular acceleration and reference thrust of the UAV are obtained from the planned flat output variable trajectory;

[0022] The X-axis vector x in the body coordinate system b = Re1 and Y-axis vector y b =Re2 and the UAV translational dynamics equation, and the expression is as follows: From this we can get:

[0023] From this we can see that the vector At the same time, it is perpendicular to the X-axis and Y-axis of the body coordinate system, thereby obtaining the Z-axis of the body coordinate system, which is also the unit vector direction of the reference thrust, and its expression is as follows:

[0024] Then, the Z-axis vector z in the body coordinate system is b =Re3 multiply by the UAV translational dynamics equation:

[0025]

[0026] The expression for thrust is as follows:

[0027] Now the Z-axis direction and yaw angle of the body coordinate system are known, and the yaw rotation quaternion is:

[0028]

[0029] The tilt rotation quaternion is:

[0030] The attitude quaternion is in represents the quaternion multiplication operator, from which the attitude rotation matrix can be uniquely determined as R = R quat (q);

[0031] Then the body rotation dynamics equation The angular velocity expression of the UAV is:

[0032]

[0033] Finally, q z ,q ψ Substituting the expression into the equation, we get the analytical expression of the angular velocity ω of the drone:

[0034]

[0035] Among them, z b，1 , z b，2 , z b，3 Represents the Z axis of the body coordinate system b The first, second, and third components of , ψ is the heading angle of the drone, cψ, sψ represent cos(ψ), sin(ψ), respectively;

[0036] Further, taking the derivative of the above formula, we get the angular acceleration The subscripts 1, 2, and 3 represent the first, second, and third components of the angular acceleration vector, respectively, and the expressions are as follows:

[0037]

[0038] Furthermore, the fixed-time integral controller in step (3) is a general controller proposed for a second-order integral system subject to Lipschitz disturbance; the controller can observe and compensate for the Lipschitz disturbance without constructing an observer; since the controller only relies on position and velocity information, even in the presence of Lipschitz disturbance, a third-order sliding mode can be achieved, that is, the position, velocity and acceleration converge to zero within a fixed time period; and the controller uses an integral terminal sliding mode surface, which can avoid controller chattering.

[0039] Furthermore, the step (3) is specifically as follows:

[0040] The position dynamics of the UAV is abstracted as a second-order integral system subject to Lipschitz perturbation, and x1 and x2 are used to represent the state of the system, that is, x1 represents the position of the UAV and x2 represents the speed of the UAV; the expression is as follows:

[0041] Where u is the control input, d is the Lipschitz disturbance, and satisfies

[0042] For any Defining functions Where sign() is a symbolic function, and the continuous differentiable function is defined by the system state quantities x1 and x2.

[0043]

[0044] in To control the parameters, is a constant, x1 and x2 are the position and velocity of the drone respectively; therefore, the continuous differentiable function Design a fixed-time integral controller: Among them, u is the control input, s is a continuously differentiable function The integral terminal sliding surface is defined as is its derivative.

[0045] Furthermore, in step (4), a fixed time position controller of the drone under the influence of rope tension is designed by using the reference velocity and reference acceleration; specifically:

[0046] First, define the position control error as e p =p r -p, speed control error is e v =v r -v, then the system error dynamics equation is:

[0047] in And a r represents the reference acceleration; then design the fixed time position controller u:

[0048]

[0049] Then the desired thrust vector direction of the UAV is:

[0050] The expected thrust is: f = m||ge3 + a r -u||2;

[0051] Furthermore, from the desired thrust vector direction z b and the reference heading angle ψ ref , then the quaternion q representing the desired posture is obtained des , which is calculated in the same way as the attitude quaternion q.

[0052] Furthermore, the step (5) is specifically as follows:

[0053] Since the rope force vector cannot always pass through the center of mass of the drone, it is necessary to estimate and compensate the external torque caused by the rope in real time; first, the thrust coefficient k of the rotor needs to be measured. T and torque coefficient k τ ; The thrust of the drone is:

[0054] In addition, the torque of a single rotor is: τ = k τ r 2 ;

[0055] First, add different weights to the drone, let the drone hover in the air, and record the rotor speed data after the drone with different loads stabilizes at a certain height; secondly, use a force measuring bench to fix the rotor on the force measuring bench, increase the rotor speed through the force measuring software, and obtain the torque values ​​corresponding to different speeds; based on these data, use the least squares method to solve the thrust coefficient k of the drone T and torque coefficient k τ ;

[0056] Based on the measurement of the propeller speed of the quadrotor, the external torque of the rope acting on the quadrotor is estimated. First, the thrust coefficient k T and torque coefficient k τ Estimate the rotor control torque τ c :

[0057]

[0058] Where r1, r2, r3, r4 are the rotor speeds, and b is the wheelbase of the drone. The external torque τ acting on the drone is estimated by the attitude dynamics equation. ext : Where J is the UAV’s moment of inertia around its center of mass, ω est is the angular velocity estimate after low-pass filtering, is the angular acceleration estimated by the tracking differentiator;

[0059] The quaternion of the error between the current attitude of the drone and the actual attitude is:

[0060] Calculate the feedback angular velocity from the attitude error quaternion:

[0061] where k roll , k pitch , k psi are the roll angle, pitch angle, and heading angle error control parameters respectively;

[0062] The desired angular velocity is obtained from the reference angular velocity and the feedback angular velocity: ω des =ω ref +ω fb ;

[0063] The error between the expected angular velocity and the actual angular velocity is:

[0064]

[0065] Calculate the feedback angular acceleration:

[0066] The expected angular acceleration is obtained from the reference angular acceleration and the feedback angular acceleration:

[0067] From this, the expected control torque of the UAV can be obtained:

[0068] Furthermore, the step (6) is specifically as follows:

[0069] The desired thrust f and the desired control torque τ are used to solve the desired rotation speed r of the four rotors through the inverse matrix of the mixing matrix. des，1 , r des，2 , r des，3 , r des，4 ; First, we need to fit the curve of the corresponding relationship between the throttle and the speed r, and model the corresponding relationship between the throttle and the speed as a piecewise quadratic polynomial curve. The expression is as follows:

[0070]

[0071] Where a0, a1, a2, a3, a4, and a5 are the polynomial coefficients that need to be fitted. Different throttle values ​​are repeatedly given, and the speed corresponding to the throttle value is recorded. The parameters of the throttle speed curve are solved by the least square method. Since the battery voltage will affect the rotor speed corresponding to the throttle value, it is necessary to control the speed through a proportional-integral (PI) controller. Specifically: First, the feedforward term thro of the desired throttle is solved through the fitted throttle speed curve. ref :

[0072]

[0073] Then, according to the difference between the current actual speed and the expected speed, the proportional feedback term thro is calculated. fbp :

[0074] thro fbp =k p (r des -r est );

[0075] In order to eliminate the speed steady-state error caused by voltage drop, the integral feedback term thro fbi ;

[0076] thro fbi =k i ∫(r des -r est )dt;

[0077] where k p , k i are proportional feedback coefficient and integral feedback coefficient respectively, r des , r est are the expected speed and the actual speed respectively, so the final output throttle is:

[0078] thro=thro ref +k p (r des -r est )+k i ∫(r des -r est )dt.

[0079] According to a second aspect of the present invention, there is provided an electronic device, comprising:

[0080] one or more processors;

[0081] A memory for storing one or more programs;

[0082] When the one or more programs are executed by the one or more processors, the one or more processors implement the trajectory tracking control method for high-speed maneuverable flight of a multi-aircraft hoisting system.

[0083] According to a third aspect of the present invention, there is provided a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the steps of a trajectory tracking control method for high-speed maneuvering flight of a multi-machine hoisting system.

[0084] The beneficial effects of the present invention are as follows:

[0085] The present invention proposes a method for calculating feedforward instructions for UAV control affected by time-varying tension of the rope. The method can solve the desired thrust, desired angular velocity and desired angular acceleration of the UAV through the differential flat variables of the multi-machine hoisting system. By solving the feedforward instructions of the high-speed maneuvering trajectory, it is helpful to improve the tracking accuracy of the high-speed maneuvering trajectory and reduce the tracking error. The present invention proposes a fixed-time integral controller for the second-order integral system subject to Lipschitz disturbance. The method has the following advantages: 1) The controller does not need to construct an observer, and can realize the observation and compensation of Lipschitz disturbance; 2) The controller only depends on the position and velocity information. Even in the presence of Lipschitz disturbance, the third-order sliding mode can still be realized, that is, the position, velocity and acceleration converge to zero within a fixed time period; 3) Compared with the traditional sliding mode controller, the controller proposed by the present invention uses the integral of the sliding mode surface, so that the controller greatly reduces the "jittering" phenomenon and ensures the continuity of the controller. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0087] Figure 1 is a force analysis diagram of the multi-machine hoisting system of the present invention;

[0088] Figure 2 is a trajectory tracking control flow chart of the present invention;

[0089] Figure 3 is a framework diagram of the present invention for real-time estimation of the external force applied by the rope to the drone;

[0090] Figure 4 It is a schematic diagram of the flight trajectory of the three-machine hanging system of the present invention;

[0091] Figure 5 This is the position tracking diagram of the No. 1 suspended UAV of the present invention. DETAILED DESCRIPTION

[0092] Here, exemplary embodiments are described in detail, and examples thereof are shown in the accompanying drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application.

[0093] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "said" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0094] The present invention proposes a robust trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system. The overall framework is as follows: Figure 1 The method includes the following steps:

[0095] S1: Dynamic modeling of the UAV in a multi-machine hoisting system is carried out, and the model considers the influence of the time-varying external force of the rope on the UAV.

[0096] S2: Calculation of reference state information. The trajectory of the weight describes the force and force vector direction of each rope. Based on the differential flatness of the drone, the reference thrust, reference angular velocity, and reference angular acceleration of the drone are calculated.

[0097] S3: Design a fixed-time integral controller. The position dynamics of the quadrotor is abstracted as a second-order integral system subject to Lipschitz perturbations, and a fixed-time integral sliding mode controller is proposed. Define two continuously differentiable fractional-order functions based on On this basis, an integral terminal sliding surface s is designed, and an integral sliding fixed time controller u is proposed, which can accurately estimate the disturbance through the integral sliding surface s. The algorithm has the following advantages: 1) The controller u does not need to build an observer, and can observe and compensate for Lipschitz disturbances; 2) The controller u only depends on the position and velocity information, and can still achieve third-order sliding mode even in the presence of Lipschitz disturbances, that is, the position, velocity and acceleration converge to zero within a fixed time period; 3) Compared with the traditional sliding mode controller, the controller u uses an integral terminal sliding surface, which greatly reduces the "jittering" phenomenon and ensures the continuity of the controller.

[0098] S4: Based on the fixed-time integral controller designed in S3, and using the reference velocity and reference acceleration, a fixed-time position controller is designed for the UAV under the influence of rope tension, and the desired attitude, desired angular velocity and desired thrust of the UAV are further solved.

[0099] S5: Based on the measurement of the propeller speed of the quadrotor, the external torque of the rope acting on the quadrotor is estimated. The exponentially stable attitude tracking geometry controller can be designed based on the current measured attitude, angular velocity, and the expected attitude and expected angular velocity law. On this basis, the external torque caused by the rope is compensated, and the control torque required for the attitude tracking of the quadrotor is calculated by the dynamic model.

[0100] S6: Design a method for calculating the expected throttle based on closed-loop speed control. The thrust coefficient and torque coefficient of the propeller are obtained from experimental tests, and the expected thrust and torque obtained in steps S4 and S5 are mapped to the expected speed required for control through the mixing matrix. Since the actual speed of the quadcopter propeller will be affected by the voltage of the power battery, the same throttle value corresponds to different propeller speeds under different voltages. Therefore, the present invention will further design a closed-loop controller for the speed to cope with the propeller speed tracking error caused by the continuous change of voltage during high-speed maneuvering flight, and to improve the control accuracy of the system solution under high-speed maneuvering flight conditions. Based on the experimental data, the changing relationship between the propeller throttle and the speed is fitted, and then the closed-loop speed expectation of the propeller is mapped to the expected throttle required for the four propellers.

[0101] S1. UAV dynamics modeling considering time-varying tension in the rope

[0102] The force analysis diagram of the multi-machine hoisting system and some symbol definitions are as follows Figure 1 shown.

[0103] First, the dynamic model of a single suspended quadrotor in the system is established.

[0104]

[0105] Where m, J are the mass of the quadrotor and its moment of inertia around the center of mass, p, v are the position and velocity in the world coordinate system, R is the rotation matrix between the body axis system and the world coordinate system, ω is the angular velocity, is the antisymmetric matrix corresponding to the angular velocity. f,τ are the thrust and control torque respectively. F is the tension applied by the rope on the body, q is the vector direction of the rope, which points from the weight to the drone, and is determined by the pitch angle θ and heading angle of the rope. The only thing that is certain is that Fq is the pulling force of the rope on the quadrotor.

[0106]

[0107] in e3=[0,0,1].

[0108] S2. Calculation of reference state information

[0109] The suspended UAV has the property of differential flatness, and its state and control quantities can be uniquely determined by the flat output and its finite-order derivative. Since there is a kinematic constraint between the UAV and the heavy object when the rope is tensioned, in order to eliminate the kinematic constraint between the UAV and the heavy object, the present invention uses the position of the heavy object and the vector of the rope to indirectly represent the position of the UAV, and let p L represents the position of the weight, ψ represents the heading angle of the drone, l represents the length of the rope connected between the drone and the weight, and q represents the vector direction of the rope. The expression of the drone position p is as follows:

[0110] p=p L +lq;

[0111] Select p L ,ψ,F,θ, is the flat output variable of the UAV. Obviously, for the reference position p of the UAV r , reference speed v r , reference acceleration a r , reference acceleration j r , can use the flat output variable p L ,θ, and its higher-order derivatives are uniquely determined by continuous differentiation of the above equation.

[0112]

[0113] Where a is the acceleration of the drone, a L is the acceleration of the weight.

[0114] The acquisition of trajectory feedforward information will help reduce the control error of high-speed maneuvering trajectory and improve the control accuracy of trajectory. Unlike traditional quadcopters, all drones in the multi-rotor hanging system are affected by the time-varying tension of the rope, and the flat mapping process of their thrust and reference angular velocity is also more complicated. In order to meet the needs of high-speed maneuvering flight, the present invention will obtain the reference attitude, reference angular velocity, reference angular acceleration and reference thrust of the drone from the planned flat output variable trajectory.

[0115] The X-axis vector x in the body coordinate system b = Re1 and Y-axis vector y b =Re2 and the UAV translational dynamics equation, and the expression is as follows:

[0116]

[0117] From this we can get:

[0118]

[0119] From this we can see that the vector At the same time, it is perpendicular to the X-axis and Y-axis of the body coordinate system, so the Z-axis of the body coordinate system can be obtained, which is the unit vector direction of the reference thrust, and its expression is as follows:

[0120]

[0121] Then, the Z-axis vector z in the body coordinate system is b =Re3 multiply by the UAV translational dynamics equation:

[0122]

[0123] The thrust expression is as follows:

[0124]

[0125] Now that the Z-axis direction and yaw angle of the body coordinate system are known, the yaw rotation quaternion can be obtained as:

[0126]

[0127] The tilt rotation quaternion is:

[0128]

[0129] The attitude quaternion is in represents the quaternion multiplication operator, from which the attitude rotation matrix can be uniquely determined as R = R quat (q);

[0130] Then the body rotation dynamics equation The angular velocity expression of the ejector body is:

[0131]

[0132] Finally, q z ,q ψ Substituting the expression into the equation, we can get the analytical expression of the angular velocity ω of the drone:

[0133]

[0134] Among them, z b,1 ,z b,2 ,z b,3 They represent the first, second, and third components of the Z-axis direction of the body coordinate system, ψ is the heading angle of the drone, and cψ and sψ represent cos(ψ) and sin(ψ), respectively.

[0135] Further, taking the derivative of the above formula, we get the angular acceleration The subscripts 1, 2, and 3 represent the first, second, and third components of the angular acceleration vector respectively:

[0136]

[0137] S3. Design a fixed-time integral sliding mode controller.

[0138] The position dynamics of the UAV is abstracted into a second-order integral system subject to Lipschitz perturbation. x1 and x2 are both state quantities of the system, x1 represents the position of the UAV, and x2 represents the speed of the UAV. The expression is as follows:

[0139]

[0140] Where u is the control input, d is the Lipschitz disturbance, and satisfies

[0141] For any Defining functions Where sign() is a symbolic function, and the continuous differentiable function is defined by the system state quantities x1 and x2.

[0142]

[0143] in To control the parameters, is a constant, x1 and x2 are the position and velocity of the drone respectively; therefore, the continuous differentiable function Design a fixed time integral controller, the expression is as follows:

[0144]

[0145] Among them, u is the control input, s is a continuously differentiable function The integral terminal sliding surface is defined as is its derivative.

[0146] S4. Design of fixed time position controller for multi-machine hoisting system

[0147] The present invention proposes a trajectory tracking control scheme suitable for high-speed maneuvering flight of a multi-machine hanging system, such as Figure 2 As shown. For its fixed time position controller, first define the position control error as e p =p r -p, speed control error is e v =v r -v, then the system error dynamics equation is:

[0148]

[0149] in And a r represents the reference acceleration; then design the fixed time position controller u:

[0150]

[0151] Then the desired thrust vector direction of the UAV is:

[0152]

[0153] The expected thrust is:

[0154] f=m||ge3+a r -u||2.

[0155] Furthermore, from the desired thrust vector direction z b and the reference heading angle ψ ref , then the quaternion q representing the desired posture is obtained des , and its calculation method is the same as that of the attitude quaternion q (refer to the calculation method of the attitude quaternion q in step S2).

[0156] S5. Design of exponentially stable attitude geometry controller

[0157] Since the rope force vector cannot always pass through the center of mass of the drone, it is necessary to estimate and compensate the external torque caused by the rope in real time; this step first requires measuring the thrust coefficient k of the rotor T and torque coefficient k τ ; The thrust of the drone is:

[0158]

[0159] In addition, the torque of a single rotor is:

[0160] τ=k τ r 2 ;

[0161] First, add different weights to the drone, let the drone hover in the air, and record the rotor speed data after the drone with different loads stabilizes at a certain height. Then, use a force measuring bench to fix the rotor on it, and use the force measuring software to slowly increase the rotor speed and obtain the torque values ​​corresponding to different speeds. Based on these data, use the least squares method to solve the thrust coefficient k of the drone. T and torque coefficient k τ The framework for real-time estimation of the external force applied by the rope to the drone is as follows Figure 3 shown.

[0162] Based on the measurement of the propeller speed of the quadrotor, the external torque of the rope acting on the quadrotor is estimated. First, the thrust coefficient k T and torque coefficient k τ Estimate the rotor control torque τ c :

[0163]

[0164] Where r1, r2, r3, r4 are the rotor speeds, and b is the wheelbase of the drone. The external torque τ acting on the drone is estimated by the attitude dynamics equation. ext ,

[0165]

[0166] Where J is the UAV’s moment of inertia around its center of mass, ω est is the angular velocity estimate after low-pass filtering, is the angular acceleration estimated by the tracking differentiator.

[0167] The quaternion of the error between the current attitude of the drone and the actual attitude is:

[0168]

[0169] Calculate the feedback angular velocity from the attitude error quaternion:

[0170] ω fb,1 =k roll sign(q e,w )q e,x

[0171] ω fb,2 =k pitch sign(q e,w )q e,y

[0172] ω fb,3 =k psi sign(q e,w )q e,z ;

[0173] where k roll ,k pitch ,k psi They are the roll angle, pitch angle, and heading angle error control parameters respectively.

[0174] The desired angular velocity is obtained from the reference angular velocity and the feedback angular velocity:

[0175] ω des =ω ref +ω fb ;

[0176] The error between the expected angular velocity and the actual angular velocity is:

[0177]

[0178] Calculate the feedback angular acceleration:

[0179]

[0180] The expected angular acceleration is obtained from the reference angular acceleration and the feedback angular acceleration:

[0181]

[0182] From this, the expected control torque of the UAV can be obtained:

[0183]

[0184] S6 calculates the desired throttle for the rotor

[0185] The desired thrust f and desired control torque τ obtained from steps S4 and S5 can be used to solve the desired rotation speed r of the four rotors through the inverse matrix of the mixing matrix. des，1 , r des，2 , r des，3 , r des，4 First, we need to fit the curve of the corresponding relationship between the throttle and the speed r, and model the throttle speed corresponding relationship as a piecewise quadratic polynomial curve:

[0186]

[0187] Among them, a0, a1, a2, a3, a4, and a5 are the polynomial coefficients that need to be fitted. Different throttle values ​​are repeatedly given, and the speed corresponding to the throttle value is recorded. The parameters of the throttle speed curve are solved using the least squares method. Since the battery voltage will affect the rotor speed corresponding to the throttle value, a proportional-integral (PI) controller is required to accurately control the speed.

[0188] First, the feedforward term thro of the desired throttle is calculated by fitting the throttle speed curve. ref :

[0189]

[0190] Then, according to the difference between the current actual speed and the expected speed, the proportional feedback term thro is calculated. fbp :

[0191] thro fbp =k p (r des -r est );

[0192] In order to eliminate the speed steady-state error caused by voltage drop, the integral feedback term thro fbi :

[0193] thro fbi =k i ∫(r des -r est )dt.

[0194] where k p , k i are proportional feedback coefficient and integral feedback coefficient respectively, r des , r est are the expected speed and the actual speed respectively, so the final output throttle is:

[0195] thro=thro ref +k p (r des -r est )+k i ∫(r des -r est )dt.

[0196] The present invention designs a robust trajectory tracking control framework for high-speed maneuvering flight of a multi-machine hoisting system. A fixed-time position tracking controller is designed to estimate and compensate for the external force of the rope, ensuring that the position error dynamics converges to zero within a fixed time. An external torque estimation method based on speed measurement and attitude dynamics is proposed, and an attitude geometry controller based on external torque compensation is designed to ensure the exponential stability of the attitude error.

[0197] The present invention proposes a throttle speed curve fitting method and a throttle compensation method for solving the voltage change problem, which effectively solves the speed tracking error caused by battery voltage change, thereby improving the control accuracy of acceleration and angle, and further improving the accuracy of trajectory tracking.

[0198] The present invention has completed the flight experiment of the three-machine hanging system, and its flight trajectory is as follows Figure 4 As shown in the figure, the three light-colored trajectories are the flight trajectories of the three suspended drones, and the dark-colored trajectories are the movement trajectories of the loads. In the maximum speed trajectory of this trajectory, the maximum acceleration of the suspended drone is 7m / s2, the maximum speed is 4.3m / s, and the maximum axis angle is 41.6°.

[0199] Experimental data such as Figure 5 As shown in the figure, the position tracking diagram of the No. 1 suspended UAV is shown. The horizontal axis represents the X direction and the vertical axis represents the Y direction. The units are both meters. The dark track is the actual position of the UAV and the light track is the expected position sent. The average position tracking error is 5.9 cm.

[0200] You skilled in the art will easily think of other embodiments of the present application after considering the specification and practicing the contents disclosed herein. This application is intended to cover any variation, use or adaptation of the present application, which follows the general principles of the present application and includes common knowledge or customary technical means in the technical field not disclosed in this application.

[0201] It should be understood that the present application is not limited to the exact construction that has been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof.

Claims

1. A trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system, characterized in that: The following steps are involved: (1) Dynamic modeling of the UAV in a multi-machine hoisting system based on the time-varying external force of the rope; (2) Based on the motion trajectory of the weight, describe the trajectory of the force and force vector acting on each rope. Based on the differential flatness property of the drone, calculate the reference state information of the drone, including reference thrust, reference angular velocity, and reference angular acceleration. (3) The position dynamics of the quadrotor of the UAV is abstracted as a second-order integral system subject to Lipschitz perturbation. For the second-order integral system subject to Lipschitz perturbation, two continuously differentiable fractional-order functions are defined, and the terminal sliding surface and fixed-time integral controller are designed. (4) Based on the fixed-time integral controller designed in step (3), a fixed-time position controller of the UAV under the influence of rope tension is designed according to the reference velocity and reference acceleration, and the desired attitude, desired angular velocity and desired thrust of the UAV are further solved; (5) Based on the measurement of the propeller speed of the quadrotor, the external torque of the rope acting on the quadrotor is estimated; and based on the current measured attitude, angular velocity, and the desired attitude and angular velocity, an exponentially stable attitude tracking geometry controller is designed; and the external torque caused by the rope is compensated, and the control torque required for the attitude tracking of the quadrotor is calculated by the dynamic model; specifically: Since the rope force vector cannot always pass through the center of mass of the drone, it is necessary to estimate and compensate the external torque caused by the rope in real time; first, the thrust coefficient k of the rotor needs to be measured. T and torque coefficient k τ ; The thrust of the drone is: Among them, r is the rotation speed. In addition, the torque of a single rotor is: t s =k τ r 2 ; First, add different weights to the drone, let the drone hover in the air, and record the rotor speed data after the drone's height stabilizes; secondly, use a force measuring bench to fix the rotor on the force measuring bench, increase the rotor speed through the force measuring software, and obtain the torque value τ corresponding to different speeds s Based on these data, the thrust coefficient k of the UAV is solved using the least squares method T and torque coefficient k τ ; Based on the measurement of the propeller speed of the quadrotor, the external torque of the rope acting on the quadrotor is estimated. First, the thrust coefficient k T and torque coefficient k τ Estimate the rotor control torque τ c : Where r1, r2, r3, r4 are the rotor speeds, and b is the wheelbase of the drone. The external torque τ acting on the drone is estimated by the attitude dynamics equation. ext : Where J is the UAV’s moment of inertia around its center of mass, ω est is the angular velocity estimate after low-pass filtering, is the angular acceleration estimated by the tracking differentiator; The quaternion of the error between the current attitude of the drone and the actual attitude is: Calculate the feedback angular velocity from the attitude error quaternion: ω fb,1 =k roll sign(q e,w )q e,x ω fb,2 =k pitch sign(q e,w )q e,y ω fb,3 =k psi sign(q e,w )q e,z ; where k roll ,k pitch ,k psi They are the error control parameters of the roll angle, pitch angle, and heading angle respectively; The desired angular velocity is obtained from the reference angular velocity and the feedback angular velocity: oh des =ω ref +oh fb ; The error between the expected angular velocity and the actual angular velocity ω e for: Calculate the feedback angular acceleration: The expected angular acceleration is obtained from the reference angular acceleration and the feedback angular acceleration: From this, the expected control torque of the UAV can be obtained: (6) Calculate the desired throttle based on closed-loop speed control. Based on the thrust coefficient and torque coefficient of the propeller, the desired thrust and control torque obtained in steps (4)-(5) are mapped to the desired speed required for control through the mixing matrix, and a closed-loop speed controller is designed to control the propeller speed tracking error caused by voltage changes during high-speed maneuvering flight; fit the change relationship between the propeller throttle and the speed based on experimental data, and then map the closed-loop speed expectation of the propeller to the desired throttle required for the four propellers.

2. The trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system according to claim 1 is characterized in that: The step (1) is specifically: The dynamic modeling of the UAV in the multi-machine hanging system is to model the single hanging quadrotor in the system and obtain the dynamic model, which is expressed as follows: Where m, J are the mass of the quadrotor and its moment of inertia around the center of mass, p, v are the position and velocity in the world coordinate system, R is the rotation matrix between the body axis system and the world coordinate system, ω is the angular velocity, is the antisymmetric matrix corresponding to the angular velocity; f, τ are the thrust and control torque respectively; F is the tension applied by the rope on the body, q is the vector direction of the rope, which points from the weight to the drone, and is determined by the pitch angle θ and heading angle of the rope. The only confirmed expression is as follows: Where Fq is the pulling force of the rope on the quadrotor, e3=[0,0,1].

3. The trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system according to claim 2 is characterized in that: The step (2) is specifically as follows: since the suspended UAV has a differentially flat property, its state quantity and control quantity can be uniquely determined by the flat output and its finite-order derivative; since there is a kinematic constraint between the UAV and the weight when the rope is tensioned, the position of the weight and the vector of the rope need to be used to indirectly represent the position of the UAV, thereby eliminating the kinematic constraint between the UAV and the weight, and setting p L represents the position of the weight, ψ represents the heading angle of the drone, l represents the length of the rope connected between the drone and the weight, and q represents the vector direction of the rope. The expression of the drone position p is as follows: p=p L +lq; Select p L ,ψ,F,θ, is the flat output variable of the UAV; for the reference position p of the UAV r , reference speed v r , reference acceleration a r , reference acceleration j r , using the flat output variable p L ,θ, And its higher-order derivatives are uniquely determined by continuous differentiation of the above formula: Where a is the acceleration of the drone, a L Indicates the acceleration of the weight; The reference attitude, reference angular velocity, reference angular acceleration and reference thrust of the UAV are obtained from the planned flat output variable trajectory; The X-axis vector x in the body coordinate system b = Re1 and Y-axis vector y b =Re2 and the UAV translational dynamics equation, and the expression is as follows: From this we can get: From this we can see that the vector At the same time, it is perpendicular to the X-axis and Y-axis of the body coordinate system, thereby obtaining the Z-axis of the body coordinate system, which is the unit vector direction of the reference thrust, and the expression is as follows: Then, the Z-axis vector z in the body coordinate system is b =Re3 multiply by the UAV translational dynamics equation: The expression for thrust is as follows: Now the Z-axis direction and yaw angle of the body coordinate system are known, and the yaw rotation quaternion is obtained as: The tilt rotation quaternion is: The attitude quaternion is in represents the quaternion multiplication operator, from which the attitude rotation matrix can be uniquely determined as R = R quat (q); Then the body rotation dynamics equation The angular velocity expression of the UAV is: Finally, q z ,q ψ Substituting the expression into the equation, we get the analytical expression of the angular velocity ω of the drone: Among them, z b,1 ,z b,2 ,z b,3 Represents the Z axis of the body coordinate system b The first, second, and third components of , ψ is the heading angle of the UAV, and cψ, sψ represent cos(ψ) and sin(ψ), respectively; Taking the derivative of the above equation, we get the angular acceleration The subscripts 1, 2, and 3 represent the first, second, and third components of the angular acceleration vector, respectively, and the expressions are as follows:

4. The trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system according to claim 1 is characterized in that: The fixed-time integral controller in step (3) is a general controller proposed for a second-order integral system subject to Lipschitz disturbance; the controller can observe and compensate for the Lipschitz disturbance without constructing an observer; since the controller only relies on position and velocity information, it can realize third-order sliding mode even in the presence of Lipschitz disturbance, and make the position, velocity and acceleration converge to zero within a fixed time period; and the controller uses an integral terminal sliding mode surface, which can avoid controller chattering.

5. The trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system according to claim 3 is characterized in that: The step (3) is specifically: The position dynamics of the UAV is abstracted as a second-order integral system subject to Lipschitz perturbation, and x1 and x2 are used to represent the state of the system. x1 and x2 represent the position and velocity of the UAV. The expression is as follows: Where u is the control input, d is the Lipschitz disturbance, and satisfies For any Defining functions Where sign() is a symbolic function, and the continuous differentiable function is defined by the system state quantities x1 and x2. in To control the parameters, is a constant; therefore, by the continuously differentiable function Design a fixed-time integral controller: Among them, u is the control input, s is a continuously differentiable function The integral terminal sliding surface is defined as is its derivative.

6. The trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system according to claim 5, characterized in that: In the step (4), the fixed time position controller of the drone under the influence of rope tension is designed by using the reference speed and reference acceleration; specifically: First, define the position control error as e p =p r -p, speed control error is e v =v r -v, then the system error dynamics equation is: in And a r represents the reference acceleration; Then design the fixed time position control law u: Then the desired thrust vector direction of the UAV is: The expected thrust is: <h2 style=";text-align:left;direction:ltr">f = m || ge3 + a<h2 style=";text-align:left;direction:ltr"> r <h2 style=";text-align:left;direction:ltr"> -u||2; The desired thrust vector direction z b and the reference heading angle ψ ref , then the quaternion q representing the desired posture is obtained des , which is calculated in the same way as the attitude quaternion q.

7. The trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system according to claim 1, characterized in that: The step (6) is specifically as follows: The desired thrust f and the desired control torque τ are used to solve the desired rotation speed r of the four rotors through the inverse matrix of the mixing matrix. des，1 , r des，2 , r des，3 , r des，4 ; First, we need to fit the curve of the corresponding relationship between the throttle and the speed r, and model the throttle speed corresponding relationship as a piecewise quadratic polynomial curve. The expression is as follows: Where a0, a1, a2, a3, a4, a5 are the polynomial coefficients to be fitted, c is the dividing throttle between the piecewise quadratic polynomial curves, different throttle values ​​are given repeatedly, the speed corresponding to the throttle value is recorded, and the parameters of the throttle speed curve are solved by the least squares method; since the battery voltage will affect the rotor speed corresponding to the throttle value, it is necessary to control the speed through a proportional-integral controller; specifically: First, the feedforward term thro of the desired throttle is calculated by fitting the throttle speed curve. ref : Then, according to the difference between the current actual speed and the expected speed, the proportional feedback term thro is calculated. fbp : thro fbp =k p (r des -r est ); In order to eliminate the speed steady-state error caused by voltage drop, the integral feedback term thro fbi ; thro fbi =k i ∫(r des -r est )dt; where k p ,k i are proportional feedback coefficient and integral feedback coefficient respectively, r des ,r est are the expected speed and the actual speed respectively, so the final output throttle is: thro=thro ref +k p (r des -r est )+k i ∫(r des -r est )dt。 8. An electronic device, characterized in that: include: one or more processors; A memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement a trajectory tracking control method for high-speed maneuverable flight of a multi-aircraft hoisting system as described in any one of claims 1-7.

9. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the instruction is executed by the processor, the steps of a trajectory tracking control method for high-speed maneuvering flight of a multi-machine hanging system as described in any one of claims 1-7 are implemented.

Images