Multi-uav suspension load safe agile transportation planning and control method

Abstract

The present application belongs to the technical field of complex environment safe flight of unmanned aerial vehicle, and particularly relates to a multi-unmanned aerial vehicle suspended load safe agile transportation planning and control method. First, a coupled dynamics model of a transportation system composed of multiple unmanned aerial vehicles, cables and loads is established, the load position and the direction angle of each cable are taken as flat outputs, the load trajectory and the cable direction trajectory are mapped into the position and state of each unmanned aerial vehicle, and the whole system safety constraint in the obstacle environment is constructed. Then, the flat outputs are segmented and polynomial parameterized, the bounded constraint variables are converted into unbounded optimization variables through differential homeomorphism mapping, the trajectory optimization problem is solved in real time, and the safe reference trajectory is generated. Finally, each unmanned aerial vehicle performs distributed tracking control according to the local state, the expected total thrust and the estimated cable force according to the generated trajectory. At the same time, according to the safety margin, the cable tension margin, the tracking error, the target point change and the load mass estimation error, the online re-planning of the transportation system is triggered.

Description

Technical Field

[0001] This invention belongs to the field of safe flight technology for unmanned aerial vehicles in complex environments, and specifically relates to a method for planning and controlling the safe and agile transportation of multiple unmanned aerial vehicle (UAV) suspended payloads. Background Technology

[0002] In unmanned aerial vehicle (UAV) transport systems, multiple UAVs collaborate to move large, heavy, or irregularly shaped objects. Multiple UAVs suspend the load together via multiple cables, distributing the thrust of a single UAV across multiple actuators. Furthermore, spatial formation adjustments enhance the flexibility of load transport, making it highly valuable for handling tasks in complex environments.

[0003] In existing technologies, multi-UAV suspended transport typically employs a centralized trajectory planning and centralized control framework, or assigns the desired positions to each UAV after a given payload trajectory. While such solutions can achieve basic transport in open environments, under low-speed movement, or external motion capture conditions, they still face challenges in scenarios involving narrow passages, dense obstacles, inaccurate payload mass, and temporary changes in target position. These challenges include high computational demands, incomplete obstacle avoidance constraints, uncontrollable cable tension, and insufficient control robustness.

[0004] The challenge of multi-UAV suspended payload systems lies in the strong coupling between the payload, cables, and UAVs. If only the payload trajectory is constrained while ignoring the sweep space of the UAVs and cables, situations can easily arise where the payload avoids obstacles but the UAVs or cables collide with them. On the other hand, directly optimizing the high-dimensional states of all UAVs and payloads would make real-time solutions difficult and hinder rapid replanning in the event of sudden changes in mission requirements.

[0005] Furthermore, existing multi-UAV suspended transport solutions often rely on payload-end status measurement, cable orientation sensors, or high-precision external positioning systems. In actual deployment, it may be inconvenient to install sensors on the payload, the cable may vibrate and have non-ideal connections, and the payload mass may change with the mission. Therefore, there is a need for a multi-UAV suspended transport method that can ensure safety at the system level while enabling real-time planning and distributed robust control. Summary of the Invention

[0006] To address the problems of existing multi-UAV suspended transport methods in complex obstacle environments, such as high planning dimensionality, incomplete system-level obstacle avoidance, difficulty in handling cable direction and tension constraints, strong load state dependence, and insufficient response to sudden replanning, this invention proposes a safe and agile transport planning and control method for multi-UAV suspended loads. This method uses the load position and cable azimuth angle as low-dimensional flat outputs. During the trajectory planning stage, it uniformly considers the safety constraints of the load, UAVs, and cables. During the control stage, each UAV independently estimates the cable force and performs distributed robust tracking. This allows for safe and agile suspended transport in complex environments without relying on dedicated state sensors at the load end. The specific technical solution is as follows:

[0007] A method for planning and controlling safe and agile transportation of multiple unmanned aerial vehicles (UAVs) carrying suspended loads includes:

[0008] Step 1: Establish a coupled dynamics model of the transportation system consisting of multiple drones, cables, and loads;

[0009] Step 2: Using the load position and the direction angle of each cable as the flat output of the system, the position and status of each UAV are mapped through the load trajectory and the direction trajectory of the cable.

[0010] Step 3: Considering the payload shape, UAV shape, UAV spacing, cable sweep body, cable tension, cable direction, and UAV dynamics feasibility, construct the full system safety constraints in the obstacle environment;

[0011] Step 4: Perform piecewise polynomial parameterization on the flat output, convert bounded constraint variables into unbounded optimization variables through differential homeomorphism mapping, and solve the trajectory optimization problem in real time to generate a safe reference trajectory;

[0012] Step 5: Each UAV performs distributed tracking control based on its own status, expected total thrust, and estimated cable force, according to the generated trajectory.

[0013] Step 6: Based on the safety margin, cable tension margin, tracking error, target point change, and load mass estimation error, trigger the online replanning, safety degradation, or mission termination strategy of the transportation system.

[0014] Furthermore, in step one, the coupled dynamics model includes:

[0015] Translational dynamics model of the load:

[0016]

[0017] in, Indicates the load mass; Indicates the load acceleration; Represents gravitational acceleration; Represents the vertical unit vector in the world coordinate system; Indicates the number of drones involved in the suspended transport; Indicates the first Cable tension; Indicates the first The cable points from the load to the drone in a unit direction vector; This represents the combined term of external load disturbances and unmodeled errors;

[0018] No. Geometric constraints of the unmanned aerial vehicle (UAV) and its payload without elongation cable:

[0019]

[0020] in, Indicates the first Location of the drone; Indicates the load location; Indicates the first Length of the cable; Indicates the first Unit vector in the direction of the root cable;

[0021] And, the translational dynamics model of the UAV:

[0022]

[0023] in, Indicates the first The quality of the drone; Indicates the acceleration of the drone; Indicates the total thrust of the drone; This represents the attitude rotation matrix of the UAV; This represents wind disturbance, thrust error, and unmodeled dynamic disturbance.

[0024] Furthermore, step two specifically involves:

[0025] Define a flat output as:

[0026]

