Optimization-based double-unmanned aerial vehicle hoisting rigid load path planning algorithm

Abstract

The invention discloses an optimization-based double-unmanned aerial vehicle hoisting rigid load path planning algorithm, and relates to the technical field of unmanned aerial vehicles. The method comprises the following steps: constructing a coordinate-free dynamic model, and describing positions and postures of a load and an unmanned aerial vehicle; generating a smooth trajectory by adopting differential flatness; initializing a trajectory through an A * algorithm constrained by a dynamical model, and optimizing trajectory parameters in combination with full-state security constraints and dynamic feasibility constraints; adopting nonlinear optimization to generate a dynamic feasible track; through nonlinear model predictive control, the state is predicted in real time, control input is generated, and an optimized trajectory is tracked. According to the invention, a path planning algorithm based on optimization of the double unmanned aerial vehicles for cooperatively hoisting the rigid load is established, the method is applied to actual testing, stable path planning is realized, and the problem of path planning of the unmanned aerial vehicles for hoisting the rigid load is solved.

Description

Technical Field

[0001] This application relates to the field of unmanned aerial vehicle (UAV) technology, specifically to an optimized dual-UAV path planning algorithm for lifting rigid loads. Background Technology

[0002] In recent years, drones have demonstrated wide-ranging application value in both civilian and military fields due to their flexibility and high maneuverability, attracting significant attention in areas such as logistics, rescue, and construction. With the rapid development of drone autonomous navigation, payload management, and airborne control technologies, traditional cargo transportation is facing unprecedented transformation. Drone aerial transport systems not only significantly improve transportation efficiency, providing flexible and efficient solutions, especially in environments where traditional transportation is constrained, but also play a crucial role in rapidly and accurately delivering supplies to designated areas during disaster relief and emergency responses.

[0003] However, despite the immense potential of unmanned aerial vehicle (UAV) air transport systems in theory and practice, numerous technical challenges remain in real-world applications. Among these, maintaining the safety and stability of the load in dynamic and complex environments is a critical issue. Especially when performing precision lifting tasks, UAVs not only need to maintain flight stability but also avoid collisions with obstacles to ensure the transported object is not damaged.

[0004] To address the challenges of drone-based load lifting, research on single-drone payload transport has been extensive. However, the limited payload capacity of individual drones and the inability to effectively control the attitude and position of rigid loads significantly restrict their application in various fields. Therefore, multi-drone collaborative load lifting has gradually become a research focus. Sreenath pioneered the differential flatness of multi-drone lifting and proposed a geometric control method to control the load's position and attitude, but did not consider how to plan safe paths for multi-drone collaborative lifting in complex environments. Zhang proposed a dual-drone collaborative lifting algorithm based on embedded formation, which can effectively traverse obstacle regions with specific geometric constraints. However, this method does not consider the case where the formation configuration plane is not coplanar with the geometrically constrained obstacle region, and it also assumes the load is a point mass model.

[0005] Furthermore, Wahba employs a hierarchical approach, first solving the geometric motion problem using a novel sampling-based sampler, and then performing trajectory optimization considering complete dynamic constraints to achieve multi-UAV collaborative lifting planning. Wang proposes a scheme based on real-time spatiotemporal trajectory planning to generate safe and dynamically feasible trajectories for multi-UAV collaborative lifting systems, ensuring obstacle avoidance and closed-loop control of the payload during flight. Although both propose effective planning schemes, they both model the load as a point mass model, neglecting the influence of the load's shape and attitude on the lifting task.

[0006] Therefore, research on optimized dual-UAV hoisting of rigid rod loads is particularly urgent. Summary of the Invention

[0007] The purpose of this application is to provide an optimized dual-UAV path planning algorithm for lifting rigid loads, which solves the path planning problem of UAVs lifting rigid loads.

[0008] To achieve the above objectives, the technical solution adopted in this application is as follows:

[0009] An optimized dual-UAV hoisting rigid load path planning algorithm includes:

[0010] S1: Two drones cooperate to transport a rigid load via a cable, and a coordinateless dynamic model is constructed to describe the position and attitude of the load and the drones;

[0011] S2: Using differential flatness, a smooth trajectory is generated by simplifying the state and control input through a flat output set;

[0012] S3: Initialize the trajectory using the A* algorithm constrained by the dynamic model, and optimize the trajectory parameters by combining full-state safety constraints and dynamic feasibility constraints;

[0013] S4: Employs nonlinear optimization to generate dynamic feasible trajectories, enabling real-time obstacle avoidance and stable load transportation;

[0014] S5: Through nonlinear model predictive control, it predicts the state in real time and generates control inputs to track and optimize the trajectory.

[0015] Furthermore, S1 includes:

[0016] The model makes the following assumptions: the two drones are connected to a rigid rod-shaped load by a massless and taut cable, the load has a uniform mass density, and the cable connection point is located at the center of mass of the quadcopter.

[0017] The two drones are transported by cable with a length of 2L. r The total mass is M r Based on the geometric relationship between the rigid rod-shaped load and the UAV, the following relationship is obtained:

[0018] x n =x r +(-1) n L r q r -l n q n

[0019] Where, x n and x rLet L be the position of the nth quadcopter and the center of mass of the rigid rod, respectively. r It is half the length of the rigid rod, l n Let q be the length of the nth cable. r Given the attitude of the rigid rod, q n Let L be the direction vector of the cable suspended on the nth quadcopter. To simplify the analysis, we assume that the length of the cable is equal to L1 = L2. When L1 ≠ L2, we can still derive similar results.

[0020] Using the Lagrange-d'Alembert variational principle, derive the Euler dynamic equations for n quadcopters and rigid rod loads:

[0021]

[0022] Among the variables The masses of the drone and the rigid rod load are respectively. R represents the rigid rod load and the length of the rope, respectively. n ∈SO(3) is the rotation matrix of the nth UAV. The position and velocity of the center of the rigid rod-shaped load particle. The posture of a rigid rod-shaped load. For pitch and yaw angles of a rigid rod load, Let n be the direction of the rope vector of the nth drone. Let n be the moment of inertia of the nth UAV. Let ω be the moment of inertia of a rigid rod-shaped load. r ∈TS 2 Let ω be the angular velocity of the rod-shaped load. n ∈TS 2 Let be the angular velocity of the rope. Let be the angular velocity of the nth drone. The thrust generated by the nth drone The torque generated by the nth drone It is a unit vector along the x, y, z axes in the world coordinate system.

[0023] Furthermore, S2 includes:

[0024] The system defines a flat output set, which describes the attitude of the load through two independent variables: the pitch angle and the azimuth angle of the rigid rod-like load. This is expressed as:

[0025] q r =[cos(θ) r cos(φ) r ),cos(θ r sin(φ) r ),sin(θr )] T .

[0026] in Pitch and yaw angles represent rigid rod-like loads;

[0027] Calculated using the above formula, the flat output set is: in Indicates the nth th The yaw angle of a quadcopter;

[0028] The trajectory is formulated using differential flatness and the MINCO method, optimizing time allocation, waypoint positions, and control force and execution time for the entire trajectory. The entire trajectory Z(t) is represented as M D-dimensional polynomials of order N = 2s⁻¹, with each segment represented as:

[0029]

[0030] in Let β(t) be the coefficient matrix, where β(t) = [1, t, ..., t 2s-1 ] T As a natural base, For the mth th The duration of each segment.

[0031] Furthermore, in S3, the trajectory is initialized using the A* algorithm constrained by the dynamic model, including:

[0032] The system adopts A* based on dynamic model constraints to expand the state-time search space;

[0033] By combining the constraints of maximum speed, acceleration, and turning radius, the dynamic reachability of the next state is predicted;

[0034] A heuristic function with minimum control cost is introduced to generate an initial trajectory that satisfies obstacle avoidance and continuity, thereby improving the solver's convergence speed and optimization quality.

