Multi-unmanned aerial vehicle cooperative hoisting control method based on graph rigidity

Abstract

The application discloses a kind of based on graph rigidity multi-unmanned aerial vehicle cooperative hoisting control method, belong to multi-unmanned aerial vehicle cooperative control field.The method first establishes the coupling dynamics model of multi-unmanned aerial vehicle and load, constructs same height geometric constraint and graph rigidity topology;Real-time state of unmanned aerial vehicle and load is obtained every control cycle, and rope direction, average height difference, synchronization error and horizontal distance square error are calculated;Desired resultant force and moment are calculated based on load dynamics, and optimal tension is solved by quadratic programming with tension as decision variable;Horizontal expected acceleration is calculated based on graph rigidity constraint, vertical expected acceleration is calculated based on same height geometric constraint, and total expected acceleration is obtained;Desired resultant force is constructed and unmanned aerial vehicle thrust and attitude control command are generated.The method unifies trajectory tracking, attitude adjustment, tension distribution, formation keeping, same height consistency and single machine execution mapping in the same framework, and improves the control coordination and formation keeping ability of multi-unmanned aerial vehicle cooperative hoisting.

Description

Technical Field

[0001] This invention belongs to the field of multi-UAV cooperative control technology, specifically relating to a multi-UAV cooperative hoisting control method based on graph rigidity. Background Technology

[0002] In recent years, multi-UAV collaborative lifting technology has shown significant application value in tasks such as material transportation in complex environments, disaster relief, special environment operations, and load transfer in complex areas. Compared with single-UAV lifting, multi-UAV collaborative lifting can significantly improve system load capacity, redundancy, and mission adaptability, but it also introduces problems such as UAV formation constraints, cable tension coupling, load attitude stability, thrust saturation, and communication coordination.

[0003] In the collaborative lifting of rigid loads by multiple drones, the drones and the load are connected by multiple ropes. Since the rope lengths are fixed and their directions change with the relative configurations of the drones and the load, the system dynamics exhibit significant geometric coupling characteristics. The multiple drones not only need to track the desired trajectory of the load but also maintain a reasonable horizontal formation to ensure that the resultant force and moment of the load meet the desired control objectives. Simultaneously, to avoid abnormal tension or unattainable thrust due to inconsistent rope geometry, the multiple drones must also maintain a geometric relationship of equal height determined by the rope lengths and formation dimensions.

[0004] Among the existing multi-UAV hoisting control methods, some methods focus on load trajectory tracking or tension distribution, while others focus on formation maintenance. However, there is still a lack of a perfect solution that can organically integrate and collaboratively solve the above control requirements. For example, Mohammadi et al. proposed a passive-based multi-quadrotor hoisting control method (Mohammadi K, Sirouspour S, Grivani A. Passivity-Based Control of Multiple Quadrotors Carrying a Cable-Suspended Payload[J]. IEEE / ASME Transactions on Mechatronics, 2022, 27(4): 2390-2400). This hoisting scheme is an important improvement on the existing passive hoisting control, breaking through the traditional constraints such as hoisting point centering, rope tensioning, and load sensing. However, its applicable scenarios are limited to indoor small and medium-sized formation hoisting of ordinary solid goods. Under the complex wind field disturbances that require stable load attitude outdoors, the system's anti-disturbance capability is insufficient, and the load attitude may deviate significantly. For example, Lee proposed a geometric control-based method for transporting a cable-suspended ribgid body (Lee T. Geometric Control of Quadrotor UAVs Transporting a Cable-Suspended Rigid Body[J]. IEEE Transactions on Control Systems Technology, 2018, 26(1): 255-264). This method has complete coupled modeling and strong resistance to conventional disturbances. However, during formation flight, if the altitude difference between each UAV is too large, it will lead to uneven distribution of cable tension and deflection of load attitude. If the horizontal distance between UAVs is too close, there is a risk of collision. At the same time, if a sudden wind shear or violent swing of the load occurs during formation flight, it will lead to an imbalance in cable tension distribution. Summary of the Invention

[0005] The purpose of this invention is to solve the problem that existing multi-UAV collaborative hoisting control methods are difficult to achieve collaborative control under multiple constraints such as load pose tracking, formation configuration maintenance and height consistency. Instead, it proposes a control scheme that integrates system coupled dynamics modeling, graph rigid topology constraints and hierarchical collaborative control.

[0006] This invention reveals that during collaborative lifting, sub-objectives such as load trajectory tracking, load attitude stabilization, rope tension distribution, horizontal formation maintenance, geometric consistency at the same height, and single-unit thrust execution are all necessary constraints for stable system operation. However, these objectives belong to different control levels and are coupled with each other through rope direction, connection point torque arm, formation scale, and UAV thrust capability. Simply superimposing these modules can easily lead to conflicts between resultant force objectives, torque objectives, formation distance objectives, and thrust feasibility, making it difficult for the system to simultaneously satisfy multiple constraints. The reason why existing methods rarely address these issues uniformly is mainly due to the strong geometric and dynamic coupling in the multi-UAV-rope-load system. The rope can only transmit tension along its own direction, and the quadcopter can only generate thrust along the body axis, making it difficult to directly allocate load-level control objectives to each UAV for execution. Based on the coupled dynamics of multiple UAVs and loads, this invention proposes a hierarchical distributed collaborative hoisting control concept based on graph rigidity. It unifies the calculation of the expected resultant force and expected torque of the load, the quadratic programming (QP) force distribution, the horizontal rigid graph formation control, the same-height geometric control, and the single-UAV resultant force-attitude mapping into the same control flow. This coordinates the relationship between multiple objectives, reduces objective conflicts, and improves load stability, formation maintenance capability, and control execution feasibility.

[0007] To achieve the above objectives, the technical solution provided by this invention is:

[0008] A multi-UAV cooperative hoisting control method based on graph rigidity is provided, including the following steps:

[0009] Step 1: Establish a coupled dynamic model of multiple UAVs and the load, define the rope vector and the unit direction vector of the rope, and establish a translational dynamic model of a single UAV, a translational dynamic model of the load, and a load attitude dynamic model; establish geometric constraints of the same height based on the geometric relationship of rope length, and determine the expected height difference of each UAV relative to the load by combining the vertical force balance of the load; at the same time, construct an undirected communication graph and define a two-dimensional rigid matrix, and construct a horizontal formation topology that satisfies the rigidity condition through the edge and diagonal structure;

[0010] Step 2: In each control cycle, acquire the real-time motion status of each UAV and the load, and calculate the unit direction vector of the rope for the current cycle according to the rope vector definition in Step 1; calculate the average height difference of each UAV relative to the load and the synchronization error of the height difference of each UAV relative to the average height difference; calculate the squared error of the horizontal distance on each side according to the graph rigid constraints and horizontal formation topology in Step 1.

[0011] Step 3: Based on the real-time load status obtained in Step 2 and the preset load expected trajectory, calculate the expected load resultant force and expected torque based on the load translational dynamics model and load attitude dynamics model in Step 1.

[0012] Step 4: Using the expected resultant force and expected torque obtained in Step 3 as the tracking target and the rope tension as the decision variable, establish the mapping relationship between resultant force and torque based on the rope unit direction vector obtained in Step 2, and solve the optimal tension of each rope through quadratic programming.

[0013] Step 5: Calculate the desired horizontal acceleration of each UAV based on the horizontal distance squared error obtained in Step 2 and the graph rigidity constraint constructed in Step 1; simultaneously, calculate the desired vertical acceleration of each UAV based on the average height difference, synchronization error obtained in Step 2, and the same height geometric constraint and desired height difference in Step 1; combine the desired horizontal acceleration of each UAV with the desired vertical acceleration of each UAV to obtain the total desired acceleration of each UAV.

[0014] Step 6: Based on the total expected acceleration obtained in Step 5, the rope tension obtained in Step 4, and the rope unit direction vector obtained in Step 2, and combined with the translational dynamics model of the single UAV in Step 1, construct the expected resultant force of each UAV, then construct the expected attitude matrix and generate the thrust and attitude control commands that each UAV can execute, so as to realize the coordinated hoisting control of the load.

[0015] Furthermore, in step 1, an inertial coordinate system is defined, and a rope vector is defined. With the unit direction vector of the rope :

[0016]

[0017] In the formula, For the first The position of the center of gravity of the drone. The location of the center of mass of the load. For the load coordinate system The position vector of the point where the rope connects to the load relative to the center of mass of the load. The length of the rope. Let be the rotation matrix from the load coordinate system to the inertial coordinate system; then the rope's position relative to the first... The force of the drone is The force exerted on the load is ,in For the first The tension of a rope.

[0018] Furthermore, in step 1, the translational dynamics model of a single UAV is as follows:

[0019]

[0020] In the formula, For the first The quality of the drone Indicates the first The speed of drones The first derivative with respect to time, It is the acceleration due to gravity. It is a unit vector in the vertically upward direction. For the first The total thrust of the drone For the first The rotation matrix from the drone's body coordinate system to its inertial coordinate system. For the first External disturbances and modeling errors of unmanned aerial vehicles (UAVs) For the first The maximum thrust of the drone;

[0021] Introducing outer loop virtual input The attitude inner loop tracking error and the cable tension estimation error are combined into a bounded perturbation. This yields the second-order equivalent outer loop model:

[0022]

[0023] In the formula, Indicates the first The center of mass of the drone The first derivative with respect to time, This is to disturb the upper bound.

[0024] Furthermore, in step 1, the load translational dynamics model is as follows:

[0025]

[0026] The load attitude dynamics model is as follows:

[0027]

[0028] In the formula, For load quality, For load speed The first derivative with respect to time, The total number of drones, For external disturbance forces on the load, Here is the load rotational inertia matrix. Load angular velocity The first derivative with respect to time, The rotation matrix from the load coordinate system to the inertial coordinate system transpose, This represents the external disturbance torque of the load.

[0029] Furthermore, in step 1, an undirected graph is constructed. ,in For a set of nodes, Let be the set of edges; edge set The construction method is as follows: on the basis of the polygon formed by the horizontal projection positions of multiple drones, in addition to each side of the polygon, diagonals connecting non-adjacent drone nodes are added so that the current geometric configuration is within an infinitesimal rigid neighborhood.

[0030] Stack the horizontal position and horizontal velocity of each drone separately. and ,and ; set of opposite edges After sorting the edges in the array, define the edge function. for:

[0031]

[0032] In the formula, and The first frame and the first The horizontal position of the drone Denotes the Euclidean norm;

[0033] Define a two-dimensional rigid matrix as follows:

[0034]

[0035] For any edge Define the squared error of horizontal distance :

[0036]

[0037] In the formula, For the edge The expected distance;

[0038] Construct global state function And differentiate, where Let be a continuously differentiable, radially unbounded positive definite function, defined as follows: and all edges corresponding to Stacked as gradient terms ,in The squared error of each horizontal distance The overall edge error vector is thus obtained:

[0039]

[0040] In the formula, For global state functions The first derivative with respect to time, Gradient term The transpose of .

[0041] Furthermore, in step 3, the expected resultant force of the load is expressed in the inertial coordinate system as:

[0042]

[0043] In the formula, For the expected resultant force of the load, For load position error The first derivative with respect to time, For the desired load location The second derivative with respect to time, For estimation of external disturbance force of load, For load position error feedback gain, For load speed error feedback gain;

[0044] The desired torque under load is expressed in the load coordinate system as:

[0045]