[0027] Generate the first [number] based on the direction angle. Root cable direction vector:

[0028]

[0029] in, This represents the flat output vector of the system. Indicates the load location; Indicates the first The pitch angle along the direction of the cable; Indicates the first The azimuth angle of the cable direction; Indicates transpose; and Let cosine and sine functions represent the functions respectively; t represents time t. Indicates the number of drones involved in the suspended transport;

[0030] Reference position of the drone The first, second, and third derivatives with respect to time yield the UAV's reference velocity, reference acceleration, and higher-order control feedforwards.

[0031] Furthermore, the specific expression for the system-wide security constraint in step three is as follows:

[0032]

[0033]

[0034]

[0035]

[0036]

[0037]

[0038]

[0039] in, Represents the obstacle distance field function; Indicates the load location; Indicates the safe envelope radius of the load; and They represent the first frame and the first Location of the drone; Indicates the safe radius of the drone; Indicates the first The first cable One sampling point; Indicates the safe expansion radius of the cable; Indicates the first frame and the first Minimum safe distance between drones; Represents the Euclidean norm; , , and These represent the upper and lower bounds of the pitch and azimuth angles, respectively, indicating the direction of the cable. and These represent the lower and upper limits of tension to prevent cable slack and to avoid exceeding the structure's load-bearing capacity, respectively. This represents the tension of the i-th cable; , and They represent the first The speed, acceleration, and total thrust of the drone; , and These represent velocity, acceleration, and upper limit of thrust, respectively.

[0040] Furthermore, in step four, the flat output is parameterized using piecewise polynomials, specifically as follows:

[0041] The flat output of the first The segment trajectory is represented using a piecewise polynomial:

[0042]

[0043] Furthermore, adjacent trajectory segments satisfy the continuity constraint:

[0044]

[0045] in, Indicates the first Segment flat output trajectory; Indicates the first Duan Di Coefficients of a polynomial of order 1; Indicates the order of a polynomial; Indicates the first Duration of segment; Indicates the order of the derivative; This indicates the highest order that needs to be maintained in sequence.

[0046] Furthermore, in step four, the bounded constraint variables are transformed into unbounded optimization variables through a differential homeomorphism mapping, and the mapping expression is as follows:

[0047]

[0048]

[0049] in, Indicates cable direction angle, tension, or segmented time variables; and They represent The lower and upper bounds of a variable; Represents an unbounded optimization variable; Represents an exponential function; This represents the mapped derivative required for chain rule differentiation.

[0050] Furthermore, the objective function of the trajectory optimization problem in step four is:

[0051]

[0052] The expression for solving the trajectory optimization problem is:

[0053]

[0054] in, Represent the overall objective function; Indicates the cost of trajectory smoothness; Indicates a safety penalty for obstruction; Indicates a penalty for dynamic feasibility; Indicates the cost of controlling input; Indicates the cost of time; This indicates the cost of replanning continuity; , , , and Indicates the weights of each item; Represents the set of polynomial coefficients; Represents the set of unbounded optimization variables; and These represent flat outputs at the start and end points, respectively.

[0055] Furthermore, in step five, the first The on-board state of the UAV includes position tracking error, velocity tracking error, and desired acceleration, expressed as:

[0056]

[0057]

[0058] in, Indicates the first Position tracking error of the drone; Indicates speed tracking error; and Indicates the actual or estimated position and speed of the drone; , and These represent the reference position, reference velocity, and reference acceleration, respectively. and Represents the feedback gain matrix; This indicates an estimate of cable tension; Indicates the estimated direction of the cable;

[0059] No. The expected total thrust of the drone is:

[0060]

[0061] in, Indicates the first Total thrust command for the drone; Indicates the quality of the drone; Indicates the desired acceleration; Represents gravitational acceleration; Represents a unit vector in the vertical direction; Represents the Euclidean norm;

[0062] The cable force is estimated based on the UAV dynamic residuals, and the expression is:

[0063]

[0064]

[0065] in, This represents the estimated force vector of the cable; This represents the estimated acceleration value of the drone; This represents the thrust estimate; This represents the attitude rotation matrix of the UAV; It represents a small positive number that is prevented from being divided by zero.

[0066] Furthermore, in step six, the estimated cable force is used to further estimate the load mass, expressed as:

[0067]

[0068] in, This represents the estimated load mass; g represents the gravitational acceleration. and Let represent the estimated tension and direction of the i-th cable, respectively;

[0069] When the difference between the estimated load mass and the planned load mass exceeds a threshold, the safety margin is increased, the maximum acceleration is reduced, or online replanning is triggered.

[0070] Furthermore, in step six, the online replanning triggered by the safety margin and the target point change is expressed as follows:

[0071]

[0072]

[0073] in, This indicates the minimum safety margin of the current system; and These represent the new target point and the old target point, respectively. Indicates the threshold for triggering changes in the target; Indicates the safe radius of the drone; Indicates the safe envelope radius of the load; Indicates the safe radius of the cable; Represents the obstacle distance field function; Indicates the position of the i-th drone; Indicates the load location; This indicates the position of the k-th sampling point on the i-th cable;

[0074] Meanwhile, the following constraints ensure that no sudden state changes occur when switching between old and new trajectories:

[0075]

[0076] in, This represents the new flat output trajectory obtained through replanning; This indicates the trajectory currently being executed; Indicates the replanning trigger time; This indicates the highest derivative order that requires the inheritance of continuity.

[0077] Compared with existing technologies, the beneficial effects of this invention include: First, it can simultaneously constrain the payload, the UAV, and the cable sweep body, improving system-level safety in complex obstacle environments; Second, it reduces the planning dimension with flat output, making real-time planning and online replanning easier to achieve; Third, it reduces the risk of optimization infeasibility by handling bounded quantities such as cable azimuth and time allocation through differential homeomorphism mapping; Fourth, it reduces the dependence on high-frequency communication between payload sensors and the UAV through distributed robust tracking control; Fifth, it can still maintain good transportation stability when the payload mass is inaccurate, the payload is non-point mass, the thrust model has errors, and there are sudden changes in the target. Attached Figure Description

