Cooperative control method for controlling suspension load by multiple unmanned aerial vehicles

Abstract

The invention discloses a cooperative control method for controlling a suspension load by multiple unmanned aerial vehicles, and the method comprises the steps: firstly constructing a frame based on a trajectory, combining an online kinematics and dynamics motion planner with an airborne trajectory tracking controller, and generating a feasible trajectory considering dynamics coupling and constraint through a finite time optimal control problem (OCP); then, an extended Kalman filter (EKF) estimator is used for fusing data of multiple unmanned aerial vehicle inertial measurement units (IMU) and a system dynamics model, and the load attitude, torsion and the cable state are estimated in real time; and finally compensating the tension interference of the cable in real time through an INDI (Incremental Nonlinear Dynamic Inversion) controller. According to the invention, the agility and robustness of the suspension load system are cooperatively improved through online motion planning and the trajectory tracking controller, the low-speed limitation of traditional cascade control is broken through, an additional sensor does not need to be deployed at the load end, and the practicability and reliability of the system under the scenes of high-speed obstacle avoidance, complex trajectory tracking and the like are remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of control algorithms and structural design, specifically relating to a collaborative control method that improves the agility and robustness of a suspension load system through online motion planning and trajectory tracking controller. Background Technology

[0002] Existing multi-UAV cooperative transportation systems typically employ cable-suspended loads. However, due to the complex dynamic coupling effect between the UAVs and the load, existing control methods (such as force-distribution-based cascaded control frameworks) struggle to achieve high-speed, high-acceleration motion. These methods rely on quasi-static assumptions or precise system dynamics models, limiting their practical application to low-speed and low-acceleration operation. Furthermore, existing solutions require additional sensors on the load (such as motion capture markers or tension sensors) for closed-loop control, increasing system complexity and deployment costs. They are also sensitive to model mismatches (such as load mass and inertia deviations), easily leading to tracking errors or even instability, thus limiting their application in time-sensitive tasks (such as search and rescue and emergency transport).

[0003] This invention proposes a trajectory planning-based framework that generates dynamically feasible trajectories by solving the full dynamics optimal control problem of a multi-UAV-payload system online. Combined with an onboard trajectory tracking controller, this significantly improves the system's flexibility, robustness, and practicality. This framework abandons the traditional cascaded control structure, directly coordinating the coupled dynamics of the UAV and payload, supporting high-speed and high-acceleration motion while avoiding dependence on additional sensors for the payload. By fusing inertial measurement unit (IMU) data with a state estimator of the dynamic model, this invention maintains stable tracking even under model mismatch and communication delays, and supports complex tasks such as dynamic obstacle avoidance and narrow-gaps, significantly expanding the applicable scenarios for cable-suspended multi-UAV systems. Summary of the Invention

[0004] The purpose of this invention is to provide a collaborative control method that improves the agility and robustness of a suspension load system through online motion planning and trajectory tracking controller, thereby solving the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a collaborative control method for multiple unmanned aerial vehicles (UAVs) manipulating suspended loads, comprising the following steps:

[0006] (1) System dynamics modeling and constraint definition: Construct a full dynamics model of the load-cable-UAV system, including the rigid body motion of the load, the tension constraint of the cable and the dynamics of the UAV, and uniformly consider dynamic coupling and safety constraints to establish a high-precision mathematical model;

[0007] (2) Online motion planning and optimal control: Design a finite-time optimal control problem (OCP), integrate the payload pose tracking target and multiple constraints, and generate a smooth trajectory through real-time iterative solution; use the multi-shot method and sequential quadratic programming (SQP) to quickly calculate the UAV reference trajectory within the future field of view;

[0008] (3) State estimation and robust trajectory tracking: Based on the extended Kalman filter (EKF) and the iterative Kabsch-Umeyama algorithm, the payload pose and cable direction are estimated in real time using only UAV IMU data; the trajectory is tracked by the incremental nonlinear dynamic inversion INDI controller to compensate for external disturbances and achieve high-precision closed-loop control without relying on the payload sensor.

[0009] Furthermore, a full dynamic model of the payload-cable-UAV system is constructed, incorporating UAV layout parameters and control parameters into a unified modeling framework; specifically, the following steps are included:

[0010] Step 1.1: Construct a load-cable dynamics model;

[0011] Step 1.2: Construct a quadrotor dynamics model;

[0012] Step 1.3: Construct kinematic constraints.

[0013] Furthermore, the specific process of constructing the load-cable dynamics model is as follows;

[0014] The load-cable dynamics model describes the six degrees of freedom motion of the load, as well as the motion of all cables attached to the load; specifically, the states of the load-cable dynamics model include:

[0015]

[0016] Where n is the number of quadrotors Let q ∈ S be the position and velocity of the load. 3 A unit quaternion to describe the load attitude. For load-fixed coordinate frame The load angular velocity represented above, denoted by subscripts i = {1, 2, ..., n}, indicates the variable of the cable connected to the i-th quadcopter. Then s i ∈S 2 It is the direction of the cable pointing from the i-th quadcopter to the load. It is the angular velocity of the i-th cable. It is the tension of the i-th cable. Let S represent the n-dimensional real space. n It is an n-dimensional unit sphere, representing the set of all points in n+1-dimensional Euclidean space that are 1 distance from the origin;

[0017] The dynamic equation for the load is:

[0018]

[0019] In the formula Let m be the load inertia, and m be the load mass. Let be the displacement of the i-th attachment point. Let q be the gravity vector. Λ(q) represents quaternion multiplication, and R(q) represents quaternion rotation. To ensure smooth acceleration of the quadcopter, the cable motion equations use the third derivative of the cable angular velocity and the second derivative of the cable tension as bounded inputs, resulting in:

[0020]

[0021] in Let λ be the third derivative of the angular velocity in the direction of the cable. i Let be the second derivative of the cable tension; when the cable remains taut, i.e., t i >0 and γ i , λ i When bounded, the trajectory p of the quadcopter drone i (t) is C 3 Smooth, meaning continuous position, velocity, acceleration, and jerk, in which case the angular velocity reference of the quadcopter drone is... C 0 Smooth, meaning continuous angular velocity;

[0022] Furthermore, the specific process of constructing the quadrotor dynamics model is as follows:

[0023] Quadrotor dynamics are established using standard rigid body dynamics. For the i-th quadrotor UAV, the state space is described as x i =[p i v i q i ω i This corresponds to the position of the center of mass in the quadcopter's body coordinate system. speed Unit quaternion rotation q i ∈S 3 Angular velocity in the quadrotor body coordinate system To represent; the dynamic equation is:

[0024]

[0025] In the formula m i and Let T be the mass and inertial matrix of the i-th quadrotor UAV, respectively; i It is the lumped thrust of the i-th quadcopter UAV; z i ∈S 2Let be the thrust direction of the i-th quadrotor UAV, and let be the coordinate system of the i-th quadrotor UAV. The z-axis direction is consistent; and These represent the air resistance and aerodynamic torque experienced by the i-th quadcopter UAV, respectively. The control torque generated by the rotor of the i-th quadcopter UAV;

[0026] Furthermore, the specific process of constructing kinematic constraints is as follows:

[0027] Assuming the cable remains taut throughout the entire operation, even during rapid motion; the position of the cable contact point in the inertial coordinate system on the i-th quadcopter UAV is represented as follows:

[0028] The kinematic constraints between the quadrotor and load-cable dynamics of the i-th quadrotor UAV are expressed as follows:

[0029]

[0030] In the formula l i Let be the length of the cable suspended during the lowering of the i-th quadcopter.

[0031] Furthermore, in step (2), based on a centralized kinematics planner, while considering the dynamic coupling between the load and the quadrotor, smooth reference trajectories for all quadrotors are generated in a backward view manner; the specific steps are as follows:

[0032] Step 2.1: Construct a model for the finite-time optimal control problem (OCP).

[0033] Step 2.2: Define dynamic constraints and path constraints; the dynamic constraints are state transition equations constructed based on the load-cable dynamic equation of formula (2) and the cable kinematic equation of formula (3); the path constraints include quadcopter thrust constraints, cable tension constraints, collision avoidance constraints, obstacle avoidance constraints and control input constraints.

[0034] Step 2.3: Solve the OCP and generate the trajectory; specifically: use the multi-shot method to discretize the OCP, and the interval of each segment increases linearly along the time domain; use the sequential quadratic programming (SQP) algorithm to solve the optimal control problem under the real-time iterative RTI framework. After calculating the optimal state sequence, use the kinematic constraints of formula (5) and its derivatives to convert them into the position, velocity, acceleration and jerk of the quadrotor, and dynamically update the quadrotor trajectory;

[0035] Step 2.4: Real-time update and trajectory tracking. The OCP is re-solved at a set frequency. The initial state provided by the load state estimator is used, and the higher-order state variables are updated by combining historical trajectory interpolation. The generated trajectory is sent to each quadrotor, and high-precision tracking is achieved through the trajectory tracking controller.

[0036] Furthermore, the specific process of modeling the finite-time optimal control problem (OCP) is as follows:

[0037] Define the state variable as the complete state vector x of the load-cable dynamics model, including: the position of the load. speed attitude quaternion q∈S 3 angular velocity The direction of each cable s i ∈S 2 angular velocity and its higher-order derivatives, as well as cable tension

[0038] Define the control input variable as the jerk in the direction of the cable. The second derivative of cable tension That is, u = [γ1, λ1, ..., γ] n , λ n ];

[0039] Construct an objective function J that includes process state error and control input error, and minimize the load pose tracking error and the smoothness of the control input;

[0040]

[0041] Where N is the number of discretization time steps, that is, the prediction time domain is discretized into N non-equidistant time steps; x k This represents the state vector of the load-cable dynamics system at the k-th time step, as shown in formula (1), u k The control input vector x at the k-th time step k,ref This represents the reference state vector of the load-cable dynamics system at the k-th time step. These reference states are pre-calculated based on the load-cable dynamics. k,ref Let x represent the reference control input vector at the k-th time step. N Let x represent the system state vector at the last time step (N). N,ref Let x represent the system reference state vector at the last time step (N), and x0 represent the system's initial state vector. init The initial state represents the optimal control problem (OCP), partly provided by the load-cable state estimator and partly obtained by resampling the trajectory generated in the previous step; x k+1=f(x) k u k ) represents the system's state transition function, describing how the system state transitions from one time step to the next. Here, the load-cable dynamics model is used (equations (2) and (3)); h(x) k+1 u k )≤0 indicates path constraints used to ensure the safety and feasibility of the system; these constraints include avoiding quadcopter overload, maintaining cable tension, avoiding internal collisions between quadcopters, and obstacle avoidance; see step 2.2 for specific constraints; Q, R, P are weighted matrices.

[0042] Furthermore, in order to update x in the planner OCP init The load pose, torsion, and cable orientation must be estimated in real time. An extended Kalman filter (EKF) is used to solve this state estimation problem. The specific steps are as follows:

[0043] Step 3.1: Construct the Extended Kalman Filter (EKF) state model: First, define the state vector as the pose and velocity of the payload, as well as the position and velocity of all quadcopters; then, implement state prediction and covariance prediction of the control input, and calculate the residuals and Kalman gain to update the state and covariance.

[0044] Step 3.2: Design the state prediction model:

[0045] State prediction is based on load dynamics equations and quadrotor dynamics equations, specifically including:

[0046] The load position, velocity, attitude and angular velocity are predicted using the load dynamics equation (2);

[0047] The position and velocity of each aircraft are predicted using the quadcopter dynamics equation (4);

[0048] The cable direction s is calculated using formula (5) based on the cable kinematic constraints. i The cable tension t was estimated by combining a spring-damping model. i :

[0049]

[0050] Where d i Let d be the distance between the position of the i-th quadcopter and its connection point on the load. i =||p i -R(q)ρ i -p||, k stiff k is the stiffness coefficient. damp The damping coefficient;

[0051] Step 3.3: Define the measurement model:

[0052] The data integrates IMU data and position / velocity measurements from the quadcopter, specifically including:

[0053] A collective thrust model was identified using accelerometer measurements. And a resistance model

[0054]

[0055] Where c t ω is the rotor thrust coefficient. j，i The rotor speed, This is the aerodynamic coefficient matrix;

[0056] According to formula (2) quadcopter dynamics, subtracting the values ​​from the accelerometer readings yields the cable thrust vector, and the cable direction is approximately:

[0057]

[0058] in These are measurements from an unbiased accelerometer.

[0059] Step 3.4: Initialize EKF state

[0060] The load pose and cable orientation were initialized using the iterative Kabsch-Umeyama algorithm.

[0061] Step 3.5: Update EKF status online

[0062] The system integrates prediction results and measurements in real time to update state estimates, including calculating Kalman gain and correcting state vectors, as well as outputting estimates of load pose, velocity, and cable orientation for subsequent motion planning and control.

[0063] Furthermore, the iterative Kabsch-Umeyama algorithm, combined with the extended Kalman filter (EKF), enables state estimation and initialization when multiple UAVs are collaboratively transporting cable-suspended loads.

[0064] The iterative Kabsch-Umeyama algorithm is used to provide initial guesses of the load attitude and cable orientation in the extended Kalman filter (EKF). The specific steps are as follows:

[0065] Step 1, Initialize parameters:

[0066] (1) Define the iterative convergence condition: position tolerance tol pos Posture tolerance (tol) att Maximum number of iterations (iter) max ;

[0067] (2) Input the number of drones n and the coordinates ρ of each cable connection point relative to the load center. i (i = 1, 2, ..., n);

[0068] (3) Calculate the average coordinates of the connection points

[0069] (4) Construct the connection point offset matrix L:

[0070] (5) Initialize load pose: position Attitude rotation matrix R = I3;