[0035] Furthermore, the full-state safety constraints in S3 include:

[0036] Define different safety margins for drones, cable sampling points, and load sampling points. Based on these margins, formulate corresponding collision avoidance constraints to prevent any component of the system from getting too close to obstacles.

[0037]

[0038] k∈[1,...,K],n∈1,2.

[0039] Where E(·) represents the nearest distance between a point and an obstacle found by the Discrete Euclidean Directed Distance Field (ESDF). These represent the safe distances for the quadcopter, rigid rod load, and cable, respectively, with K being the maximum number of sampling points.

[0040] Based on the collision avoidance constraint, the penalty function J can be obtained. safe :

[0041]

[0042] Calculate the gradient of the penalty function with respect to the trajectory parameters to achieve gradient-based trajectory optimization under collision avoidance constraints:

[0043]

[0044] Where β=[1,t,\dots,t 2s-1 ] T As a natural base, The penalty function is relative to x r The derivative of The penalty function is relative to θ r The derivative of The penalty function is relative to θ r The derivative of This represents the gradient between a point and an obstacle obtained from a Discrete Euclidean Directed Range Field (ESDF) query. For q r Relative to θ r The derivative of For q r Relative to φ r The derivative;

[0045] The above gradients are used to optimize the trajectory parameters, enabling the entire system to effectively meet obstacle avoidance constraints.

[0046] Furthermore, the dynamic feasibility constraints in S3 include:

[0047] By imposing maximum speed and acceleration limits, upper and lower limits on standard thrust, and a maximum allowable tilt angle limit on each quadcopter during trajectory optimization, the speed and acceleration of the quadcopter are kept within a reasonable range.

[0048]

[0049] in These are the maximum speed and acceleration values ​​of the drone.

[0050] Constraints are imposed on the standardized thrust to ensure that all control inputs are within physically feasible limits:

[0051] f min ≤||f n||≤f max ,n∈1,2.

[0052] Where f min ,f max These are the minimum and maximum thrust of the drone.

[0053] Specify constraints on the tilt angle, and set the maximum allowable tilt angle to be [value missing]. To ensure flight stability and safety:

[0054]

[0055] Furthermore, S4 includes:

[0056] A nonlinear optimization model is constructed, balancing trajectory smoothness, attitude adjustment, and time minimization through weighting factors. While satisfying given constraints, the model minimizes control workload and execution time. The trajectory optimization can be expressed as follows:

[0057]

[0058]

[0059] Where λ x , and λ T This is a weighting factor used to balance the relative importance of trajectory smoothness, attitude adjustment, and time minimization. Represents the set of all the above constraints;

[0060] A gradient-based optimization algorithm is employed, and the ACADO toolkit is used to achieve efficient solution.

[0061] The optimized dynamic feasible trajectory supports real-time obstacle avoidance and stable load transportation in complex environments.

[0062] Furthermore, S5 includes:

[0063] Nonlinear model predictive control is used to predict the future system state in real time;

[0064] The controller generates control inputs that satisfy dynamic constraints and tracks the optimized trajectory.

[0065] The controller addresses external disturbances and model uncertainties through automatic differentiation and a fast numerical solver;

[0066] In dense jungle and narrow passage scenarios, airborne lidar is used to detect and replan trajectories in real time;

[0067] The position and attitude of the UAV are dynamically adjusted according to the payload geometry to maintain payload stability.

[0068] In summary, this application includes at least one of the following beneficial technical effects:

[0069] An optimized path planning algorithm for the collaborative lifting of rigid loads by two UAVs was established, and an algorithm model for lifting rigid bodies by two UAVs was given. Based on the optimized path planning algorithm, optimization functions and constraints were designed, and an optimized path planning algorithm was constructed. The designed method was applied to a real-world scenario for practical testing, achieving stable path planning and controlling the collaborative lifting of rigid loads by two UAVs to successfully reach the target position, thus solving the path planning problem of UAVs lifting rigid loads. Attached Figure Description

[0070] Figure 1 This is a flowchart of the application;