[0078] Figure 1 This is a schematic diagram of the overall process of a method for planning and controlling safe and agile transportation of multiple UAVs with suspended loads, as described in this embodiment.

[0079] Figure 2 This is a schematic diagram of the structure and coordinate system of the multi-UAV, cable, and payload system in this embodiment;

[0080] Figure 3 This is a schematic diagram of the overall architecture for safe and agile trajectory planning and distributed control in this embodiment;

[0081] Figure 4 This is a schematic diagram of the combined safety constraints of the load path, UAV path, and cable sweep body in this embodiment;

[0082] Figure 5This is a schematic diagram illustrating the relationship between the cable azimuth constraint and the differential homeomorphism in this embodiment.

[0083] Figure 6 This is a schematic diagram of the piecewise polynomial trajectory parameterization and time allocation in this embodiment;

[0084] Figure 7 This is a schematic diagram of the distributed robust tracking controller structure in this embodiment;

[0085] Figure 8 This is a schematic diagram of the load mass estimation and cable force compensation process in this embodiment;

[0086] Figure 9 This is a schematic diagram illustrating safe passage through narrow passages and drone formation adjustment in this embodiment;

[0087] Figure 10 This is a schematic diagram of the online replanning process for sudden target changes in this embodiment;

[0088] Figure 11 This is a schematic diagram illustrating the evaluation of trajectory agility, tracking error, and computation time in this embodiment. Detailed Implementation

[0089] To make the objectives, technical solutions, and technical effects of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention. Unless otherwise specified, all positions, velocities, and accelerations are represented in the world coordinate system, all angles are in radians (rad), all distances are in meters (m), and all forces are in newtons (N).

[0090] like Figure 1 As shown, this embodiment provides a method for safe and agile transportation planning and control of multiple unmanned aerial vehicles (UAVs) carrying suspended loads, including the following steps:

[0091] Step 1: Establish a coupled dynamics model of the transportation system consisting of multiple drones, cables, and loads.

[0092] Specifically, a multi-drone transport system consists of N drones, N cables, and a payload to be transported, where N is an integer greater than or equal to 2. Each drone is connected to the payload via a cable, which can be connected to the same equivalent reference point of the payload or to different connection points of the payload.

[0093] set up , where X represents the set of system states; This represents the position vector of the load reference point in the world coordinate system. Represents the load velocity vector; Represents the position vector of the i-th UAV; Represents the velocity vector of the i-th UAV; Let represent the rotation matrix of the i-th UAV from the machine system to the world system; This represents the angular velocity vector of the i-th UAV. This represents the unit direction vector from the load to the UAV of the i-th cable; This represents the tension of the i-th cable; the value of i ranges from 1 to N.

[0094] The inputs to the transportation system include the current system state set X and the obstacle distance field. Load radius or envelope size Drone safety radius Cable safety radius Minimum drone spacing Load start point Load target point and the maximum allowable planning time for transportation tasks .

[0095] The output of the transportation system includes the load reference trajectory, cable direction reference trajectory, reference trajectories of each UAV, and control execution status flags.

[0096]

[0097] Where Y represents the set of planning and control outputs; the superscript ref indicates a reference value; and t represents the task time variable. Indicates the reference acceleration of the i-th UAV; This represents the reference total thrust or equivalent control input for the i-th UAV.

[0098] To facilitate real-time planning, this invention treats the cable as a lightweight link with a known length and no extensibility, and absorbs errors caused by flexible vibration, connection bias, and non-point mass of load through force estimation at the control layer.

[0099] like Figure 2 As shown, the translational dynamics model of the common load of the multi-UAV suspended load transportation system in this embodiment is expressed as follows:

[0100]

[0101] in, Indicates the load mass; g represents the acceleration due to the load; g represents the acceleration due to gravity. Alternatively, it can be defined as a vertical unit vector according to the selected world coordinate system; This represents the tension of the i-th cable; This represents the unit direction vector of the i-th cable. This represents the combined term of external disturbances, air resistance, and model errors experienced by the load.

[0102] The position of the UAV is determined by both the load position and the cable direction. The geometric constraint expression for the UAV position is:

[0103]

[0104] in, Indicates the position of the i-th drone; Indicates the load location; This represents the length of the i-th cable; Let represent the unit vector of the direction of the i-th cable, and let be the cable direction angle generated by the cable pitch angle and azimuth angle, satisfying that its L2 norm is equal to 1.

[0105] The translational dynamics model of the UAV can be expressed as:

[0106]

[0107] in, Indicate the mass of the i-th drone; Indicates the acceleration of the i-th drone; This indicates the total thrust generated by the drone; This represents the attitude rotation matrix of the UAV; This represents the vector of tension exerted by the cable on the drone. This represents wind disturbance, thrust model error, and unmodeled dynamic disturbance.

[0108] In practical engineering implementation, cable length quality of drones Load envelope radius and drone safety radius This can be provided by calibration or mission configuration. Payload mass Estimation errors are permissible and can be corrected at the distributed control layer by estimating cable forces.

[0109] Step 2: Using the load position and the direction angle of each cable as the flat output, analyze and obtain the position and status of each UAV.

[0110] Specifically, to reduce the dimensionality of trajectory planning, this embodiment does not directly use the attitude and input of all UAVs as optimization variables. Instead, it selects the payload position and cable direction as flat outputs. With this design, once the payload trajectory and cable direction trajectory are determined, the position and higher-order motion state of each UAV can be obtained analytically or recursively.

[0111] The specific expression for the flat output is defined as follows:

[0112]

[0113] Where t represents time t, and Z(t) represents the flat output vector of the system; Indicates the load location; The pitch angle represents the direction of the i-th cable; The azimuth angle represents the direction of the i-th cable; T represents the transpose symbol; the dimension of this output vector is 3+2N.

[0114] Generate the cable direction vector based on the direction angle:

[0115]

[0116] in, This represents the unit direction vector of the i-th cable generated by the direction angle; The range of values ​​for is determined by the maximum swing angle of the cable relative to the vertical direction; cos and sin are the cosine and sine functions, respectively.