[0071] Step 2, iteratively optimize the load pose and cable orientation:

[0072] For each iteration k = 1, 2, ..., iter max Perform the following operations:

[0073] (1) Calculate the drone's end point: based on the current cable direction s i The initial value is Calculate the terminal points of each drone:

[0074] c i =p i +s i l i

[0075] Among them, l i Let p be the length of the i-th cable. i Let i be the position of the i-th drone;

[0076] (2) Calculate the mean of the endpoints With offset matrix C:

[0077] (3) Solve the rotation matrix R by performing singular value decomposition (SVD) on matrix LC:

[0078] (4) Update load position: Transform the connection point using rotation matrix R and calculate the load center p:

[0079]

[0080] (5) Update the cable direction:

[0081]

[0082] Step 3, Convergence check:

[0083] The iteration terminates if the following conditions are met:

[0084] ||pp last ||<tolpos and det(R -1 R last )<tol att

[0085] Otherwise, update p last =p and R last =R, continue iterating.

[0086] Step 4, Output the result:

[0087] Return the final load position p, attitude quaternion q(R), and cable directions s. i This serves as the initial state of EKF.

[0088] Furthermore, the trajectory tracking control method based on incremental nonlinear dynamic inverse solution (INDI) achieves high-precision tracking of the dynamic trajectory of a quadcopter UAV by fusing differential flatness characteristics with real-time IMU data, while compensating for cable tension disturbances; specifically, it includes the following steps:

[0089] Step 1: Receiving and Sampling Reference Trajectory:

[0090] Each quadcopter drone receives a reference trajectory from the central planner via a communication module. The trajectory includes position, velocity, acceleration, and jerk information within a future time window. A time sampler performs linear interpolation on the reference trajectory at a frequency of 300Hz to generate a high-frequency reference point sequence.

[0091] Step 2: Estimate external disturbances: Use the acceleration a measured by the IMU i With the filtered rotor thrust Calculate the external disturbance force f ext :

[0092]

[0093] in, For the aerodynamic drag model, D a This is the drag coefficient matrix;

[0094] Step 3: Calculate thrust command:

[0095] Based on the current position and velocity errors, and combined with the feedforward acceleration term, calculate the desired thrust direction and magnitude:

[0096]

[0097] Where T i，des Let z represent the expected thrust of the i-th quadcopter UAV. i，des p represents the desired thrust direction of the i-th quadcopter UAV. i and v iLet p represent the position and velocity of the i-th quadcopter UAV in the inertial coordinate system. i，ref and v i，ref Let K represent the desired position and desired velocity of the i-th quadcopter UAV in the inertial coordinate system, respectively. p ∈R 3×3 and Let f be the proportional and differential gain matrix. ext This is the external disturbance force, measured in real time from IMU data, m i This represents the mass of the i-th quadcopter UAV;

[0098] Step 4: Generate attitude and angular velocity references

[0099] According to the desired thrust direction z i,des To meet the zero yaw rate requirement, the desired attitude quaternion q is calculated using a tilt-prioritized control strategy. i,des With angular acceleration

[0100]

[0101] Where, ψ i，des For the desired yaw angle, satisfying , (·) is the mapping function, which converts the thrust direction and yaw angle into attitude quaternions through geometric transformation, f ext The external disturbance force is measured in real time from IMU data, K. ω For angular velocity control gain, ω i，ref Generated by the flatness property of the trajectory differential, representing the desired angular velocity;

[0102] Step 5: INDI control distributes rotor speed

[0103] Based on the incremental nonlinear dynamic inversion (INDI) algorithm, the angular acceleration command is... With thrust command T i，des Distributed to rotor speed:

[0104]

[0105] Where M is the control allocation matrix. For the desired torque, d est The disturbance compensation term is estimated using the INDI observer;

[0106] Step 6: Real-time feedback and updates:

[0107] The current state p is obtained through IMU and position sensor. i v i q iω i Closed-loop updates of thrust and attitude commands ensure that tracking errors are minimized.

[0108] Compared with existing technologies, the advantages of this invention are: This invention generates dynamically feasible trajectories by solving the full dynamics optimal control problem of a multi-UAV-payload system online, directly coordinating the coupled dynamics of the UAV and the payload, and overcoming the bandwidth limitations caused by the time-scale separation assumption in traditional cascaded control structures. Experiments show that the system can operate at speeds up to 5 m / s and 8 m / s². 2 It operates stably under acceleration, compared to existing technologies (maximum speed 1.5 m / s, acceleration 0.5 m / s²). 2 The system boasts over eight times the dynamic performance. Furthermore, it supports complex maneuvering tasks, such as high-speed passage through narrow gaps and dynamic obstacle avoidance, addressing the limitations of traditional methods in time-sensitive tasks (such as search and rescue).

[0109] This invention achieves high-precision closed-loop control without the need for additional sensors on the payload by fusing IMU data with a state estimator based on a dynamic model, significantly reducing deployment complexity and cost. Furthermore, the trajectory tracking controller compensates for external disturbances (such as model mismatch and communication delays), maintaining stability even under extreme conditions with payload mass deviations of up to 50% and communication delays as high as 0.5 seconds, overcoming the dependence of existing technologies on accurate models and high-frequency measurements. The system also supports multi-UAV collaborative expansion (verified to 9 units) and ensures safety through real-time optimization of constraints (thrust limits, obstacle avoidance), providing an efficient solution for large-scale collaborative transportation and operations in complex environments. Attached Figure Description

[0110] Figure 1 This is a schematic diagram of the system framework of the present invention.

[0111] Figure 2 The reference frame and symbols of this invention are defined as follows: Let represent the inertial frame, the load's own coordinate system, and the i-th quadrotor's body coordinate system, respectively.

[0112] Figure 3 This is a schematic diagram of the quadcopter cooperative transport payload crossing an obstacle according to the present invention.

[0113] Figure 4 This is a schematic diagram of the quadcopter cooperative transport payload of the present invention passing through a narrow space.

[0114] Figure 5 This is a flowchart of the iterative Kabsch-Umeyama algorithm of the present invention. Detailed Implementation

[0115] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0116] Please see Figure 1 , Figure 2 , Figure 3 , Figure 4 and Figure 5 This application provides a collaborative control method for multiple UAV control suspension loads. Given loads, reference attitudes, and a predefined no-fly zone, a dynamic motion planner solves the OCP problem online to generate a reference trajectory for the quadrotor. It utilizes the system's whole-body dynamics, including quadrotor and load cable models. The initial state is provided by an EKF-based estimator, and the remaining variables are obtained through resampling and trajectory prediction, avoiding quadrotor jitter. The load state estimator fuses the load-cable model and quadrotor data, with initialization relying on an iterative Kabsch-Umeyam algorithm. The trajectory tracking controller, based on INDI technology, compensates for external disturbances such as cable tension, achieving accurate trajectory tracking. Specifically, this invention includes the following steps:

[0117] System dynamics modeling and constraint definition: Construct a full dynamic model of the load-cable-UAV system, covering the rigid body motion of the load, cable tension constraints and UAV dynamics, and uniformly consider dynamic coupling and safety constraints (such as thrust limitation, collision avoidance and obstacle avoidance), and establish a high-precision mathematical framework to support subsequent planning and control.

[0118] Online motion planning and optimal control: Design a finite-time optimal control problem (OCP) that integrates payload pose tracking target with multiple constraints (dynamic, path, and input limitations). Generate a smooth trajectory through real-time iterative solution. Employ a multi-shot method and sequential quadratic programming (SQP) to quickly calculate the UAV reference trajectory within the future field of view, ensuring dynamic feasibility and agility.

[0119] State estimation and robust trajectory tracking: Based on the extended Kalman filter (EKF) and iterative Kabsch-Umeyama algorithm, the payload pose and cable orientation are estimated in real time using only UAV IMU data; the trajectory is tracked by an incremental nonlinear dynamic inversion (INDI) controller to compensate for external disturbances (such as sudden changes in cable tension), achieving high-precision closed-loop control without relying on payload sensors.

[0120] In order to achieve efficient and accurate dynamic modeling and control of the quadcopter system with cable-suspended load, in this embodiment, preferably, the full dynamic model of the load-cable-UAV system includes the rigid body motion of the load, cable tension constraints and UAV dynamics, and incorporates the UAV layout parameters and control parameters into a unified modeling framework.

[0121] Step 1: Construct a load-cable dynamics model

[0122] The load-cable dynamics model describes the six degrees of freedom motion of the load, as well as the motion of all cables attached to the load; specifically, the states of the load-cable dynamics model include:

[0123]

[0124] Where n is the number of quadrotors Let q ∈ S be the position and velocity of the load. 3 A unit quaternion to describe the load attitude. For load-fixed coordinate frame The load angular velocity represented above, denoted by subscripts i = {1, 2, ..., n}, indicates the variable of the cable connected to the i-th quadcopter. Then s i ∈S 2 It is the direction of the cable pointing from the i-th quadcopter to the load. It is the angular velocity of the i-th cable. It is the tension of the i-th cable. Let S represent the n-dimensional real space. n It is an n-dimensional unit sphere, representing the set of all points in n+1-dimensional Euclidean space that are 1 distance from the origin;

[0125] The dynamic equation for the load is:

[0126]

[0127] In the formula Let m be the load inertia, and m be the load mass. Let be the displacement of the i-th attachment point. Let q be the gravity vector. Λ(q) represents quaternion multiplication, and R(q) represents quaternion rotation. To ensure smooth acceleration of the quadcopter, the cable motion equations use the third derivative of the cable angular velocity and the second derivative of the cable tension as bounded inputs, resulting in:

[0128]

[0129] in Let λ be the third derivative of the angular velocity in the direction of the cable. i Let be the second derivative of the cable tension; when the cable remains taut, i.e., t i>0 and γ i , λ i When bounded, the trajectory p of the quadcopter drone i (t) is C 3 Smooth, meaning continuous position, velocity, acceleration, and jerk, in which case the angular velocity reference of the quadcopter drone is... C 0 Smooth, meaning continuous angular velocity;

[0130] Step 1.2: Constructing the quadrotor dynamics model

[0131] Quadrotor dynamics are established using standard rigid body dynamics. For the i-th quadrotor UAV, the state space is described as x i =[p i v i q i ω i This corresponds to the position of the center of mass in the quadcopter's body coordinate system. speed Unit quaternion rotation q i ∈S 3 Angular velocity in the quadrotor body coordinate system To represent; the dynamic equation is:

[0132]

[0133] In the formula m i and Let T be the mass and inertial matrix of the i-th quadrotor UAV, respectively; i It is the lumped thrust of the i-th quadcopter UAV; z i ∈S 2 Let be the thrust direction of the i-th quadrotor UAV, and let be the coordinate system of the i-th quadrotor UAV. The z-axis direction is consistent; and These represent the air resistance and aerodynamic torque experienced by the i-th quadcopter UAV, respectively. The control torque generated by the rotor of the i-th quadcopter UAV;

[0134] Step 1.3: Construct kinematic constraints

[0135] Assuming the cable remains taut throughout the entire operation, even during rapid motion; the position of the cable contact point in the inertial coordinate system on the i-th quadcopter UAV is represented as follows:

[0136] The kinematic constraints between the quadrotor and load-cable dynamics of the i-th quadrotor UAV are expressed as follows:

[0137]

[0138] In the formula l i Let be the length of the cable suspended during the lowering of the i-th quadcopter.

[0139] To improve the maneuverability of quadcopter unmanned aerial vehicle systems under cable-suspended loads, in this embodiment, preferably, the invented framework includes a centralized kinematic planner that generates smooth reference trajectories for all quadcopters in a backward-looking manner while taking into account the dynamic coupling between the load and the quadcopter.

[0140] Step 1: Constructing a Finite-Time Optimal Control Problem (OCP) Model

[0141] Define the state variable as the complete state vector x of the load-cable dynamics model, including: the position of the load. speed attitude quaternion q∈S 3 angular velocity The direction of each cable s i ∈S 2 angular velocity and its higher-order derivatives, as well as cable tension

[0142] Define the control input variable as the jerk in the direction of the cable. The second derivative of cable tension That is, u = [γ1, λ1, ..., γ] n , λ n ];

[0143] Construct an objective function J to minimize the load pose tracking error and the smoothness of the control input. It is a standard quadratic function consisting of two parts: process state error and control input error.

[0144]

[0145] Where N is the number of discretization time steps, that is, the prediction time domain is discretized into N non-equidistant time steps. k This represents the state vector of the load-cable dynamics system at the k-th time step, as shown in formula (1), u k The control input vector x at the k-th time step k,ref This represents the reference state vector of the load-cable dynamics system at the k-th time step. These reference states are pre-calculated based on the load-cable dynamics. k,ref Let x represent the reference control input vector at the k-th time step. N Let x represent the system state vector at the last time step (N). N,refLet x represent the system reference state vector at the last time step (N), and x0 represent the system's initial state vector. init The initial state represents the optimal control problem (OCP), provided partly by the load-cable state estimator and partly by resampling the trajectory generated in the previous step. k+1 =f(x) k u k ) represents the system's state transition function, describing how the system state transitions from one time step to the next. Here, the load-cable dynamics model is used (Equations (2) and (3)). h(x) k+1 u k )≤0 indicates path constraints used to ensure system safety and feasibility. These constraints include avoiding quadcopter overload, maintaining cable tension, avoiding internal collisions between quadcopters, and obstacle avoidance. See step 2.2 for specific constraints. Q, R, and P are weighting matrices;

[0146] Step 2.2: Define dynamic constraints and path constraints

[0147] Dynamic constraints: Based on the load-cable dynamics equation (2) and the cable kinematics equation (3), the state transition equation x is constructed. k+1 =f(x) k u k );