[0071] Figure 2 This is a schematic diagram of the modeling used in this application;

[0072] Figure 3 This is a diagram showing the initial experimental results of this application;

[0073] Figure 4 This is a diagram showing the results of the second stage of the experiment in this application;

[0074] Figure 5 This is a diagram showing the final stage of the experiment in this application. Detailed Implementation

[0075] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. Furthermore, the technical features involved in the various embodiments described below can be combined with each other as long as they do not conflict with each other.

[0076] like Figure 1 As shown, an optimized dual-UAV hoisting rigid load path planning algorithm includes:

[0077] S1: Two UAVs collaboratively transport a rigid load via a cable. A coordinateless dynamic model is constructed to describe the position and attitude of the load and the UAVs, such as... Figure 2 As shown, it includes:

[0078] The model makes the following assumptions: the two drones are connected to a rigid rod-shaped load by a massless and taut cable with uniform load mass density, and the cable connection point is located at the center of mass of the quadcopter.

[0079] The two drones are transported by cable with a length of 2L. r The total mass is M rBased on the geometric relationship between the rigid rod-shaped load and the UAV, the following relationship is obtained:

[0080] x n =x r +(-1) n L r q r -l n q n

[0081] Where, x n and x r Let L be the position of the nth quadcopter and the center of mass of the rigid rod, respectively. r It is half the length of the rigid rod, l n Let q be the length of the nth cable. r Given the attitude of the rigid rod, q n Let L be the direction vector of the cable suspended on the nth quadcopter. To simplify the analysis, we assume that the length of the cable is equal to L1 = L2. When L1 ≠ L2, we can still derive similar results.

[0082] Using the Lagrange-d'Alembert variational principle, derive the Euler dynamic equations for n quadcopters and rigid rod loads:

[0083]

[0084] Among the variables The masses of the drone and the rigid rod load are respectively. R represents the rigid rod load and the length of the rope, respectively. n ∈SO(3) is the rotation matrix of the nth UAV. The position and velocity of the center of the rigid rod-shaped load particle. The posture of a rigid rod-shaped load. For pitch and yaw angles of a rigid rod load, Let n be the direction of the rope vector of the nth drone. Let n be the moment of inertia of the nth UAV. Let ω be the moment of inertia of a rigid rod-shaped load. r ∈TS 2 Let ω be the angular velocity of the rod-shaped load. n ∈TS 2 Let be the angular velocity of the rope. Let be the angular velocity of the nth drone. The thrust generated by the nth drone The torque generated by the nth drone It is a unit vector along the x, y, z axes in the world coordinate system.

[0085] S2: Employing differential flatness, a smooth trajectory is generated by simplifying the state and control inputs through a flat output set, including:

[0086] The system defines a flat output set. Since the pitch angle is ignored for the rigid rod load, it can be simplified by a unit vector. To make load attitude optimization easier, the load attitude is described by two independent variables: the pitch angle and the azimuth angle of the rigid rod load. Therefore, the load attitude can be represented as:

[0087] q r =[cos(θ) r cos(φ) r ),cos(θ r sin(φ) r ),sin(θ r )] T .

[0088] in The pitch and yaw angles represent rigid rod-like loads.

[0089] Calculated using the above formula, the flat output set is: in Indicates the nth th The yaw angle of a quadcopter.

[0090] By utilizing differential flatness, we can avoid obtaining state and control inputs through integral dynamic equations. Thus, we can use multidimensional piecewise polynomials to represent the flat output trajectory, and then realize the motion planning of the entire system by deforming the flat output trajectory.

[0091] The trajectory is formulated using differential flatness and the MINCO method, optimizing time allocation, waypoint positions, and control force and execution time for the entire trajectory. The entire trajectory Z(t) is represented as M D-dimensional polynomials of order N = 2s⁻¹, with each segment represented as:

[0092]

[0093] in Let β(t) be the coefficient matrix, where β(t) = [1, t, ..., t 2s-1 ] T As a natural base, For the mth th The duration of each segment.