[0117] By using the first, second, and third derivatives of the UAV's position with respect to time, the UAV's reference velocity, reference acceleration, and higher-order control feedforwards can be obtained. This mapping allows the planning layer to directly constrain the cable direction and load position, while avoiding explicitly incorporating the full attitude dynamics of each UAV into the optimization variables.

[0118] The position expression for the UAV is:

[0119]

[0120] in, Indicates the position of the i-th drone at time t; This represents the length of the i-th cable; Indicates by and The direction of the generated cable.

[0121] Step 3: In an obstacle-prone environment, considering the payload shape, UAV shape, UAV spacing, tether sweep body, tether tension, tether direction, and UAV dynamics feasibility, construct comprehensive system safety constraints, referring to... Figure 4 and Figure 5 As shown.

[0122] In complex environments, simply constraining the load center point to be far from obstacles cannot guarantee the safety of the transportation system. Therefore, this invention simultaneously constrains the load, the drone, the cable sampling points, and the drone spacing, and embeds these constraints uniformly into the trajectory optimization problem, as follows:

[0123] The constraints on the load are:

[0124]

[0125] in, This represents the obstacle distance field function generated from the environment map; Indicates the load location; This indicates the radius of expansion of the load's shape or the radius of its safety envelope. This constraint means that the load's safety envelope must not intersect with any obstacle.

[0126] The constraints on drones are:

[0127]

[0128] in, Indicates the position of the i-th drone; This represents the safe radius formed by the combined external radius of the UAV, positioning error, and control error. This constraint is used to ensure obstacle avoidance by the UAV itself.

[0129] The expression for the cable sampling points is:

[0130]

[0131] in, This indicates the position of the k-th sampling point on the i-th cable; This represents the discrete sampling ratio of the cable, satisfying 0 less than or equal to Less than or equal to 1; Indicates the length of the cable; This represents the direction vector of the cable.

[0132] The constraints on the cable sampling points are as follows:

[0133]

[0134] in, Indicates the cable sampling point; This represents the cable's safe expansion radius, which includes the cable's actual radius, sway margin, and map error margin. This constraint is used to prevent the cable sweep body from colliding with obstacles.

[0135] The constraint on the spacing between drones is:

[0136]

[0137] in, and Let i and j represent the positions of the i-th and j-th UAVs, respectively. Indicates Euclidean distance; This represents the minimum safe distance between two drones; i not equal to j indicates that the constraint applies to different drone pairs.

[0138] With the above constraints, the present invention can address the risk of collision caused by the UAV deploying laterally when the load passes through the gaps in the obstacle, and can also address the risk of the cable sweeping obliquely along the edge of the obstacle.

[0139] To further illustrate the feasible range of pitch and azimuth angles and tension in the cable direction, it is important to understand that insufficient cable tension may lead to slack, while excessive tension may exceed the thrust capacity of the connectors or the UAV; excessively large pitch and azimuth angles in the cable direction may result in overly wide UAV formation deployment or excessive control input. Therefore, this invention explicitly considers the constraints of pitch and azimuth angles in the cable direction during the planning phase, as follows:

[0140]

[0141] in, and Let represent the pitch angle and azimuth angle of the i-th cable, respectively; , , and This indicates the upper and lower limits of the azimuth angle set according to the load structure, the allowable range of the formation, and the flight space of the UAV.

[0142] The constraint on cable tension is as follows:

[0143]

[0144] in, This represents the tension of the i-th cable; This indicates the minimum tension threshold to prevent cable slack. This represents the maximum tension threshold determined by the combined thrust of the cable, connectors, and drone.

[0145] The constraints on the dynamic feasibility of the UAV are:

[0146]

[0147] in, Indicates the speed of the i-th drone; Indicates the acceleration of the i-th drone; This represents the total thrust of the i-th drone; , and These represent velocity, acceleration, and upper limit of thrust, respectively.

[0148] The aforementioned constraints ensure that the generated trajectory is not only geometrically safe but also practically trackable by the UAV's closed-loop controller. For highly agile transport missions, different safety levels can be set. and This allows the system to strike a balance between speed and safety margin.

[0149] Step 4: Perform piecewise polynomial parameterization on the flat output, transform bounded constraint variables into unbounded optimization variables through differential homeomorphism mapping, and solve the trajectory optimization problem in real time to generate a safe reference trajectory. Figure 3 As shown.

[0150] To achieve real-time trajectory planning, this invention represents both the load position and cable direction angle as piecewise polynomials, and controls the total trajectory duration and local velocity distribution through piecewise time variables, such as... Figure 6 As shown.

[0151] Specifically, the flat output of the first The segment trajectory is represented using a piecewise polynomial:

[0152]

[0153] in, This represents the m-th flat output trajectory; s represents the coefficients of the m-th segment of the r-th polynomial; s represents the order of the polynomial. The m-th segment represents the duration of the trajectory; t represents the time within that segment.

[0154]

[0155] Where T represents the total duration of the entire trajectory; M represents the number of segments; This represents the time of the m-th trajectory segment.

[0156] By adjusting M and It can strike a balance between planning accuracy and calculation speed.

[0157] Furthermore, adjacent trajectory segments satisfy the continuity constraint:

[0158]

[0159] Where q represents the order of the derivative; This indicates the highest order that needs to be maintained in sequence; and These represent two adjacent trajectory segments. This formula ensures that adjacent trajectory segments are continuous in position, velocity, acceleration, or higher-order derivatives, avoiding abrupt changes during UAV execution.

[0160] In one alternative implementation, the load position is represented by a fifth-order or seventh-order polynomial, and the cable orientation angle is represented by a fifth-order polynomial with consecutive orders. Choose 2 or 3 to ensure smooth acceleration and control input.

[0161] Because planning problems involve numerous bounded variables, such as cable direction angle, piecewise time, and tension range, direct optimization within the bounded space can easily lead to instability due to boundary violations during iteration. This invention employs a differential homeomorphism mapping to transform bounded constraint variables into unbounded optimization variables. The mapping expression is as follows:

[0162]

[0163] in, It can represent any bounded physical variable, such as cable direction angle, tension, or time-related variables. and These represent the lower and upper bounds of a bounded physical variable, respectively. represents an unbounded optimization variable whose range is all real numbers; exp represents an exponential function.