[0046] In the formula, For the desired torque of the load, For load attitude error, For load angular velocity error, Load angular velocity The corresponding antisymmetric matrix, Let the load be the desired attitude matrix. Desired angular velocity for load The first derivative with respect to time, For estimating the external disturbance torque of the load, For load attitude error feedback gain, This is the load angular velocity error feedback gain.

[0047] Furthermore, in step 4, the quadratic programming solution for the optimal tension of each rope is as follows:

[0048]

[0049] In the formula, The unit direction vector of the rope The matrix formed Due to the tension of the rope The vector formed This is the mapping matrix between resultant force and torque. , and These are the weighting coefficients for the resultant force matching term, the moment matching term, and the tension uniformity term, respectively. This represents the average rope tension. This represents a vector of all 1s, used to express the average rope tension in vector form; the constraint condition is that the tension of each rope must meet the upper limit constraint of the UAV thrust.

[0050] Furthermore, in step 5, the expected horizontal acceleration of each UAV is:

[0051]

[0052] In the formula, Stack vectors representing the expected horizontal acceleration of each UAV; For the horizontal velocity error feedback gain of the UAV; For the introduced auxiliary variables, where For virtual speed input, This is to reduce the gain for the squared distance error; Input for virtual speed The first derivative with respect to time, Two-dimensional rigid matrix The transpose of .

[0053] Furthermore, in step 5, the first The desired vertical acceleration of the drone is:

[0054]

[0055] In the formula, For the first The expected vertical acceleration of the drone. Vertical coordinates of the load's centroid The second derivative with respect to time, Let the expected altitude difference between each drone relative to the payload be denoted as . Desired height difference The first derivative with respect to time, Desired height difference The second derivative with respect to time, The average altitude difference of each drone relative to the payload. Average height difference The first derivative with respect to time, and These are the feedback gain for the average height difference error and the feedback gain for the rate of change of the average height difference error, respectively. The average height difference error refers to the average height difference... Difference from expected height The error between them For the first The height difference between the drone and its payload The first derivative with respect to time, For the first The height difference of the drone Relative to average height difference Synchronization error, and These are the synchronization error feedback gain and the synchronization error rate of change feedback gain, respectively.

[0056] Furthermore, in step 6, the desired combined force of each UAV is constructed as follows:

[0057]

[0058] In the formula, For the first The combined expectations of drones For the first The total expected acceleration of the drone For the first The disturbance estimation of the drone;

[0059] Define the desired vertical axis direction of the drone body:

[0060]

[0061] In the formula, For the first The vertical axis direction of the drone's body;

[0062] In the given first Desired yaw angle of the drone Under the condition of, construct the first Desired attitude matrix of the drone ,in and According to and The desired first axis direction and desired second axis direction of the UAV body are constructed, and the desired thrust is obtained based on the magnitude of the desired resultant force:

[0063]

[0064] In the formula, For the first The expected thrust of the frame.

[0065] The advantages of this invention are:

[0066] This invention integrates load translation outer loop control, load attitude outer loop control, rope tension quadratic programming allocation, horizontal rigid formation control, height-coordinated geometric consistency control, and single-machine execution layer resultant force and attitude mapping into a single control framework, achieving collaborative solution of multiple control objectives in a multi-UAV collaborative lifting system. At the load control level, this invention generates desired resultant force and desired torque based on the load's desired trajectory and combines this with quadratic programming for rope tension allocation, ensuring that the resultant force and torque generated by multiple ropes approximate the desired control objective, thereby effectively guaranteeing the tracking accuracy of the load's position and attitude. At the formation maintenance level, this invention introduces graph rigid constraints to construct a horizontal formation topology, ensuring that multiple UAVs maintain a uniquely determined geometric configuration during movement, avoiding shear deformation, mirror flipping, or scale mismatch that may result from only satisfying local distance constraints. At the height coordination level, this invention uses height-coordinated geometric constraints and synchronization error control to ensure that the height difference between each UAV and the load remains consistent, avoiding tension anomalies or thrust infeasibility caused by inconsistent rope geometry. At the tension distribution level, this invention achieves optimized distribution of rope tension through secondary planning while meeting the upper limit constraint of UAV thrust. This allows the tension of each rope to gradually become consistent after a short period of adjustment in the early stage of lifting, enabling each UAV to share the load more evenly. Attached Figure Description

[0067] The above and other features and advantages of the present invention will become more readily understood from the following description with reference to the accompanying drawings, in which:

[0068] Figure 1 This is a flowchart of a multi-UAV cooperative hoisting control method based on graph rigidity according to an embodiment of the present invention;

[0069] Figure 2 This is a schematic diagram of the multi-UAV-load collaborative hoisting system in an embodiment of the present invention;

[0070] Figure 3 This is a schematic diagram of the multi-UAV communication topology and rigid graph constraints in an embodiment of the present invention;

[0071] Figure 4 This is a block diagram of the single-machine execution layer resultant force-attitude mapping control system in an embodiment of the present invention;

[0072] Figure 5 This is a convergence curve of load position error in an embodiment of the present invention, where (a) is the convergence curve of the composite error of load position error over time, and (b) is the curve of the load position error components in three directions changing over time.

[0073] Figure 6 This is a convergence curve of load attitude error in an embodiment of the present invention;

[0074] Figure 7The above is a convergence curve of the distance error of the horizontal rigid formation of multiple UAVs in an embodiment of the present invention, where (a) is the convergence curve of the average value of the squared distance error of each side over time, and (b) is the convergence curve of the squared distance error of each side over time.

[0075] Figure 8 The above is a convergence curve of the height difference error of multiple UAVs in an embodiment of the present invention, where (a) is the convergence curve of the average height difference over time, and (b) is the convergence curve of the individual synchronization error over time.