[0094] Differential flatness is a structural property of a class of nonlinear systems that completely describes the system's state and control inputs through a set of so-called flat outputs. Specifically, if a system is differentially flat, then there exists a set of output variables, and all the system's states and control variables can be expressed as functions of these output variables and their finite-order derivatives. This property allows trajectory planning problems to be performed directly in the flat output space: simply plan a smooth trajectory for the flat outputs, and then use algebraic operations and derivative relationships to inversely deduce the corresponding state trajectory and control inputs, thus avoiding numerical integration or solving differential equations for the nonlinear dynamic system. This method is not only computationally efficient but also facilitates the embedding of constraints and trajectory optimization.

[0095] S3: Initialize the trajectory using the A* algorithm constrained by the dynamic model, and optimize the trajectory parameters by combining full-state safety constraints and dynamic feasibility constraints.

[0096] To improve optimization efficiency and ensure the feasibility of initial trajectory values, an A* algorithm based on dynamic model constraints is introduced for trajectory initialization. Traditional A* algorithms plan paths solely based on geometric information, which is insufficient to meet the controllability and continuity requirements of dynamic systems. Dynamic A*, however, incorporates the system's dynamic constraints (such as maximum velocity, acceleration, and turning radius) into the search process, ensuring that the generated path is not only effective in obstacle avoidance but also executable by the system. Specifically, the search space is expanded to a state-time space, considering the system's current velocity and attitude at each search node, predicting the next state based on discrete control inputs, and evaluating its dynamic reachability. In terms of heuristic function design, in addition to the traditional Euclidean distance, a cost estimation based on minimum control cost is introduced, thus better reflecting real-world execution costs. The resulting path, as the initial trajectory for the optimization problem, not only meets basic environmental obstacle avoidance requirements but also provides dynamically feasible and continuous initial values ​​for subsequent flat trajectory optimization, significantly improving the solver's convergence speed and optimization quality.

[0097] Full-state safety constraints are implemented to ensure the overall safety of the system. All components (including the trajectories of the two UAVs, the suspension cable, and the rigid rod load) must maintain a safe distance from any obstacles detected by the onboard sensing system. Different safety margins are defined for the UAVs, cable sampling points, and load sampling points. Based on these margins, corresponding collision avoidance constraints are developed to prevent any component of the system from getting too close to obstacles.

[0098]

[0099] k∈[1,...,K],n∈1,2.

[0100] Where E(·) represents the nearest distance between a point and an obstacle found by the Discrete Euclidean Directed Distance Field (ESDF). These represent the safe distances for the quadcopter, rigid rod load, and cable, respectively, with K being the maximum number of sampling points.

[0101] Based on the collision avoidance constraint, the penalty function J can be obtained. safe :

[0102]

[0103] Calculate the gradient of the penalty function with respect to the trajectory parameters to achieve gradient-based trajectory optimization under collision avoidance constraints:

[0104]

[0105] Where β=[1,t,\dots,t 2s-1 ] T As a natural base, The penalty function is relative to x r The derivative of The penalty function is relative to θ r The derivative of The penalty function is relative to θ r The derivative of This represents the gradient between a point and an obstacle obtained from a Discrete Euclidean Directed Range Field (ESDF) query. For q r Relative to θ r The derivative of For q r Relative to φ r The derivative of .

[0106] The above gradients are used to optimize the trajectory parameters, enabling the entire system to effectively meet obstacle avoidance constraints.

[0107] Dynamic feasibility constraints were designed to ensure that the system state and control inputs adhered to specified constraints and maintained trajectory feasibility. These constraints primarily included the maximum speed and acceleration limits of the quadcopter, the upper and lower limits of the standard thrust, and the maximum permissible tilt angle limit for each quadcopter. By explicitly incorporating these constraints into the trajectory optimization process, we ensured that the final trajectory was not only dynamically feasible but also physically executable by the quadcopter, thereby improving the safety and reliability of mission execution.