[0164]

[0165] in, This represents the derivative of a bounded variable with respect to an unbounded variable. This derivative is used in chain rule differentiation in gradient-based optimization, allowing the optimizer to... Iterate in space and maintain automatically It lies between the upper and lower boundaries.

[0166] when When representing time segments, you can... Set to minimum control cycle or minimum executable time. Set to the maximum local time allowed by the task. To avoid value oversaturation, you can... A scaling factor is added to the initial value settings and gradient updates.

[0167] The objective function of the trajectory optimization problem not only requires a smooth trajectory but also demands that penalties be added near obstacles, agility be reduced when dynamics approach their limits, and continuity with the current trajectory be maintained during replanning. The objective function is:

[0168]

[0169] Where J represents the overall objective function; Indicates the cost of smoothness; Indicates a safety penalty for obstruction; Indicates a penalty for dynamic feasibility; Indicates the cost of controlling input; Indicates the cost of time; This indicates the cost of replanning continuity; , , , and These represent the weight coefficients for each item.

[0170]

[0171] in, Indicates the trajectory smoothing term; The third derivative represents the load position; This term represents the third derivative of the direction vector of the i-th cable; T represents the total trajectory time. This term is used to suppress violent swings and control input abrupt changes.

[0172]

[0173] in, Indicates the penalty for obstacles; This represents the safety distance penalty function, which takes the value 0 when the distance margin is greater than the set threshold, and increases with the penetration amount or degree of danger when the distance is insufficient; the meanings of the other variables are the same as those described above.

[0174] Solve the following trajectory optimization problem:

[0175]

[0176] Where c represents the set of all polynomial coefficients; Represents the set of all unbounded optimization variables; Indicates a flat initial output; This indicates that the target output is flat. This optimization problem is used to find a safe and agile trajectory that satisfies the start and end boundary conditions.

[0177] In practical solutions, a coarse path can be generated first through system-level path search, and then continuous trajectory optimization can be used to improve smoothness and dynamic feasibility. If the planning time exceeds a threshold, the previous safe trajectory can be retained and the desired speed reduced.

[0178] Step 5: Each UAV performs distributed robust tracking control on the generated trajectory based on its own status, propeller speed (i.e., the desired total thrust), and estimated cable force.

[0179] After trajectory planning is completed, each UAV independently receives its own reference state and executes distributed tracking control with the same structure, such as... Figure 7 As shown. The controller used in the system does not require additional sensors to be installed on the payload, nor does it require high-frequency switching of all UAV statuses.

[0180] Specifically, the first The position tracking error and velocity tracking error of the drone are:

[0181]

[0182] in, This represents the position tracking error of the i-th UAV; Indicates speed tracking error; and Indicates the measured or estimated position and velocity of the drone; and Indicates the reference position and reference velocity.

[0183] No. The expected acceleration of the drone is:

[0184]

[0185] in, Indicates the expected acceleration of the i-th drone; This represents the reference acceleration given in the planning; and These represent the position error and velocity error feedback gain matrices, respectively. Indicates the quality of the drone; This indicates an estimate of cable tension; This indicates an estimated cable direction.

[0186] No. The total thrust command for the drone is:

[0187]

[0188] in, This represents the expected total thrust of the i-th drone; Indicates the desired acceleration; Represents gravitational acceleration; Represents a unit vector in the vertical direction; This represents the Euclidean norm.

[0189] The controller can be used in conjunction with a conventional attitude control loop. The outer loop outputs the desired thrust and desired attitude, while the inner loop generates motor speed commands based on the aircraft's attitude, angular velocity, and motor model. Because each UAV explicitly compensates for its own cable forces, the impact of load coupling on the single-unit position loop is weakened.

[0190] like Figure 8 As shown, in order to reduce the dependence on the sensors at the payload end, this invention uses the dynamic residuals of the UAV itself to estimate the cable force and further estimate the payload mass. The estimation of the payload mass does not require strict identification of all dynamic parameters, but rather provides usable online information for control compensation, anomaly detection and replanning.

[0191] Based on the UAV dynamic residuals, the cable force and load mass are estimated, where the cable force estimation expression is:

[0192]

[0193] in, Let represent the estimated cable force vector for the i-th UAV; This represents the UAV acceleration estimated by velocity difference or filter. This represents the total thrust estimated by the motor speed and thrust model; This represents the attitude rotation matrix of the UAV; the meanings of the other variables are the same as described above.

[0194]

[0195] in, This indicates an estimate of the cable tension. Indicates the estimated direction of the cable; It represents a small positive number that is prevented from being divided by zero.

[0196] The expression for load mass estimation is:

[0197]

[0198] in, This represents the estimated load mass. Represents gravitational acceleration; and Let represent the estimated tension and direction of the i-th cable, respectively. This formula has good estimation stability in low acceleration or quasi-static phases, and can be used in conjunction with low-pass filtering and confidence level determination in high-agility motion phases.

[0199] When the difference between the estimated load mass and the planned load mass exceeds a threshold, the safety margin is increased, the maximum acceleration is reduced, or a replanning is triggered to avoid the tension constraint being violated.

[0200] like Figure 9 As shown, in an embodiment with a narrow passage, the passage conditions for the payload, drones, and cables differ. The payload may be able to pass through the gap, but if the drone formation remains in its naturally extended state, it will collide with obstacles on either side of the passage. Therefore, this invention optimizes the automatic retraction or rotation of the formation by adjusting the cable's directional angle.

[0201] First, determine whether the load body has the conditions for passage, as shown in the following expression:

[0202]

[0203] in, This represents the passable width at position s along the arc length of the candidate path; Indicates the safe envelope radius of the load; This indicates the safety margin for load passage.

[0204] At the same time, the lateral width occupied by drones in the passageway is restricted:

[0205]

[0206] in, Indicates the safe radius of the drone; Indicates the safety margin for drone traffic; This represents the lateral deployment distance of the UAV relative to the payload, determined by the direction of the i-th cable.