[0148] Path constraints include:

[0149] Quadrotor thrust constraint:

[0150] T i，min ≤T i (x)≤T i，max (7)

[0151] Among them, T i，min and T i，max Indicates minimum and maximum thrust; T i (x) is the thrust of each quadrotor as a function of the load-cable dynamics model state; where T is obtained through quadrotor dynamics using formula (4). i (x):

[0152]

[0153] in It is calculated from the second derivative of the kinematic constraints in formula (5);

[0154] Cable tension constraint:

[0155] 0 < t min ≤t i ≤tmax (9)

[0156] Among them, t min and t max Indicates the minimum and maximum cable tension;

[0157] Collision avoidance constraints:

[0158]

[0159] Where d min p is the predefined minimum distance i (x) and p j (x) represents the positions of the i-th and j-th quadcopter aircraft;

[0160] Obstacle avoidance constraints:

[0161] Using additional points on the servos and loads of each quadcopter as control points, for each obstacle and each... c (x) represents a control point. The following constraint ensures that no control point enters the no-fly zone containing the obstacle:

[0162]

[0163] In the formula To control the diagonal matrix of the no-fly zone shape, p o As the center of the no-fly zone, d o,min The safe distance from the control point to the center; assuming the position and shape of the obstacles are known, and the no-fly zone is determined at the start;

[0164] Control input constraints:

[0165] u min ≤u≤u max (12)

[0166] u min and u max These are the minimum and maximum values ​​of the input given by the user, respectively.

[0167] All path constraints are inequality constraints and are handled using slack variables to ensure the feasibility of the solver.

[0168] Step 2.3: Solve for OCP and generate trajectory

[0169] The OCP is discretized into N=20 non-equal interval nodes using the multi-shot method. The interval of each segment increases linearly along the time domain. That is, more dense discrete nodes are used in the near future period and gradually sparse in the far future period, so as to improve the fidelity of near trajectory prediction without increasing the total number of nodes.

[0170] The optimal control problem is solved within the real-time iterative RTI framework using the Sequential Quadratic Programming (SQP) algorithm. This method transforms the optimal control problem (OCP) into a constrained quadratic programming subproblem and iteratively updates the control input u. k and load-cable condition x k The solution to OCP is the optimal input along the horizon. and load-cable condition

[0171]

[0172] After calculating the optimal state sequence X*, the kinematic constraints of formula (5) and its derivatives are used to convert it into the position p of the quadcopter. i Speed ​​v i acceleration and accelerometer Satisfy C 3 Continuity and dynamic updates of the quadrotor trajectory ensure dynamic coupling with the load motion;

[0173] Step 2.4: Real-time updates and trajectory tracking

[0174] The OCP is resolved at a fixed frequency of 10Hz, using the initial state x provided by the load state estimator. init And update higher-order state variables by combining historical trajectory interpolation;

[0175] The generated trajectory is sent to each quadcopter, and high-precision tracking is achieved through the trajectory tracking controller.

[0176] To improve the practicality and cost-effectiveness of the payload-cable-quadrotor system, and to ensure real-time and accurate state estimation in actual operation, this embodiment preferably proposes a payload-cable state estimation method that does not rely on additional sensors. It uses only IMU accelerometer data and the payload-cable dynamics model on the quadrotor to estimate the payload's pose, torsion, and cable orientation in real time through an extended Kalman filter (EKF).

[0177] To update x in the planner OCP init The load pose, torsion, and cable orientation must be estimated in real time. An extended Kalman filter (EKF) is used to solve this state estimation problem. The specific steps are as follows:

[0178] Step 1: Construct the Extended Kalman Filter (EKF) state model

[0179] (1) Define the state vector as the attitude and velocity of the payload, and the position and velocity of all quadcopters, i.e.

[0180] Specifically, this includes the three-dimensional position of the load. and speed The attitude quaternion of the load q∈S 3 and angular velocity The positions of all quadcopters are: and speed Where i = 1, 2, ..., n are the aircraft numbers;

[0181] (2) Prediction phase:

[0182] State prediction:

[0183]

[0184] Where f is the system dynamics function, u k-1 This is the control input at time k-1. This represents the predicted state (prior state estimate) at time k based on the estimate at time k-1. This represents the updated state estimate (posterior state estimate) at time k-1.

[0185] Covariance prediction:

[0186]

[0187] in Let f(·) represent the state transition Jacobian matrix, which is the state transition function f(·) in the system. The linearization matrix at Q is used to locally linearize the nonlinear system. k For process noise covariance, describing the modeling uncertainty, P k|k-1 Let P represent the prior covariance matrix at time k, describing the uncertainty of the predicted state. k-1|k-1 Let represent the posterior covariance matrix at time k-1;

[0188] (3) Update phase:

[0189] Measurement residuals:

[0190]

[0191] Where y k This represents the measurement residual, reflecting the deviation between the predicted state and the actual measurement. That is, the cable direction and the position and velocity of the quadcopter are measured, where h is the measurement function, describing the mapping relationship from the state vector to the measurement vector;

[0192] Kalman gain calculation:

[0193]

[0194] where K kKalman gain is used to weigh the predicted state against the measured value. To measure the Jacobian matrix, to measure the function h(·) in The linearized matrix at point R describes the effect of state changes on the measured value. k The noise covariance is measured due to sensor measurement errors.

[0195] State and covariance updates:

[0196]

[0197] in, P represents the updated state estimate (posterior state estimate) at time k, which combines the predicted value and the measurement residual. k|k Let I represent the posterior covariance matrix at time k, which describes the uncertainty of the updated state estimate. I is the identity matrix.

[0198] Step 3.2: Design the state prediction model:

[0199] State prediction is based on load dynamics equations and quadrotor dynamics equations, specifically including:

[0200] The load position, velocity, attitude and angular velocity are predicted using the load dynamics equation (2);

[0201] The position and velocity of each aircraft are predicted using the quadcopter dynamics equation (4);

[0202] The cable direction s is calculated using formula (5) based on the cable kinematic constraints. i And combined with the spring-damping model, the cable tension t is estimated. i :

[0203]

[0204] Where d i Let d be the distance between the position of the i-th quadcopter and its connection point on the load. i =||p i -R(q)ρ i -p||, k stiff k is the stiffness coefficient. damp The damping coefficient;

[0205] Step 3.3: Define the measurement model:

[0206] The data integrates IMU data and position / velocity measurements from the quadcopter, specifically including:

[0207] A collective thrust model was identified using accelerometer measurements. And a resistance model

[0208]

[0209] Where c t ω is the rotor thrust coefficient. j，i The rotor speed, This is the aerodynamic coefficient matrix;

[0210] According to formula (2) quadcopter dynamics, subtracting the values ​​from the accelerometer readings yields the cable thrust vector, and the cable direction is approximately:

[0211]

[0212] in These are measurements from an unbiased accelerometer.