[0108] By imposing maximum speed and acceleration limits, upper and lower limits on standard thrust, and a maximum allowable tilt angle limit on each quadcopter during trajectory optimization, the speed and acceleration of the quadcopter are kept within a reasonable range.

[0109]

[0110] in These are the maximum speed and acceleration values ​​of the drone.

[0111] Constraints are imposed on the standardized thrust to ensure that all control inputs are within physically feasible limits:

[0112] f min ≤||f n ||≤f max ,n∈1,2.

[0113] Where f min ,f max These are the minimum and maximum thrust of the drone.

[0114] Specify constraints on the tilt angle, and set the maximum allowable tilt angle to be [value missing]. To ensure flight stability and safety:

[0115]

[0116] S4: Employs nonlinear optimization to generate dynamic feasible trajectories, enabling real-time obstacle avoidance and stable load transportation.

[0117] A nonlinear optimization model is constructed, balancing trajectory smoothness, attitude adjustment, and time minimization through weighting factors. While satisfying given constraints, the model minimizes control workload and execution time. The trajectory optimization can be expressed as follows:

[0118]

[0119] Where λ x , and λ T This is a weighting factor used to balance the relative importance of trajectory smoothness, attitude adjustment, and time minimization. Represents the set of all the above constraints;

[0120] A gradient-based optimization algorithm is employed, and the ACADO toolkit is used to achieve efficient solution.

[0121] The optimized dynamic feasible trajectory supports real-time obstacle avoidance and stable load transportation in complex environments.

[0122] S5: Through nonlinear model predictive control, it predicts the state in real time and generates control inputs, tracking and optimizing the trajectory, including:

[0123] Nonlinear model predictive control is used to predict the future system state in real time;

[0124] The controller generates control inputs that satisfy dynamic constraints and tracks the optimized trajectory.

[0125] The controller addresses external disturbances and model uncertainties through automatic differentiation and a fast numerical solver;

[0126] In dense jungle and narrow passage scenarios, airborne lidar is used to detect and replan trajectories in real time;

[0127] The position and attitude of the UAV are dynamically adjusted according to the payload geometry to maintain payload stability.

[0128] This application constructs complex scenarios and uses two drones to lift a rigid rod-shaped load across various obstacles to reach the target location. The method proposed in this application is then tested in these complex scenarios.

[0129] like Figures 3-5 As shown, the experiment is divided into three representative stages.

[0130] In the initial stage ( Figure 3 The drone performs active path planning in the dense jungle, operating with limited perception and generating a local safe trajectory based on nearby obstacles. Because distant obstacles are not detected, the trajectory changes very little.

[0131] In the second stage ( Figure 4 When two drones encounter a narrow passage, they actively plan feasible paths, while an onboard LiDAR detects unknown obstacles ahead. The drones must navigate through the narrow passage, triggering the planner to replan their trajectories in real time. This achieves obstacle avoidance with minimal deviation while ensuring payload stability.

[0132] In the final stage ( Figure 5 The two drones successfully traversed a narrow passageway and avoided all new obstacles. The drones' position and orientation were dynamically adjusted based on the payload geometry, demonstrating powerful real-time obstacle avoidance capabilities.

[0133] These results demonstrate that the proposed planner can handle dynamic obstacle avoidance, adapt to real-time updates, and navigate effectively in confined spaces. Unlike methods that rely on complete maps or conservative behavior, it enables flexible, perception-driven navigation suitable for complex aerial maneuvering missions.

[0134] The proposed algorithm was validated in complex real-world scenarios. By constructing complex scenarios, dual UAVs were used to sling a rigid rod-shaped load across various obstacles to reach the target location. The algorithm was tested in these complex scenarios. The results validated the effectiveness of the algorithm, demonstrating that the dual UAVs possess the ability to avoid obstacles in complex environments in real time. This application can handle dynamic obstacle avoidance, adapt to real-time updates, and effectively navigate in confined spaces. Unlike methods that rely on complete maps or conservative behavior, it enables flexible, perception-driven navigation suitable for complex aerial maneuvering tasks.

[0135] Embodiments of this application may be provided as methods, systems, or computer program products. Therefore, this application may take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0136] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0137] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0138] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0139] Content not described in detail in this application specification belongs to the prior art known to those skilled in the art. It is hereby indicated that the above description is intended to help those skilled in the art understand this application, but does not limit the scope of protection of this application. Any equivalent substitutions, modifications, improvements, or simplifications of the above descriptions that do not depart from the essential content of this application fall within the scope of protection of this application.