[0207] When insufficient channel width is detected, the system's planner adjusts... This reduces the lateral spread of the drone's relative load, while adding a smoothing term and tension penalty to the objective function to avoid excessive load swing caused by sudden formation contraction.

[0208] Step 6: Based on the safety margin, cable tension margin, tracking error, target point change, and load mass estimation error, trigger online replanning, deceleration transport, hovering and waiting, or mission abort.

[0209] like Figure 10 As shown, for rescue and logistics missions, the target point may change during transportation, and obstacles may appear temporarily. A replanning trigger and trajectory inheritance mechanism are set up to enable the system to generate a new trajectory while continuing to execute the current safe trajectory.

[0210] The replanning trigger initiates the system to enter replanning or degradation mode based on the safety margin and the change in the objective, as detailed below:

[0211]

[0212] in, This represents the minimum safety margin of the current system; min represents the minimum safety distance margin for the payload, all UAVs, and all cable sampling points.

[0213] when When the value is less than the preset threshold, the system enters replanning or degradation mode.

[0214]

[0215] in, Indicates the new target point location; Indicates the location of the old target point; This represents the threshold for triggering a change in the objective. This formula is used to determine whether a change in the objective is sufficient to trigger a replanning process.

[0216] Meanwhile, the following constraints ensure that no sudden state changes occur when switching between old and new trajectories:

[0217]

[0218] in, This represents the new flat output trajectory obtained through replanning; This indicates the trajectory currently being executed; Indicates the replanning trigger time; This indicates the highest order of inheritance that needs to be inherited.

[0219] If replanning fails or the planning time exceeds the allowable limit, the system continues to execute the remaining part of the previous safe trajectory and gradually reduces speed, increases the safe distance from obstacles, or enters a hovering waiting state.

[0220] In a specific engineering parameter deployment, the system planner performs local replanning at a frequency of 5Hz to 20Hz, while the controller executes outer and inner loop control at a frequency of 100Hz to 500Hz. When the map update frequency is lower than the control frequency, the planner uses the most recent valid range field and compensates for map latency by increasing the safety margin. This is based on the UAV size, payload size, tether length, and positioning accuracy. It can be set as the sum of the UAV rotor radius, the upper limit of the positioning error, and the upper limit of the control error; It can be set to the sum of the maximum circumscribed ball radius and the oscillation margin; It can be determined based on the actual radius of the cable, map resolution, and estimation error.

[0221]

[0222] in, Indicates the safe radius of the drone; Indicates the radius of the machine body structure; Indicates the radius of the rotor or protective shield; Indicates the positioning error margin; This indicates the control tracking error margin.

[0223]

[0224] in, Indicates the safe envelope radius of the load; Indicates the radius of the load geometry; Indicates the load positioning error margin; This indicates the load swing and the model uncertainty margin.

[0225] like Figure 11 As shown, the method of the present invention was verified and evaluated through simulation scenarios and real multi-UAV platforms. The simulation scenarios included free space agile tracking, random obstacle avoidance, narrow passage crossing, and continuous target change; the real-world scenarios included three UAVs coordinating to suspend loads, indoor obstacle avoidance, narrow doorway passage, and sudden replanning.

[0226]

[0227] in, This represents the root mean square error of the load trajectory; Indicates the number of sampling points for evaluation; This indicates the actual position of the load at the k-th sampling time. This indicates the load reference position at the corresponding time.

[0228]

[0229] in, This represents the maximum trajectory error of the load; max represents the maximum value at all sampling times; the meanings of the other variables are the same as described above.

[0230]

[0231] in, Indicates the time taken for a single trajectory planning or replanning operation; This indicates the maximum allowable planning time for the task. This metric is used to evaluate whether the planner meets real-time requirements.

[0232] In addition to the above indicators, the following can also be measured: minimum obstacle distance, minimum drone spacing, maximum tension, minimum tension, mission completion time, replanning success rate, and narrow passage passability. If any safety indicator falls below the threshold, the corresponding penalty weight or safety radius should be increased in the next planning round.

[0233] It should be noted that, without departing from the core idea of ​​this invention, it can be extended to heterogeneous drone swarms, variable-length cables, multiple payloads, moving obstacles, and outdoor wind-prone environments. For heterogeneous drones, the mass of each drone... Upper thrust limit Speed ​​limit and safety radius These can be set separately; for variable length cables, they can be... Include flat output or constraint variables.

[0234]

[0235] in, This represents the length of the i-th cable at time t; and These represent the minimum and maximum allowable lengths for the cable winding and unwinding mechanism, respectively.

[0236]

[0237] in, This represents the dynamic obstacle distance field that changes over time; p(t) represents the sampling point location of the load, drone, or cable to be inspected; and r represents the safety radius of the corresponding object. This formula is used for moving obstacle scenarios.

[0238] The parameters, thresholds, optimizer types, and controller gains in the above embodiments can all be adjusted according to the actual platform. As long as the multi-UAV suspended transport system is parameterized by both load position and cable direction, and system-level safety constraints, cable feasibility, and distributed robust execution are considered simultaneously in planning and control, it falls within the scope of the technical concept of this invention.

[0239] In practical applications, a mission initiation, cable pretensioning, and safety check process is also designed. Specifically, before the transportation mission begins, the ground station or airborne computing unit reads the initial estimated payload mass, payload dimensions, cable length, UAV battery level, positioning quality, communication link quality, and environmental map version number. If any parameter is missing or exceeds a safety threshold, the system will not enter automatic transportation mode, but will prompt for recalibration or switch to manual takeover mode.

[0240]

[0241] in, Indicates the task start permission flag; Indicates whether the battery and power system checks have passed; Indicates whether the positioning quality inspection has passed; Indicates whether the map and obstacle distance field is valid; This indicates whether the cable length and connection status have passed the inspection; It indicates whether the communication link meets the task requirements; the symbol "land" represents a logical AND relationship.

[0242] During the cable pre-tensioning phase, each UAV first ascends to an initial formation above the load and increases the distance between itself and the load at a low speed along the cable direction, gradually bringing each cable from a slack state to a stressed state. This method uses cable tension estimated by thrust residual or motor speed to determine whether pre-tensioning is complete, preventing any UAV from prematurely entering the transport trajectory and causing load tilting.