[0213] Step 3.4: Initialize EKF state

[0214] The load pose and cable orientation were initialized using the iterative Kabsch-Umeyama algorithm.

[0215] Step 3.5: Update EKF status online

[0216] The system integrates prediction results and measurements in real time to update state estimates, including calculating Kalman gain and correcting state vectors, as well as outputting estimates of load pose, velocity, and cable orientation for subsequent motion planning and control.

[0217] like Figure 5 As shown, to quickly obtain the initial guesses of the load attitude and cable orientation in the EKF, this embodiment preferably proposes an iterative Kabsch-Umeyama algorithm combined with an extended Kalman filter (EKF) to achieve state estimation and initialization when multiple UAVs are collaboratively transporting cable-suspended loads. This method significantly improves the estimation accuracy and convergence speed by iteratively optimizing the load attitude and cable orientation, and is suitable for high-speed, high-acceleration scenarios.

[0218] The iterative Kabsch-Umeyama algorithm is used to provide initial guesses of the load attitude and cable orientation in the extended Kalman filter (EKF). The specific steps are as follows:

[0219] Step 1, Initialize parameters:

[0220] (1) Define the iterative convergence condition: position tolerance tol pos Posture tolerance (tol) att Maximum number of iterations (iter) max .

[0221] (2) Input the number of drones n and the coordinates ρ of each cable connection point relative to the load center. i (i = 1, 2, ..., n).

[0222] (3) Calculate the average coordinates of the connection points

[0223]

[0224] (4) Construct the connection point offset matrix L:

[0225]

[0226] (5) Initialize load pose: position The attitude rotation matrix R = I3 (identity matrix).

[0227] Step 2, iteratively optimize the load pose and cable orientation:

[0228] For each iteration k = 1, 2, ..., iter max Among them, iter max Indicates the maximum number of iterations, and performs the following operations:

[0229] (1) Calculate the drone's terminal point:

[0230] Based on the current cable direction s i (initial value is) ), calculate the terminal point of each UAV:

[0231] c i =p i +s i l i (twenty four)

[0232] Among them, l i Let p be the length of the i-th cable. i Let be the position of the i-th drone.

[0233] (2) Calculate the mean of the endpoints With offset matrix C:

[0234]

[0235] (3) Solving for the rotation matrix using Singular Value Decomposition (SVD):

[0236] For matrix Perform SVD decomposition:

[0237]

[0238] Calculate the rotation matrix:

[0239]

[0240] (4) Update load location:

[0241] Transform the connection points using the rotation matrix R and calculate the load center p:

[0242]

[0243] (5) Update the cable direction:

[0244]

[0245] Step 3, Convergence check:

[0246] The iteration terminates if the following conditions are met:

[0247] ||pp last ||<tol pos and det(R -1 R last )<tol att (30)

[0248] Otherwise, update p last =p and R last =R, continue iterating.

[0249] Step 4, Output the result:

[0250] Return the final load position p, attitude quaternion q(R), and cable directions s. i This serves as the initial state of EKF.

[0251] To achieve high-precision trajectory tracking of a quadrotor UAV in dynamic and complex environments and effectively compensate for cable tension disturbances, this embodiment preferably develops a trajectory tracking control method based on incremental nonlinear dynamic inverse solution (INDI). By fusing differential flatness characteristics with real-time IMU data, the method enables high-precision tracking of the quadrotor UAV's dynamic trajectory while compensating for cable tension disturbances. This method significantly improves system robustness, does not rely on load sensors, and is suitable for complex dynamic environments.

[0252] The method includes the following steps:

[0253] Step 1: Receiving and Sampling Reference Trajectory

[0254] Each quadcopter drone receives a reference trajectory from the central planner via a communication module. The trajectory includes position, velocity, acceleration, and jerk information within a future time window. A time sampler performs linear interpolation on the reference trajectory at a frequency of 300Hz to generate a high-frequency reference point sequence x. ref(t):

[0255]

[0256] Where, p ref For reference position, v ref For reference speed, For reference acceleration, For reference acceleration.

[0257] Step 2: Estimate external disturbances

[0258] Acceleration a measured by IMU i With the filtered rotor thrust Calculate the external disturbance force f ext :

[0259]

[0260] in, For the aerodynamic drag model, D a This is the drag coefficient matrix;

[0261] Step 3: Calculate the thrust command

[0262] Based on the current position and velocity errors, and combined with the feedforward acceleration term, calculate the desired thrust direction and magnitude:

[0263]

[0264] Where T i，des Let z represent the expected thrust of the i-th quadcopter UAV. i,des p represents the desired thrust direction of the i-th quadcopter UAV. i and v i Let p represent the position and velocity of the i-th quadcopter UAV in the inertial coordinate system. i,ref and v i，ref Let their respective positions and velocities be the desired positions and velocities of the i-th quadrotor UAV in the inertial coordinate system. and Let f be the proportional and differential gain matrix. ext This is the external disturbance force, measured in real time from IMU data, m i This represents the mass of the i-th quadcopter UAV;

[0265] Step 4: Generate attitude and angular velocity references

[0266] According to the desired thrust direction z i，des To meet the zero yaw rate requirement, the desired attitude quaternion q is calculated using a tilt-prioritized control strategy. i,desWith angular acceleration

[0267]

[0268] Where, ψ i，des For the desired yaw angle, satisfying F(·) is a mapping function that converts the thrust direction and yaw angle into attitude quaternions through geometric transformation. ext The external disturbance force is measured in real time from IMU data, K. ω For angular velocity control gain, ω i，ref Generated by the flatness property of the trajectory differential, representing the desired angular velocity;

[0269] Step 5: INDI control distributes rotor speed

[0270] Based on the incremental nonlinear dynamic inversion (INDI) algorithm, the angular acceleration command is... With thrust command T i，des Distributed to rotor speed:

[0271]

[0272] Where M is the control allocation matrix. For the desired torque, d est The disturbance compensation term is estimated using the INDI observer;

[0273] Step 6: Real-time feedback and updates:

[0274] The current state p is obtained through IMU and position sensor. i v i q i ω i Closed-loop updates of thrust and attitude commands ensure that tracking errors are minimized.

Claims

1. A collaborative control method for manipulating suspended loads by multiple unmanned aerial vehicles (UAVs), characterized in that, It includes the following steps: (1) System dynamics modeling and constraint definition: Construct a full dynamics model of the load-cable-UAV system, including the rigid body motion of the load, the tension constraint of the cable and the dynamics of the UAV, and uniformly consider dynamic coupling and safety constraints to establish a high-precision mathematical model; (2) Online motion planning and optimal control: Design the finite-time optimal control problem OCP, integrate the load pose tracking target and multiple constraints, and generate a smooth trajectory through real-time iterative solution; A multi-shot method and sequential quadratic programming (SQP) are used to quickly calculate the reference trajectory of the UAV within the future field of view. (3) State estimation and robust trajectory tracking: Based on the extended Kalman filter (EKF) and the iterative Kabsch-Umeyama algorithm, the payload pose and cable direction are estimated in real time using only UAV IMU data; the trajectory is tracked by the incremental nonlinear dynamic inversion INDI controller to compensate for external disturbances and achieve high-precision closed-loop control without relying on the payload sensor.