Claims

1. A method for path planning of rigid load lifting based on optimization of dual UAV cooperative lifting, characterized in that, include: S1: Two drones cooperate to transport a rigid load via a cable, and a coordinateless dynamic model is constructed to describe the position and attitude of the load and the drones; S2: Using differential flatness, a smooth trajectory is generated by simplifying the state and control input through a flat output set; S3: Initialize the trajectory using the A* algorithm constrained by the dynamic model, and optimize the trajectory parameters by combining full-state safety constraints and dynamic feasibility constraints; S4: Employs nonlinear optimization to generate dynamic feasible trajectories, enabling real-time obstacle avoidance and stable load transportation; S5: Through nonlinear model predictive control, it predicts the state in real time and generates control inputs to track and optimize the trajectory.

2. The method for optimizing the path planning of a dual-UAV cooperative hoisting of a rigid load according to claim 1, characterized in that, S1 includes: The model makes the following assumptions: the two drones are connected to a rigid rod-shaped load by a massless and taut cable, the load has a uniform mass density, and the cable connection point is located at the center of mass of the quadcopter. The two drones are transported by cable with a length of 2L. r The total mass is M r Based on the geometric relationship between the rigid rod-shaped load and the UAV, the following relationship is obtained: x n =x r +(-1) n L r q r -l n q n Where, x n and x r Let L be the position of the nth quadcopter and the center of mass of the rigid rod, respectively. r It is half the length of the rigid rod, l n Let q be the length of the nth cable. r Given the attitude of the rigid rod, q n Let L be the direction vector of the cable suspended on the nth quadcopter. To simplify the analysis, we assume that the length of the cable is equal to L1 = L2. When L1 ≠ L2, we can still derive similar results. Using the Lagrange-d'Alembert variational principle, derive the Euler dynamic equations for n quadcopters and rigid rod loads: Among the variables The masses of the drone and the rigid rod load are respectively. R represents the rigid rod load and the length of the rope, respectively. n ∈SO(3) is the rotation matrix of the nth UAV. The position and velocity of the center of the rigid rod-shaped load particle. The posture of a rigid rod-shaped load. For pitch and yaw angles of a rigid rod load, Let n be the direction of the rope vector of the nth drone. Let n be the moment of inertia of the nth UAV. Let ω be the moment of inertia of a rigid rod-shaped load. r ∈TS 2 Let ω be the angular velocity of the rod-shaped load. n ∈TS 2 Let be the angular velocity of the rope. Let be the angular velocity of the nth drone. The thrust generated by the nth drone The torque generated by the nth drone It is a unit vector along the x, y, z axes in the world coordinate system.

3. The method for optimizing the path planning of a dual-UAV cooperative hoisting of a rigid load according to claim 1, characterized in that, S2 include: The system defines a flat output set, which describes the attitude of the load through two independent variables: the pitch angle and the azimuth angle of the rigid rod-like load. This is expressed as: q r =[cos(θ r )cos(φ r ),cos(θ r )sin(φ r ),sin(θ r )] T . in Pitch and yaw angles represent rigid rod-like loads; Calculated using the above formula, the flat output set is: in Indicates the nth th The yaw angle of a quadcopter; The trajectory is formulated using differential flatness and the MINCO method, optimizing time allocation, waypoint positions, and control force and execution time for the entire trajectory. The entire trajectory Z(t) is represented as M D-dimensional polynomials of order N = 2s⁻¹, with each segment represented as: in Let β(t) be the coefficient matrix, where β(t) = [1, t, ..., t 2s-1 ] T As a natural base, For the mth th The duration of each segment.