[0243]

[0244] in, This represents the estimated tension of the i-th cable during the pre-tensioning stage; This indicates the minimum preload required for the cable to be reliably taut; This indicates the maximum allowable tension during the pre-tightening stage.

[0245] When all cable tensions are within the pretension range, and the UAV attitude angle, position error, and initial load oscillation speed are below the threshold, the system uses the current state as the starting point for trajectory planning. If pretensioning of a cable fails, the planner does not generate an agile trajectory, but instead outputs hover hold, slow descent, or re-pretensioning commands.

[0246] In practical applications, multi-UAV load transport is a highly coupled task, and insufficient thrust, positioning abrupt changes, or communication delays of a single UAV can all affect load safety. This invention also incorporates anomaly detection, safety degradation, and mission abort strategies. Specifically, an anomaly monitoring module is set up between the planning and control layers to synchronously monitor tracking errors, tension estimation, thrust margin, obstacle distance, and communication delays.

[0247]

[0248] in, This represents the thrust margin of the i-th drone; This indicates the maximum available thrust of the drone; This indicates the thrust currently requested by the controller. The smaller the thrust margin, the closer the drone is to its capability boundary.

[0249]

[0250] in, This represents a comprehensive risk indicator; to Indicates risk weight; This represents a non-negative risk mapping function; Indicates the minimum safety margin of the system; Indicates thrust margin; Indicates cable tension; Indicates the upper limit of tension; Let represent the position error of the i-th UAV.

[0251] When the comprehensive risk index is in the low-risk range, the system continues to execute the current trajectory; when the comprehensive risk index enters the medium-risk range, the system reduces the reference speed and acceleration and increases the obstacle safety radius; when the comprehensive risk index enters the high-risk range, the system triggers local replanning; when replanning fails or the risk continues to rise, the system enters the safety abort procedure.

[0252]

[0253] Where mode represents the system execution mode; normal represents the normal transportation mode; slow represents the decelerated transportation mode; replan represents the replanning mode; and abort represents the task abort mode. , and These represent risk thresholds from smallest to largest.

[0254] Mission abort does not mean immediately shutting down the motors. Instead, the system will choose to hover safely, descend slowly, return to the starting point, or land nearby, depending on the payload altitude, obstacle distribution, and available thrust. For relief delivery missions, if the area below the payload is safe and the mission allows, the payload can be released. For non-releaseable payloads, the system will maintain a multi-UAV formation and move at low speed to a pre-set safe area.

[0255] To ensure the reproducibility of experimental results, the system records the planning input, optimization variables, trajectory output, control commands, state estimation, cable tension estimation, replanning trigger reasons, and abnormal mode switching times for each transportation task. These records can be used for both offline parameter tuning and effect verification in the embodiments.

[0256]

[0257] Where H represents the set of data records for a single task; This represents the system state at the k-th sampling time. Indicates the reference trajectory output; Indicates control input; Represents the set of cable tension estimates; Represents the set of safety margins; Indicates the mode flag; This indicates the number of sampling points recorded in the task.

[0258] During the offline analysis phase, the system recalculates trajectory tracking error, minimum obstacle distance, number of tension constraint violations, replanning time, and control input saturation counts based on recorded data. If a certain indicator is found to be close to the safety boundary, the next task can automatically increase the corresponding safety margin or decrease the agility weight.

[0259]

[0260] in, Indicates the number of constraint violations; I represents an indicator function, which takes the value 1 when the condition inside the parentheses is true, and 0 otherwise. This represents the minimum safety margin at the k-th sampling time. This represents the tension estimate of the i-th cable at the k-th sampling time; This indicates the upper limit of tension.

[0261] In a set of optional verifications, five types of tasks can be set up: agile flight in open areas, obstacle avoidance, narrow passages, sudden changes in target points, and payload mass deviations. Each type of task is repeated at least a certain number of times, and the task success rate, average planning time, and maximum trajectory error are recorded. If the present invention has a lower collision risk, shorter replanning time, or higher narrow passage throughput compared to fixed formation transportation or payload-only obstacle avoidance methods, then the system-level safety planning and distributed robust control combination of the present invention has practical effects.

[0262] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any way. Although the implementation process of the present invention has been described in detail above, those skilled in the art can still modify the technical solutions described in the foregoing examples or make equivalent substitutions for some of the technical features. All modifications and equivalent substitutions made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for safe and agile transportation planning and control of multiple unmanned aerial vehicles (UAVs) carrying suspended loads, characterized in that, include: Step 1: Establish a coupled dynamics model of the transportation system consisting of multiple drones, cables, and loads; Step 2: Using the load position and the direction angle of each cable as the flat output of the system, the position and status of each UAV are mapped through the load trajectory and the direction trajectory of the cable. Step 3: Considering the payload shape, UAV shape, UAV spacing, cable sweep body, cable tension, cable direction, and UAV dynamics feasibility, construct the full system safety constraints in the obstacle environment; Step 4: Perform piecewise polynomial parameterization on the flat output, convert bounded constraint variables into unbounded optimization variables through differential homeomorphism mapping, and solve the trajectory optimization problem in real time to generate a safe reference trajectory; Step 5: Each UAV performs distributed tracking control based on its own status, expected total thrust, and estimated cable force, according to the generated trajectory. Step 6: Based on the safety margin, cable tension margin, tracking error, target point change, and load mass estimation error, trigger the online replanning, safety degradation, or mission termination strategy of the transportation system.

2. The method for safe and agile transportation planning and control of multiple UAVs carrying suspended loads as described in claim 1, characterized in that, In step one, the coupled dynamics model includes: Translational dynamics model of the load: in, Indicates the load mass; Indicates the load acceleration; Represents gravitational acceleration; Represents the vertical unit vector in the world coordinate system; Indicates the number of drones involved in the suspended transport; Indicates the first Cable tension; Indicates the first The cable points from the load to the drone in a unit direction vector; This represents the combined term of external load disturbances and unmodeled errors; No. Geometric constraints of the unmanned aerial vehicle (UAV) and its payload without elongation cable: in, Indicates the first Location of the drone; Indicates the load location; Indicates the first Length of the cable; Indicates the first Unit vector in the direction of the root cable; And, the translational dynamics model of the UAV: in, Indicates the first The quality of the drone; Indicates the acceleration of the drone; Indicates the total thrust of the drone; This represents the attitude rotation matrix of the UAV; This represents wind disturbance, thrust error, and unmodeled dynamic disturbance.