[0076] Figure 9 The diagram shows the tension variation and convergence curves of multiple ropes in an embodiment of the present invention, where (a) is the curve of the tension of each rope changing with time, and (b) is the curve of the tension deviation of each rope changing with time.

[0077] Figure 10 This is a three-dimensional dynamic simulation result of multi-UAV collaborative hoisting in an embodiment of the present invention. Detailed Implementation

[0078] The present invention will now be described in detail with reference to the accompanying drawings and exemplary embodiments thereof. It should be noted that the following detailed description of the present invention is for illustrative purposes only and is not intended to limit the scope of the invention.

[0079] The present invention provides a multi-UAV cooperative hoisting control method based on graph rigidity, which is used to solve multi-objective cooperative control problems such as load trajectory tracking, load attitude adjustment, rope tension distribution, formation rigidity maintenance, geometric consistency at the same height, and single-UAV execution mapping during the cooperative hoisting of rigid loads by multiple UAVs via ropes.

[0080] Reference Figure 1 As an exemplary embodiment of the present invention, a multi-UAV cooperative hoisting control method based on graph rigidity includes the following steps: establishing a coupled dynamic model of multiple UAVs and the load, and graph rigidity topological constraints as offline preparation; acquiring the real-time motion state of the system and calculating relevant error quantities as online control inputs in each control cycle; calculating the expected resultant force and expected torque of the load based on the expected trajectory of the load; solving for the optimal tension of each rope through quadratic programming; calculating the expected horizontal acceleration and expected vertical acceleration of each UAV based on graph rigidity constraints and geometric constraints at the same height, and combining them to obtain the total expected acceleration; generating executable thrust and attitude control commands for each UAV based on the total expected acceleration and rope tension, and returning to the next control cycle until the hoisting is completed. The steps are described in detail below with reference to the accompanying drawings and specific embodiments.

[0081] Step S1: Establish a coupled dynamic model of multiple UAVs and payloads and graphical rigid topological constraints.

[0082] like Figure 2As shown, this embodiment uses four quadcopter drones collaboratively hoisting a rigid load via four cables as an example. An inertial coordinate system is defined. for The unit vector in the vertically upward direction is Define the load coordinate system for Its origin is located at the center of mass of the load. The center of mass of the drone is located at , , The total number of drones, with the payload centroid located at... , No. The connection point of the rope on the load is Its position vector in the load coordinate system is Define the rope vector pointing from the drone to the load connection point. With the unit direction vector of the rope :

[0083] (1)

[0084] in, The length of the rope. In this embodiment , Let be the rotation matrix from the load coordinate system to the inertial coordinate system. The rope is always taut, of constant length, and of negligible mass; therefore, the ____ is the rotation matrix from the load coordinate system to the inertial coordinate system. The tension of a rope ( ) in the direction of Then the rope is related to the first... The force of the drone is The force exerted on the load is .

[0085] The translational dynamics model of a single UAV is as follows:

[0086] (2)

[0087] In the formula, For the first The mass of the drone in this embodiment kg, Indicates the first The speed of drones The first derivative with respect to time, For gravitational acceleration, 9.81 is used in this embodiment. , For the first The total thrust of the drone For the first The rotation matrix from the drone's body coordinate system to its inertial coordinate system. For the first External disturbances and modeling errors of unmanned aerial vehicles (UAVs) For the first The maximum thrust of the drone, i.e., the upper limit of the drone's thrust, is specified in this embodiment. N.

[0088] To construct a provable control law for convoy hoisting, an outer-loop virtual input is introduced. (Desired acceleration) summarizes unmodeled dynamics such as attitude inner loop tracking error and cable tension estimation error into a bounded disturbance. This yields the second-order equivalent outer loop model:

[0089] (3)

[0090] In the formula, Indicates the first The center of mass of the drone The first derivative with respect to time, This serves as the upper bound for the perturbation. The introduction of this second-order equivalent outer-loop model allows the upper-level controller to calculate only the desired acceleration. This eliminates the need to concern oneself with the specific implementation of underlying attitude control and thrust generation, thereby simplifying the complex nonlinear UAV dynamics into a linear model that facilitates controller design.

[0091] The load translational dynamics model is as follows:

[0092] (4)

[0093] The load attitude dynamics model is as follows:

[0094] (5)