2. The collaborative control method for multiple UAVs controlling suspended loads according to claim 1, characterized in that: Construct a full dynamic model of the payload-cable-UAV system, incorporating UAV layout and control parameters into a unified modeling framework; specifically, this includes the following steps: Step 1.1: Construct a load-cable dynamics model; Step 1.2: Construct a quadrotor dynamics model; Step 1.3: Construct kinematic constraints.

3. The collaborative control method for multiple UAVs controlling suspended loads according to claim 2, characterized in that: The specific process of constructing the load-cable dynamics model is as follows; The load-cable dynamics model describes the six degrees of freedom motion of the load, as well as the motion of all cables attached to the load; specifically, the states of the load-cable dynamics model include: Where n is the number of quadrotors Let q ∈ S be the position and velocity of the load. 3 A unit quaternion to describe the load attitude. For load-fixed coordinate frame The load angular velocity represented above, denoted by subscripts i = {1, 2, ..., n}, represents the variable of the cable connected to the i-th quadcopter. Then s i ∈S 2 It is the direction of the cable pointing from the i-th quadcopter to the load. It is the angular velocity of the i-th cable. It is the tension of the i-th cable. Let Sn represent the n-dimensional real space, where Sn is the n-dimensional unit sphere, representing the set of all points in the (n+1)-dimensional Euclidean space that are 1 distance from the origin. The dynamic equation for the load is: In the formula Let m be the load inertia, and m be the load mass. Let be the displacement of the i-th attachment point. Let be the gravity vector; Λ(q) represents quaternion multiplication, and R(q) represents quaternion rotation; to ensure smooth acceleration of the quadcopter, the cable motion equations use the third derivative of the cable angular velocity and the second derivative of the cable tension as bounded inputs, resulting in: in Let λ be the third derivative of the angular velocity in the direction of the cable. i Let be the second derivative of the cable tension; when the cable remains taut, i.e., t i >0 and γ i , λ i When bounded, the trajectory p of the quadcopter drone i (t) is C 3 Smooth, meaning continuous position, velocity, acceleration, and jerk, in which case the angular velocity reference of the quadcopter drone is... C 0 Smooth, meaning the angular velocity is continuous.

4. The collaborative control method for multiple UAV control and suspended loads according to claim 2, characterized in that: The specific process of constructing the quadrotor dynamics model is as follows: Quadrotor dynamics are established using standard rigid body dynamics. For the i-th quadrotor UAV, the state space is described as x i =[p i , v i q i ω i This corresponds to the position of the center of mass in the quadcopter's body coordinate system. speed Unit quaternion rotation q i ∈S 3 Angular velocity in the quadrotor body coordinate system To represent; the dynamic equation is: In the formula m i and Let T be the mass and inertial matrix of the i-th quadrotor UAV, respectively; i It is the lumped thrust of the i-th quadcopter UAV; z i ∈S 2 Let be the thrust direction of the i-th quadrotor UAV, and let be the coordinate system of the i-th quadrotor UAV. The z-axis direction is consistent; and These represent the air resistance and aerodynamic torque experienced by the i-th quadcopter UAV, respectively. The control torque generated by the rotor of the i-th quadcopter UAV.

5. The collaborative control method for multiple unmanned aerial vehicle (UAV) controlled suspended loads according to claim 2, characterized in that: The specific process of constructing kinematic constraints is as follows: Assuming the cable remains taut throughout the entire operation, even during rapid motion; the position of the cable contact point on the i-th quadcopter UAV in the inertial coordinate system is represented as follows: The kinematic constraints between the quadrotor and load-cable dynamics of the i-th quadrotor UAV are expressed as follows: In the formula l i Let be the length of the cable suspended during the lowering of the i-th quadcopter.

6. The collaborative control method for multiple unmanned aerial vehicle (UAV) controlled suspended loads according to claim 1, characterized in that: In step (2), based on a centralized kinematics planner, while considering the dynamic coupling between the load and the quadrotor, smooth reference trajectories for all quadrotors are generated in a backward view manner; the specific steps are as follows: Step 2.1: Construct a model for the finite-time optimal control problem (OCP). Step 2.2: Define dynamic constraints and path constraints; the dynamic constraints are state transition equations constructed based on the load-cable dynamic equation of formula (2) and the cable kinematic equation of formula (3); the path constraints include quadcopter thrust constraints, cable tension constraints, collision avoidance constraints, obstacle avoidance constraints and control input constraints. Step 2.3: Solve the OCP and generate the trajectory; specifically: use the multi-shot method to discretize the OCP, and the interval of each segment increases linearly along the time domain; use the sequential quadratic programming (SQP) algorithm to solve the optimal control problem under the real-time iterative RTI framework. After calculating the optimal state sequence, use the kinematic constraints of formula (5) and its derivatives to convert them into the position, velocity, acceleration and jerk of the quadrotor, and dynamically update the quadrotor trajectory; Step 2.4: Real-time update and trajectory tracking. The OCP is re-solved at a set frequency. The initial state provided by the load state estimator is used, and the higher-order state variables are updated by combining historical trajectory interpolation. The generated trajectory is sent to each quadrotor, and high-precision tracking is achieved through the trajectory tracking controller.

7. The collaborative control method for multiple unmanned aerial vehicle (UAV) controlled suspended loads according to claim 6, characterized in that: The specific process for modeling the finite-time optimal control problem (OCP) is as follows: Define the state variable as the complete state vector x of the load-cable dynamics model, including: the position of the load. speed attitude quaternion q∈S 3 angular velocity The direction of each cable s i ∈S 2 angular velocity and its higher-order derivatives, as well as cable tension Define the control input variable as the jerk in the direction of the cable. The second derivative of cable tension That is, u = [γ1, λ1, ..., γ] n , λ n ]; Construct an objective function J that includes process state error and control input error, and minimize the load pose tracking error and the smoothness of the control input; Where N is the number of discretization time steps, that is, the prediction time domain is discretized into N non-equidistant time steps; x k This represents the state vector of the load-cable dynamics system at the k-th time step, as shown in formula (1), u k The control input vector x at the k-th time step k，ref This represents the reference state vector of the load-cable dynamics system at the k-th time step. These reference states are pre-calculated based on the load-cable dynamics. k,ref Let x represent the reference control input vector at the k-th time step. N Let x represent the system state vector at the last time step (N). N,ref Let x represent the system reference state vector at the last time step (N), and x0 represent the system's initial state vector. init The initial state represents the optimal control problem (OCP), partly provided by the load-cable state estimator and partly obtained by resampling the trajectory generated in the previous step; x k+1 =f(x) k u k ) represents the system's state transition function, describing how the system state transitions from one time step to the next. Here, the load-cable dynamics model is used (equations (2) and (3)); h(x) k+1 u k )≤0 indicates path constraints used to ensure the safety and feasibility of the system; these constraints include avoiding quadcopter overload, maintaining cable tension, avoiding internal collisions between quadcopters, and obstacle avoidance; see step 2.2 for specific constraints; Q, R, P are weighted matrices.