3. The method for safe and agile transportation planning and control of multiple UAVs carrying suspended loads as described in claim 2, characterized in that, Step two specifically involves: Define flat output as: Generate the first [number] based on the direction angle. Root cable direction vector: in, This represents the flat output vector of the system; Indicates the load location; Indicates the first The pitch angle along the direction of the cable; Indicates the first The azimuth angle of the cable direction; Indicates transpose; and Let cosine and sine functions represent the functions respectively; t represents time t. Indicates the number of drones involved in the suspended transport; Reference position of the drone The first, second, and third derivatives with respect to time yield the UAV's reference velocity, reference acceleration, and higher-order control feedforwards.

4. The method for safe and agile transportation planning and control of multiple UAVs carrying suspended loads as described in claim 1, characterized in that, The specific expression for the system-wide security constraint in step three is as follows: in, Represents the obstacle distance field function; Indicates the load location; Indicates the safe envelope radius of the load; and They represent the first frame and the first Location of the drone; Indicates the safe radius of the drone; Indicates the first The first cable One sampling point; Indicates the safe expansion radius of the cable; Indicates the first frame and the first Minimum safe distance between drones; Represents the Euclidean norm; , , and These represent the upper and lower bounds of the pitch and azimuth angles, respectively, indicating the direction of the cable. and These represent the lower and upper limits of tension to prevent cable slack and to avoid exceeding the structure's load-bearing capacity, respectively. This represents the tension of the i-th cable; , and They represent the first The speed, acceleration, and total thrust of the drone; , and These represent velocity, acceleration, and upper limit of thrust, respectively.

5. The method for safe and agile transportation planning and control of multiple UAVs with suspended loads as described in claim 1, characterized in that, In step four, the flat output is parameterized using a piecewise polynomial, specifically as follows: The flat output of the first The segment trajectory is represented using a piecewise polynomial: Furthermore, adjacent trajectory segments satisfy the continuity constraint: in, Indicates the first Segment flat output trajectory; Indicates the first Duan Di Coefficients of a polynomial of order 1; Indicates the order of a polynomial; Indicates the first Duration of segment; Indicates the order of the derivative; This indicates the highest order that needs to be maintained in sequence.

6. The method for safe and agile transportation planning and control of multiple UAVs with suspended loads as described in claim 1, characterized in that, In step four, the bounded constraint variables are transformed into unbounded optimization variables through differential homeomorphism mapping, and the mapping expression is as follows: in, Indicates cable direction angle, tension, or segmented time variables; and They represent The lower and upper bounds of a variable; Represents an unbounded optimization variable; Represents an exponential function; This represents the mapped derivative required for chain rule differentiation.

7. The method for safe and agile transportation planning and control of multiple UAVs with suspended loads as described in claim 1, characterized in that, The objective function for the trajectory optimization problem in step four is: The expression for solving the trajectory optimization problem is: in, Represent the overall objective function; Indicates the cost of trajectory smoothness; Indicates a safety penalty for obstruction; Indicates a penalty for dynamic feasibility; Indicates the cost of controlling input; Indicates the cost of time; This indicates the cost of replanning continuity; , , , and Indicates the weights of each item; Represents the set of polynomial coefficients; Represents the set of unbounded optimization variables; and These represent flat outputs at the start and end points, respectively.

8. The method for safe and agile transportation planning and control of multiple UAVs carrying suspended loads as described in claim 1, characterized in that, In step five, the first The on-board state of the UAV includes position tracking error, velocity tracking error, and desired acceleration, expressed as: in, Indicates the first Position tracking error of the drone; Indicates speed tracking error; and Indicates the actual or estimated position and speed of the drone; , and These represent the reference position, reference velocity, and reference acceleration, respectively. and Represents the feedback gain matrix; This indicates an estimate of cable tension; Indicates the estimated direction of the cable; No. The expected total thrust of the drone is: in, Indicates the first Total thrust command for the drone; Indicates the quality of the drone; Indicates the desired acceleration; Represents gravitational acceleration; Represents a unit vector in the vertical direction; Represents the Euclidean norm; The cable force is estimated based on the UAV dynamic residuals, and the expression is: in, This represents the estimated force vector of the cable; This represents the estimated acceleration value of the drone; This represents the thrust estimate; This represents the attitude rotation matrix of the UAV; It represents a small positive number that is prevented from being divided by zero.

9. The method for safe and agile transportation planning and control of multiple UAVs carrying suspended loads as described in claim 8, characterized in that, In step six, the estimated cable force is used to further estimate the load mass, expressed as: in, This represents the estimated load mass; g represents the gravitational acceleration. and Let represent the estimated tension and direction of the i-th cable, respectively; When the difference between the estimated load mass and the planned load mass exceeds a threshold, the safety margin is increased, the maximum acceleration is reduced, or online replanning is triggered.

10. The method for safe and agile transportation planning and control of multiple UAVs with suspended loads as described in claim 1, characterized in that, In step six, the online replanning triggered by the safety margin and the target point change is expressed as follows: in, This indicates the minimum safety margin of the current system; and These represent the new target point and the old target point, respectively. Indicates the threshold for triggering changes in the target; Indicates the safe radius of the drone; Indicates the safe envelope radius of the load; Indicates the safe radius of the cable; Represents the obstacle distance field function; Indicates the position of the i-th drone; Indicates the load location; This indicates the position of the k-th sampling point on the i-th cable; Meanwhile, the following constraints ensure that no sudden state changes occur when switching between old and new trajectories: in, This represents the new flat output trajectory obtained through replanning; This indicates the trajectory currently being executed; Indicates the replanning trigger time; This indicates the highest derivative order that requires the inheritance of continuity.

Images