4. The method for optimizing the path planning of a dual-UAV cooperative hoisting of a rigid load according to claim 1, characterized in that, The trajectory initialization in S3 using the A* algorithm constrained by the dynamic model includes: The system adopts A* based on dynamic model constraints to expand the state-time search space; By combining the constraints of maximum speed, acceleration, and turning radius, the dynamic reachability of the next state is predicted; A heuristic function with minimum control cost is introduced to generate an initial trajectory that satisfies obstacle avoidance and continuity, thereby improving the solver's convergence speed and optimization quality.

5. The method for optimizing the path planning of a dual-UAV cooperative hoisting of a rigid load according to claim 1, characterized in that, The full-state safety constraints in S3 include: Define different safety margins for drones, cable sampling points, and load sampling points. Based on these margins, formulate corresponding collision avoidance constraints to prevent any component of the system from getting too close to obstacles. Where E(·) represents the nearest distance between a point and an obstacle found by the Discrete Euclidean Directed Distance Field (ESDF). These represent the safe distances for the quadcopter, rigid rod load, and cable, respectively, with K being the maximum number of sampling points. Based on the collision avoidance constraint, the penalty function J can be obtained. safe : Calculate the gradient of the penalty function with respect to the trajectory parameters to achieve gradient-based trajectory optimization under collision avoidance constraints: Where β=[1,t,\dots,t 2s-1 ] T As a natural base, The penalty function is relative to x r The derivative, The penalty function is relative to θ r The derivative, The penalty function is relative to θ r The derivative, This represents the gradient between a point and an obstacle obtained from a Discrete Euclidean Directed Range Field (ESDF) query. For q r Relative to θ r The derivative, For q r Relative to φ r The derivative; The above gradients are used to optimize the trajectory parameters, enabling the entire system to effectively meet obstacle avoidance constraints.

6. The method for optimizing the path planning of a dual-UAV cooperative hoisting of a rigid load according to claim 1, characterized in that, The dynamic feasibility constraints in S3 include: By imposing maximum speed and acceleration limits, upper and lower limits on standard thrust, and a maximum allowable tilt angle limit on each quadcopter during trajectory optimization, the speed and acceleration of the quadcopter are kept within a reasonable range. in These are the maximum speed and acceleration values ​​of the drone; Constraints are imposed on the standardized thrust to ensure that all control inputs are within physically feasible limits: f min ≤||f n ||≤f max ,n∈1,2. Where f min ,f max These are the minimum and maximum thrust of the drone; Specify constraints on the tilt angle, and set the maximum allowable tilt angle to be [value missing]. To ensure flight stability and safety:

7. The method for optimizing the path planning of a dual-UAV cooperative hoisting of a rigid load according to claim 1, characterized in that, S4 includes: A nonlinear optimization model is constructed, balancing trajectory smoothness, attitude adjustment, and time minimization through weighting factors. While satisfying given constraints, the model minimizes control workload and execution time. The trajectory optimization can be expressed as follows: Where λ x , and λ T This is a weighting factor used to balance the relative importance of trajectory smoothness, attitude adjustment, and time minimization. Represents the set of all the above constraints; A gradient-based optimization algorithm is employed, and efficient solutions are obtained using the ACADO toolkit. The optimized dynamic feasible trajectory supports real-time obstacle avoidance and stable load transportation in complex environments.

8. The method for optimizing the path planning of a dual-UAV cooperative hoisting of a rigid load according to claim 1, characterized in that, S5 include: Nonlinear model predictive control is used to predict the future system state in real time; The controller generates control inputs that satisfy dynamic constraints and tracks the optimized trajectory. The controller addresses external disturbances and model uncertainties through automatic differentiation and a fast numerical solver; In dense jungle and narrow passage scenarios, airborne lidar is used to detect and replan trajectories in real time; The position and attitude of the UAV are dynamically adjusted according to the payload geometry to maintain payload stability.

Images