8. The collaborative control method for multiple unmanned aerial vehicle (UAV) controlled suspended loads according to claim 1, characterized in that: To update x in the planner OCP init The load pose, torsion, and cable orientation must be estimated in real time. An extended Kalman filter (EKF) is used to solve this state estimation problem. The specific steps are as follows: Step 3.1: Construct the Extended Kalman Filter (EKF) state model: First, define the state vector as the pose and velocity of the payload, as well as the position and velocity of all quadcopters; then, implement state prediction and covariance prediction of the control input, and calculate the residuals and Kalman gain to update the state and covariance. Step 3.2: Design the state prediction model: State prediction is based on load dynamics equations and quadrotor dynamics equations, specifically including: The load position, velocity, attitude and angular velocity are predicted using the load dynamics equation (2); The position and velocity of each aircraft are predicted using the quadcopter dynamics equation (4); The cable direction s is calculated using formula (5) based on the cable kinematic constraints. i And combined with the spring-damping model, the cable tension t is estimated. i : Where d i Let d be the distance between the position of the i-th quadcopter and its connection point on the load. i =||p i -R(q)ρ i -p||, k stiff k is the stiffness coefficient. damp The damping coefficient; Step 3.3: Define the measurement model: The data integrates IMU data and position / velocity measurements from the quadcopter, specifically including: A collective thrust model was identified using accelerometer measurements. And a resistance model Where c t ω is the rotor thrust coefficient. j，i The rotor speed, This is the aerodynamic coefficient matrix; According to formula (2) quadcopter dynamics, subtracting the values ​​from the accelerometer readings yields the cable thrust vector, and the cable direction is approximately: in These are measurements from an unbiased accelerometer. Step 3.4: Initialize EKF state The load pose and cable orientation were initialized using the iterative Kabsch-Umeyama algorithm. Step 3.5: Update EKF status online The system integrates prediction results and measurements in real time to update state estimates, including calculating Kalman gain and correcting state vectors, as well as outputting estimates of load pose, velocity, and cable orientation for subsequent motion planning and control.

9. The collaborative control method for multiple unmanned aerial vehicle (UAV) controlled suspended loads according to claim 8, characterized in that: The iterative Kabsch-Umeyama algorithm, combined with the extended Kalman filter (EKF), enables state estimation and initialization when multiple UAVs are collaboratively transporting cable-suspended loads. The iterative Kabsch-Umeyama algorithm is used to provide initial guesses of the load attitude and cable orientation in the extended Kalman filter (EKF). The specific steps are as follows: Step 1, Initialize parameters: (1) Define the iterative convergence condition: position tolerance tol pos Posture tolerance (tol) att Maximum number of iterations (iter) max ; (2) Input the number of drones n and the coordinates ρ of each cable connection point relative to the load center. i (i = 1, 2, ..., n); (3) Calculate the average coordinates of the connection points (4) Construct the connection point offset matrix L: (5) Initialize load pose: position Attitude rotation matrix R = I3; Step 2, iteratively optimize the load pose and cable orientation: For each iteration k = 1, 2, ..., iter max Perform the following operations: (1) Calculate the drone's end point: based on the current cable direction s i The initial value is Calculate the terminal points of each drone: c i =p i +s i l i Among them, l i Let p be the length of the i-th cable. i Let i be the position of the i-th drone; (2) Calculate the mean of the endpoints With offset matrix C: (3) For the matrix Solve the rotation matrix R using Singular Value Decomposition (SVD): (4) Update load position: Transform the connection point using rotation matrix R and calculate the load center p: (5) Update the cable direction: Step 3, Convergence check: The iteration terminates if the following conditions are met: ||p-p last ||<tol pos and det(R -1 R last )<tol att Otherwise, update p last =p and R last =R, continue iterating; Step 4, Output the result: Return the final load position p, attitude quaternion q(R), and cable directions s. i This serves as the initial state of EKF.

10. The collaborative control method for multiple unmanned aerial vehicle (UAV) controlled suspended loads according to claim 1, characterized in that: The trajectory tracking control method based on incremental nonlinear dynamic inverse diffraction (INDI) achieves high-precision tracking of dynamic trajectories by a quadcopter UAV by fusing differential flatness characteristics with real-time IMU data, while simultaneously compensating for cable tension disturbances. Specifically, it includes the following steps: Step 1: Receiving and Sampling Reference Trajectory: Each quadcopter drone receives a reference trajectory from the central planner via a communication module. The trajectory includes position, velocity, acceleration, and jerk information within a future time window. A time sampler performs linear interpolation on the reference trajectory at a frequency of 300Hz to generate a high-frequency reference point sequence. Step 2: Estimate external disturbances: Use the acceleration a measured by the IMU i With the filtered rotor thrust Calculate the external disturbance force f ext : in, For the aerodynamic drag model, D a This is the drag coefficient matrix; Step 3: Calculate thrust command: Based on the current position and velocity errors, and combined with the feedforward acceleration term, calculate the desired thrust direction and magnitude: Where T i，des Let z represent the expected thrust of the i-th quadcopter UAV. i，des p represents the desired thrust direction of the i-th quadcopter UAV. i and v i Let p represent the position and velocity of the i-th quadcopter UAV in the inertial coordinate system. i,ref and v i，ref Let K represent the desired position and desired velocity of the i-th quadcopter UAV in the inertial coordinate system, respectively. p ∈R 3×3 and Let f be the proportional and differential gain matrix. ext This is the external disturbance force, measured in real time from IMU data, m i This represents the mass of the i-th quadcopter UAV; Step 4: Generate attitude and angular velocity references According to the desired thrust direction z i,des To meet the zero yaw rate requirement, the desired attitude quaternion q is calculated using a tilt-prioritized control strategy. i,des With angular acceleration Where, ψ i，des For the desired yaw angle, satisfying F(·) is a mapping function that converts the thrust direction and yaw angle into attitude quaternions through geometric transformation. ext The external disturbance force is measured in real time from IMU data, K. ω For angular velocity control gain, ω i，ref Generated by the flatness property of the trajectory differential, representing the desired angular velocity; Step 5: INDI control distributes rotor speed Based on the incremental nonlinear dynamic inversion (INDI) algorithm, the angular acceleration command is... With thrust command T i，des Distributed to rotor speed: Where M is the control allocation matrix. For the desired torque, d est The disturbance compensation term is estimated using the INDI observer; Step 6: Real-time feedback and updates: The current state p is obtained through IMU and position sensor. i , v i q i ω i Closed-loop updates of thrust and attitude commands ensure that tracking errors are minimized.

Images