[0095] In the formula, For load quality, in this embodiment kg, For load speed The first derivative with respect to time, For external disturbance forces on the load, This is the load rotational inertia matrix (expressed in the load coordinate system relative to the load's center of mass). Load angular velocity The first derivative with respect to time, The rotation matrix from the load coordinate system to the inertial coordinate system transpose, The external disturbance torque of the load is represented by the load dynamics model described above. This model describes the motion of the load under the action of the resultant force of the rope and the external torque, and serves as the theoretical basis for the subsequent design of the load controller.

[0096] Let the first The vertical coordinate of the center of mass of the drone is The vertical coordinate of the load centroid is Definition of the first The altitude difference between the drone and the load is:

[0097] (6)

[0098] For a symmetrical multi-UAV lifting configuration, under ideal conditions, the vertical projection of the load's center of mass coincides with the geometric center of the formation. The multiple UAVs are uniformly distributed around the load.

[0099] (7)

[0100] in, and These are the projected positions of the UAV's center of mass and the payload's center of mass in the horizontal plane (referred to as "horizontal projection positions"). This refers to the horizontal distance from the drone to the geometric center of the formation.

[0101] Under ideal symmetrical hoisting conditions, multiple drones should meet the following requirements: ,in In this embodiment, the desired height difference is uniquely determined by the current formation scale. m. This desired height difference is uniquely determined by the rope length and the formation's horizontal dimensions through geometric relationships. Combining this with the vertical force balance of the load further ensures that this height difference is within the range achievable by the UAV's thrust.

[0102] like Figure 3 As shown, to avoid shear deformation, mirror flipping, or scale mismatch in the overall configuration due to only satisfying local distance constraints during collaborative lifting by multiple UAVs, this invention introduces graph rigid constraints within the horizontal projection plane of the multiple UAVs. The horizontal projection positions of the multiple UAVs are used as nodes of the rigid graph, and the communication links and geometric distance constraints between the UAVs are used as edges of the rigid graph. By constructing a redundant rigid topology containing polygonal edges plus diagonals between UAVs, the horizontal configuration of the multiple UAVs maintains a uniquely determined geometry during movement, thereby providing a stable configuration basis for load attitude stability, cable tension distribution, and geometric control at the same height.

[0103] Specifically, constructing an undirected graph ,in For a set of nodes, Let be the set of edges. edge set The construction method is as follows: based on the polygon formed by the horizontal projection positions of multiple drones, in addition to the sides of the polygon, diagonals connecting non-adjacent drone nodes are added, so that the current geometric configuration is within an infinitesimal rigid neighborhood. Taking four drones as an example, its communication topology includes the four sides of a quadrilateral plus two diagonals, forming a redundant rigid topology, so that the horizontal configuration of multiple drones maintains a uniquely determined geometric shape during movement.

[0104] Stack the horizontal position and horizontal velocity of each drone separately. and ,and ; set of opposite edges After sorting the edges in the array, define the edge function. for:

[0105] (8)

[0106] In the formula, and The first frame and the first The horizontal position of the drone Denotes the Euclidean norm;

[0107] Define a two-dimensional rigid matrix as follows:

[0108] (9)

[0109] For any edge Define the squared error of horizontal distance :

[0110] (10)

[0111] In the formula, For the edge The expected distance.

[0112] Construct global state function And differentiate, where Let be a continuously differentiable, radially unbounded positive definite function, defined as follows: and all edges corresponding to Stacked as gradient terms ,in The squared error of each horizontal distance The overall edge error vector is thus obtained:

[0113] (11)

[0114] In the formula, For global state functions The first derivative with respect to time, Gradient term The transpose of . A two-dimensional rigid matrix. Each row corresponds to an edge, reflecting the influence of the current horizontal velocity on the rate of change of the error of each edge length. This rigid matrix transforms the horizontal formation error of multiple UAVs into edge space error dynamics. This relationship indicates that the edge error... The changes are determined by two parts: firstly, the current geometric configuration. Determined rigid matrix Secondly, the current horizontal speed Therefore, as long as a suitable horizontal speed can be designed... Or its derivative, the edge error can be directly adjusted. The evolutionary trend of this provides a theoretical basis for the design of subsequent formation control laws.

[0115] Step S2: Obtain the real-time motion state of the system and calculate the online control input.

[0116] After completing the multi-UAV-load coupled dynamics modeling and communication topology and graph rigid constraint modeling in step S1, in order for the subsequent hierarchical control law to update the control input online based on the current system state in each control cycle, it is necessary to acquire the real-time motion state of each UAV and load in each control cycle. Specifically, the centroid position, velocity, and attitude information of each UAV, as well as the centroid position, load velocity, load attitude, and load angular velocity of the load are acquired in real time. Among them, the position and attitude information can be obtained by GPS and inertial measurement unit, while the velocity and angular velocity can be directly measured by airborne sensors.

[0117] Based on the rope vector definition in step S1, calculate the unit direction vector of each rope from the current position of each UAV and the position of the load. According to the definition of height difference Calculate the altitude difference of each drone relative to the payload. The average altitude difference of each drone relative to the load was further calculated. and the altitude differences between the various drones Relative to average height difference Synchronization error Simultaneously, based on the horizontal projection position, edge set, and squared distance error defined in step S1, the squared horizontal distance error on each edge is calculated online. The aforementioned unit direction vector of the rope, average height difference, synchronization error, and squared horizontal distance error together constitute the online state input for the subsequent hierarchical control law.

[0118] Step S3: Calculate the expected resultant force and expected moment of the load.

[0119] Before implementing load translation outer loop control, the desired load trajectory is first generated based on the mission start point, target position, hoisting time, and flight safety constraints. And by taking the first and second derivatives, the desired speed is obtained. With expected acceleration The desired trajectory can be generated online by the upper-level task planner, or generated from a pre-defined analytical trajectory, piecewise polynomial trajectory, or spline trajectory.

[0120] Based on the reference trajectory, the load position error is defined. ,in Define the desired load position and define the load speed error, i.e., the load position error. First derivative with respect to time ,in Let the desired velocity of the load be denoted as . Based on the load translational dynamics model, the desired resultant force of the load in the inertial coordinate system is expressed as:

[0121] (12)

[0122] In the formula, For the expected resultant force of the load, For the desired load location The second derivative with respect to time, For estimation of external disturbance force of load, For load position error feedback gain, In this embodiment, the load speed error feedback gain is... , Ideally, it should satisfy... That is, the reaction force of the resultant force of the rope is equal to the expected resultant force.

[0123] Load attitude control employs a geometric attitude error definition. Based on the load attitude dynamics model, the load attitude error is defined. and load angular velocity error Then the desired torque under the load coordinate system is expressed as:

[0124] (13)

[0125] In the formula, For the desired torque of the load, Load angular velocity The corresponding antisymmetric matrix, Let the load be the desired attitude matrix. Desired angular velocity for load The first derivative with respect to time, For estimating the external disturbance torque of the load, For load attitude error feedback gain, In this embodiment, the load angular velocity error feedback gain is... , The goal of this attitude control law is to minimize the actual torque generated by the rope. Approximating the desired torque This enables effective control of the load posture.

[0126] Step S4: Solve for the optimal tension of the rope using quadratic programming.

[0127] The expected resultant force obtained in step S3 and expected torque This needs to be achieved by properly distributing the tension in each rope. The tension in the ropes will be... The vector formed As a decision variable, a mapping matrix between resultant force and torque is defined. for:

[0128] (14)

[0129] Then define Given a matrix consisting of unit direction vectors of the rope, we can formulate the following quadratic programming problem:

[0130] (15)

[0131] In the formula, , and These are the weighting coefficients for the resultant force matching term, the torque matching term, and the tension uniformity term, respectively. In this embodiment, , , ; This represents the average rope tension. This represents a vector of all ones, used to express the average rope tension in vector form. The objective function contains three terms: the first term makes the resultant force of the rope approximate the desired resultant force (…). The second term makes the resultant torque of the rope approximate the desired torque. The third term aims to make the tension of each rope more consistent, avoiding overloading of individual ropes. The constraint is that the tension of each rope must satisfy the upper limit constraint of the UAV's thrust. This is because the tension distribution problem not only requires the resultant force and resultant torque of the ropes to approximate the desired resultant force and desired torque, but also needs to consider the execution capabilities of each UAV. By solving this quadratic programming problem, the optimal tension of each rope is obtained.

[0132] Step S5: Calculate the desired horizontal and vertical accelerations of each UAV.

[0133] After obtaining the optimal tension for each rope, it is necessary to further determine the desired acceleration for each UAV. The desired acceleration in the horizontal direction is based on the horizontal distance squared error obtained from the rigid constraint design and calculation, while the desired acceleration in the vertical direction is based on the same-height geometric constraint design and the set desired height difference.

[0134] Specifically, introducing auxiliary variables Describe the actual horizontal speed and virtual speed input The differences between them, among which , , To reduce the gain of the squared distance error, in this embodiment , Two-dimensional rigid matrix The transpose of . Then the expected horizontal acceleration of each UAV is:

[0135] (16)

[0136] In the formula, Stack vectors representing the expected horizontal acceleration of each UAV; For the horizontal velocity error feedback gain of the UAV, in this embodiment , Input for virtual speed The first derivative with respect to time. In this formula, It is the speed error damping term. It is a virtual velocity feedforward term. It is a geometric correction term generated by the edge distance error. Through this control law, both the edge error term and the auxiliary variable term have a convergence trend, and the overall formation configuration is maintained.

[0137] To simultaneously achieve "geometric consistency tracking" and "synchronization at the same height", the first... The desired vertical acceleration of the drone is:

[0138] (17)

[0139] In the formula, For the first The expected vertical acceleration of the drone. Vertical coordinates of the load's centroid The second derivative with respect to time, Desired height difference The first derivative with respect to time, Desired height difference The second derivative with respect to time, Average height difference The first derivative with respect to time, and These are the average height difference error feedback gain and the average height difference error change rate feedback gain, respectively. In this embodiment... , The average altitude difference error refers to the average altitude difference of each drone relative to the load. Difference from expected height The error between them For the first The height difference between the drone and its payload The first derivative with respect to time, and These are the synchronization error feedback gain and the synchronization error rate of change feedback gain, respectively. In this embodiment... , This two-tiered design, combining group tracking and individual synchronization, ensures both the accuracy of the overall height level and the consistency of height within a group.

[0140] Combining the desired horizontal acceleration with the desired vertical acceleration, we obtain the first... Total expected acceleration of the drone:

[0141] (18)

[0142] In the formula, For the first The total expected acceleration of the drone For the first The expected horizontal acceleration of the drone, specifically as mentioned above. The amount.

[0143] Step S6: Generate thrust and attitude control commands that can be executed by each UAV.

[0144] Obtain the Total expected acceleration of drones and rope tension Then, it needs to be converted into thrust and attitude commands that the drone can execute.

[0145] Based on the translational dynamics model of a single UAV, in order to achieve the desired acceleration The desired combined force of all drones is:

[0146] (19)

[0147] In the formula, For the first The combined expectations of drones For the first The disturbance estimate of the drone.

[0148] Because quadcopters can only operate along the vertical axis of the drone's body. The thrust is generated along the axis, therefore the direction of the thrust of the machine body needs to be consistent with the direction of the desired resultant force. Define the desired machine body. Axial direction:

[0149] (20)

[0150] In the given first Desired yaw angle of the drone Under the condition of constructing a horizontal reference direction :

[0151] (twenty one)

[0152] This yields the orthogonal unit vector:

[0153] (twenty two)

[0154] In the formula, and According to and The desired first axis direction and the desired second axis direction of the constructed UAV body are two orthogonal axis directions within the horizontal plane of the body. , and It forms a right-handed orthogonal unit basis.

[0155] Thus constructing the first Desired attitude matrix of the drone :

[0156] (twenty three)

[0157] The desired thrust is obtained from the magnitude of the desired resultant force:

[0158] (twenty four)

[0159] In the formula, For the first The desired thrust of the UAV is determined. A geometric controller is used to track the desired attitude matrix. A thrust controller is used to track the desired thrust. This enables the tracking of the translational acceleration of the UAV, ensuring the validity of the second-order equivalent outer loop model.

[0160] The entire control process is executed cyclically within each sampling cycle: after completing step S6, it returns to step S2 to obtain the real-time status of a new round and enters the next control cycle until the load reaches the target position. Figure 4The complete mapping relationship from desired acceleration to desired thrust and desired attitude is shown. The controller calculates the desired resultant force direction into desired attitude and the desired resultant force magnitude into desired thrust, thereby converting the upper-level acceleration command into attitude and thrust commands that can be executed by the lower-level actuator.

[0161] To verify the effectiveness of the collaborative hoisting control method proposed in this invention, this embodiment uses the MATLAB simulation platform to conduct numerical simulation verification of a scenario in which four UAVs collaboratively hoist a rigid load.

[0162] The simulation parameters are set as follows: the initial centroid positions of the four UAVs are... , , , The initial speed of the four drones was , , , The initial speed of the load is The load size is m.

[0163] Figure 5 The convergence curve of the load position error is shown. As can be seen from the figure, the load position error exhibits a short-term peak at the initial stage of lifting, then rapidly decays and stabilizes within a small range. The error components in all three directions gradually converge to near zero, with the vertical direction converging faster, indicating that the system can quickly establish a stable lifting height and effectively track the desired trajectory.

[0164] Figure 6 The convergence curve of the load attitude error is shown. As can be seen from the figure, the load attitude error fluctuates to some extent in the initial stage, but quickly converges and remains at a low level, indicating that the designed attitude control and tension distribution can effectively suppress load sway and ensure attitude stability during hoisting.

[0165] Figure 7 The convergence curve of the horizontal rigid formation distance error of multiple UAVs is shown. As can be seen from the figure, the horizontal formation distance error rapidly decays to near zero after the initial adjustment, and the errors of each side basically coincide in the later stage, indicating that the formation control law based on graph rigidity of the present invention can effectively maintain the horizontal geometric configuration of the four UAVs and avoid formation deformation.

[0166] Figure 8 The figure shows the convergence curve of the height error of multiple UAVs. The average altitude difference of each drone relative to its payload Difference from expected height The average height difference error. As can be seen from the figure, the average height difference error... Synchronization error with individuals Both converged to near zero in a short time, indicating that the designed same-height control law can simultaneously achieve overall height difference tracking and vertical synchronization within the group, ensuring the stability of the hoisting geometry.

[0167] Figure 9 The figure shows the tension changes and convergence curves of the four ropes. As can be seen from the figure, the tension of the four ropes starts from zero, and after a short adjustment in the early stage of lifting, it gradually tends to be consistent. The tension deviation decreases rapidly, indicating that the secondary planning tension distribution method of the present invention can take into account the requirements of resultant force, torque matching and tension balance, so that each UAV can share the load more evenly.

[0168] Figure 10 The results of a three-dimensional dynamic simulation of four UAVs coordinating lifting are shown. As can be seen from the figure, the four UAVs first ascend synchronously, then complete the load lifting, and after a brief adjustment, enter a stable lifting phase. The actual trajectory of the load closely approximates the desired trajectory, and the four UAVs maintain a stable spatial configuration throughout, indicating that the method of this invention can completely complete the collaborative lifting task.

[0169] Based on the above simulation results, the multi-UAV cooperative hoisting control method based on graph rigidity proposed in this invention can simultaneously achieve load position tracking, load attitude stabilization, horizontal formation maintenance, geometric consistency at the same height, and optimized distribution of rope tension within a unified framework. All control indicators have good convergence performance, verifying the effectiveness and feasibility of the method.

[0170] Finally, it should be noted that the features mentioned and / or shown in the above description of exemplary embodiments of the present invention can be combined in the same or similar manner with one or more other embodiments, combined with or substituted for corresponding features in other embodiments. These combined or substituted technical solutions should also be considered to be included within the scope of protection of the present invention.

Claims

1. A multi-UAV cooperative hoisting control method based on graph rigidity, characterized in that, Includes the following steps: Step 1: Establish a coupled dynamic model of multiple UAVs and the load, define the rope vector and the unit direction vector of the rope, and establish a translational dynamic model of a single UAV, a translational dynamic model of the load, and a load attitude dynamic model; establish geometric constraints of the same height based on the geometric relationship of rope length, and determine the expected height difference of each UAV relative to the load by combining the vertical force balance of the load; at the same time, construct an undirected communication graph and define a two-dimensional rigid matrix, and construct a horizontal formation topology that satisfies the rigidity condition through the edge and diagonal structure; Step 2: In each control cycle, acquire the real-time motion status of each UAV and the load, and calculate the unit direction vector of the rope for the current cycle according to the rope vector definition in Step 1; calculate the average height difference of each UAV relative to the load and the synchronization error of the height difference of each UAV relative to the average height difference; calculate the squared error of the horizontal distance on each side according to the graph rigid constraints and horizontal formation topology in Step 1. Step 3: Based on the real-time load status obtained in Step 2 and the preset load expected trajectory, calculate the expected load resultant force and expected torque based on the load translational dynamics model and load attitude dynamics model in Step 1. Step 4: Using the expected resultant force and expected torque obtained in Step 3 as the tracking target and the rope tension as the decision variable, establish the mapping relationship between resultant force and torque based on the rope unit direction vector obtained in Step 2, and solve the optimal tension of each rope through quadratic programming. Step 5: Calculate the desired horizontal acceleration of each UAV based on the horizontal distance squared error obtained in Step 2 and the graph rigidity constraint constructed in Step 1; simultaneously, calculate the desired vertical acceleration of each UAV based on the average height difference, synchronization error obtained in Step 2, and the same height geometric constraint and desired height difference in Step 1; combine the desired horizontal acceleration of each UAV with the desired vertical acceleration of each UAV to obtain the total desired acceleration of each UAV. Step 6: Based on the total expected acceleration obtained in Step 5, the rope tension obtained in Step 4, and the rope unit direction vector obtained in Step 2, and combined with the translational dynamics model of the single UAV in Step 1, construct the expected resultant force of each UAV, then construct the expected attitude matrix and generate the thrust and attitude control commands that each UAV can execute, so as to realize the coordinated hoisting control of the load.

2. The multi-UAV cooperative hoisting control method according to claim 1, characterized in that, In step 1, an inertial coordinate system is defined, and a rope vector is defined. With the unit direction vector of the rope : In the formula, For the first The position of the center of gravity of the drone. The location of the center of mass of the load. For the load coordinate system The position vector of the point where the rope connects to the load relative to the center of mass of the load. The length of the rope. Let be the rotation matrix from the load coordinate system to the inertial coordinate system; then the rope's position relative to the first... The force of the drone is The force exerted on the load is ,in For the first The tension of a rope.

3. The multi-UAV cooperative hoisting control method according to claim 2, characterized in that, In step 1, the translational dynamics model of a single UAV is as follows: In the formula, For the first The quality of the drone Indicates the first The speed of drones The first derivative with respect to time, It is the acceleration due to gravity. It is a unit vector in the vertically upward direction. For the first The total thrust of the drone For the first The rotation matrix from the drone's body coordinate system to its inertial coordinate system. For the first External disturbances and modeling errors of unmanned aerial vehicles (UAVs) For the first The maximum thrust of the drone; Introducing outer loop virtual input The attitude inner loop tracking error and the cable tension estimation error are combined into a bounded perturbation. This yields the second-order equivalent outer loop model: In the formula, Indicates the first The center of mass of the drone The first derivative with respect to time, This is to disturb the upper bound.

4. The multi-UAV cooperative hoisting control method according to claim 3, characterized in that, In step 1, the load translational dynamics model is as follows: The load attitude dynamics model is as follows: In the formula, For load quality, For load speed The first derivative with respect to time, The total number of drones, For external disturbance forces on the load, Here is the load rotational inertia matrix. Load angular velocity The first derivative with respect to time, The rotation matrix from the load coordinate system to the inertial coordinate system transpose, This represents the external disturbance torque of the load.

5. The multi-UAV cooperative hoisting control method according to claim 4, characterized in that, In step 1, an undirected graph is constructed. ,in For a set of nodes, Let be the set of edges; edge set The construction method is as follows: on the basis of the polygon formed by the horizontal projection positions of multiple drones, in addition to each side of the polygon, diagonals connecting non-adjacent drone nodes are added so that the current geometric configuration is within an infinitesimal rigid neighborhood. Stack the horizontal position and horizontal velocity of each drone separately. and ,and ; set of opposite edges After sorting the edges in the array, define the edge function. for: In the formula, and The first frame and the first The horizontal position of the drone Denotes the Euclidean norm; Define a two-dimensional rigid matrix as follows: For any edge Define the squared error of horizontal distance : In the formula, For the edge The expected distance; Construct global state function And differentiate, where Let be a continuously differentiable, radially unbounded positive definite function, defined as follows: and all edges corresponding to Stacked as gradient terms ,in The squared error of each horizontal distance The overall edge error vector is thus obtained: In the formula, For global state functions The first derivative with respect to time, Gradient term The transpose of .

6. The multi-UAV cooperative hoisting control method according to claim 5, characterized in that, In step 3, the expected resultant force of the load is expressed in the inertial coordinate system as: In the formula, For the expected resultant force of the load, For load position error The first derivative with respect to time, For the desired load location The second derivative with respect to time, For estimation of external disturbance force of load, For load position error feedback gain, For load speed error feedback gain; The desired torque under load is expressed in the load coordinate system as: In the formula, For the desired torque of the load, For load attitude error, For load angular velocity error, Load angular velocity The corresponding antisymmetric matrix, Let the load be the desired attitude matrix. Desired angular velocity for load The first derivative with respect to time, For estimating the external disturbance torque of the load, For load attitude error feedback gain, This is the load angular velocity error feedback gain.

7. The multi-UAV cooperative hoisting control method according to claim 6, characterized in that, In step 4, the quadratic programming solution for the optimal tension of each rope is as follows: In the formula, The unit direction vector of the rope The matrix formed Due to the tension of the rope The vector formed This is the mapping matrix between resultant force and torque. , and These are the weighting coefficients for the resultant force matching term, the moment matching term, and the tension uniformity term, respectively. This represents the average rope tension. This represents a vector of all 1s, used to express the average rope tension in vector form; the constraint condition is that the tension of each rope must meet the upper limit constraint of the UAV thrust.

8. The multi-UAV cooperative hoisting control method according to claim 6, characterized in that, In step 5, the expected horizontal acceleration for each UAV is: In the formula, Stack vectors representing the expected horizontal acceleration of each UAV; For the horizontal velocity error feedback gain of the UAV; The auxiliary variables introduced, where For virtual speed input, This is to reduce the gain for the squared distance error; Input for virtual speed The first derivative with respect to time, Two-dimensional rigid matrix The transpose of .

9. The multi-UAV cooperative hoisting control method according to claim 8, characterized in that, In step 5, the first The desired vertical acceleration of the drone is: In the formula, For the first The expected vertical acceleration of the drone. Vertical coordinates of the load's centroid The second derivative with respect to time, Let the expected altitude difference between each drone relative to the payload be denoted as . Desired height difference The first derivative with respect to time, Desired height difference The second derivative with respect to time, The average altitude difference of each drone relative to the payload. Average height difference The first derivative with respect to time, and These are the feedback gain for the average height difference error and the feedback gain for the rate of change of the average height difference error, respectively. The average height difference error is the average height difference... Difference from expected height The error between them For the first The height difference between the drone and its payload The first derivative with respect to time, For the first The altitude difference of the drone Relative to average height difference Synchronization error, and These are the synchronization error feedback gain and the synchronization error rate of change feedback gain, respectively.

10. The multi-UAV cooperative hoisting control method according to any one of claims 6 to 9, characterized in that, In step 6, the desired resultant force of each drone is constructed as follows: In the formula, For the first The combined expectations of drones For the first The total expected acceleration of the drone For the first The disturbance estimation of the drone; Define the desired vertical axis direction of the drone body: In the formula, For the first The vertical axis direction of the drone's body; In the given first Desired yaw angle of the drone Under the condition of, construct the first Desired attitude matrix of the drone ,in and According to and The desired first axis direction and desired second axis direction of the UAV body are constructed, and the desired thrust is obtained based on the magnitude of the desired resultant force: In the formula, For the first The expected thrust of the frame.

Images