Unmanned aerial vehicle cluster trajectory planning control method under manual guidance

Abstract

The invention discloses an unmanned aerial vehicle cluster trajectory planning control method under manual guidance. The unmanned aerial vehicle cluster trajectory planning control method comprises the steps of 1, modeling a trajectory optimization problem by utilizing manual guidance information; 2, designing a cluster control method based on a local interaction mechanism; 3, generating an initial track of the artificially guided unmanned aerial vehicle based on the current environment; and 4, rapidly re-planning the trajectory of the person in the loop. According to the method, the intention of the human operator can be quickly converted into the control instruction of the unmanned aerial vehicle cluster through the man-machine interaction interface, the man-machine cooperation efficiency is improved, and the group safety is ensured. Then, considering the communication burden of the cluster, and dividing the unmanned aerial vehicles in the cluster into manually guided unmanned aerial vehicles and subordinate unmanned aerial vehicles; and the artificially guided unmanned aerial vehicle tracks the planned trajectory. For a lower-level unmanned aerial vehicle, a corresponding control algorithm is designed based on a local interaction mechanism. This enables a cluster of drones to safely and collaboratively traverse a dynamic, dense environment.

Description

Technical Field

[0001] The invention discloses a trajectory planning and control method for a cluster of unmanned aerial vehicles (UAVs) under artificial guidance, and belongs to the field of autonomous control of UAVs. Background Art

[0002] In the current field of aerospace and robotics, efficient multi-machine cluster trajectory planning and control systems are of great significance for performing complex tasks and improving operational efficiency.

[0003] Currently, UAV swarms face challenges from dynamic external environments and communication constraints between UAVs. Existing methods are difficult to ensure the coordination and optimality of swarm motion due to local interactions within the swarm. To address this problem, this paper proposes a method that integrates the intelligence of human operators in perception and decision-making into the trajectory planning cost function through a human-computer interaction interface, which makes the planned trajectory more reasonable.

[0004] In addition, some typical birds (such as Harris hawks and pigeons) have obvious social hierarchical structures, and the hierarchical relationship has a sequential transmission phenomenon. For Harris hawks, age plays a major role in dominating hierarchical relationships, and older individuals have a higher dominance than younger individuals. If the prey escapes from the sight of the hawks, the hawks usually adopt a hierarchical attack strategy, that is, the strongest hawk is always the "leader" to lead other hawks to catch the prey. For pigeons, the pigeons can not only form a coordinated and orderly movement pattern based on the topological hierarchical relationship, but also quickly traverse complex environments through reactive obstacle avoidance. Drawing on biological swarm intelligence, individuals in drone swarms can be divided into different levels, which can better guide drone swarms to fly safely in complex environments. High-level drones are designated as manually guided drones to track the planned trajectory, thereby ensuring the smoothness and optimality of group movement. Low-level drones operate based on the designed control algorithm, enabling drone swarms to quickly and safely traverse dense environments. This method has advantages in improving the smoothness and coordination of group movement trajectories.

[0005] In the current research on drone systems, drone clusters and ground stations are connected through communication equipment to form a "platform without humans, system with humans" system. Therefore, the problem of authority allocation between operators and drone clusters has arisen. For example, during the recovery process of drones, although the existing planning and control algorithms can better enable drones to dock under ideal conditions, when faced with complex environments, they are still highly dependent on skilled pilots for control. Especially in complex meteorological conditions, such as strong winds, rain and snow or low visibility environments, or in spatially restricted areas (no-fly zones), such as gaps between urban buildings, densely forested areas or narrow valleys, it becomes more difficult to safely recover drones.

[0006] In addition, the dynamic external environment poses a more severe challenge to the perception and decision-making of the UAV system. In this case, the UAV swarm needs to intelligently adapt to the changing environment while completing the task. However, due to the limitations of perception capabilities or the system itself, this may lead to inaccurate recognition of surrounding environmental information, thus affecting the perception and decision-making of the UAV, especially in complex environments without human guidance.

[0007] Some reactive control methods, such as speed barriers and artificial potential field methods, have been used for dynamic obstacle avoidance of UAVs. These methods only consider the currently moving obstacles and only calculate one step of operation. Some other methods consider the speed of the obstacle to be a constant, or use filtering-based methods to predict the motion trajectory of the obstacle. This is difficult to achieve in complex and dynamic environments because many objects have various motion patterns and do not meet the assumptions of constant speed or constant acceleration, and sensors may not be able to accurately sense dangers. Due to these problems, the above technologies lack flexibility and may result in performance degradation in complex and dynamic environments. Therefore, incorporating the rich flight experience and decision-making ability of human operators into drone clusters can bring many benefits. Human operators only need to issue a command to change the state of the entire drone cluster.

[0008] To ensure data transmission and communication within a drone cluster, each drone communicates with all other drones. While this can enhance data sharing and decision consistency, it may cause excessive communication delays and system data redundancy. Swarm intelligence provides a feasible idea for distributed cluster control of drones and helps to reduce the communication burden. Bird flocks and drone clusters have similarities in mission requirements. In the hierarchy, individuals with more environmental information will be located at a higher level or have a higher priority, which helps to maintain the consistency and stability of the drone cluster movement and improve decision-making efficiency. Therefore, in drone clusters, the hierarchical design idea of ​​using high-level drones as artificial guide drones, giving full play to the advantages of humans in decision-making, can enable drone clusters to flexibly respond to dynamic and complex environments. Summary of the invention

[0009] The present invention proposes a UAV cluster trajectory planning and control method under artificial guidance, which integrates the decision-making ability of human operators into the trajectory planning of UAV clusters through a human-computer interaction interface, and designs a corresponding cluster control algorithm on this basis, aiming to solve the trajectory planning and control of the UAV cluster system and cope with the needs of complex dynamic scenes.

[0010] The present invention solves the problem of cooperative control of UAV cluster system in complex environment by using the following technology. The framework of this method is as follows: Figure 1 As shown, the specific steps are as follows:

[0011] Step 1: Use the information of manual guidance to model the trajectory optimization problem, including: modeling the trajectory optimization problem of the manually guided UAV, which is expressed as an unconstrained nonlinear optimization problem, including manual guidance penalty, collision penalty, control input penalty, trajectory smoothness, and dynamic feasibility. The specific process is as follows:

[0012] In UAV trajectory planning, B-spline can represent a smooth and easy-to-optimize flight trajectory, and B-spline is uniquely determined by its degree, control points and node vectors. The trajectories planned in the present invention are all characterized by a 3rd-order uniform B-spline curve s(t) parameterized by time t, where Q is the position control point, V is the velocity control point, A is the acceleration control point, J is the jerk control point, and the time of each trajectory is dt.

[0013] The trajectory optimization problem of a manually guided UAV consists of five aspects and is formulated as an unconstrained nonlinear optimization problem with the following cost function:

[0014]

[0015] Among them, Q represents the position control point, f h is the artificial guidance penalty term, f c is the collision penalty term, f u is the control input penalty term, f s represents the trajectory smoothness, f d represents the kinetic feasibility, f total represents the total cost function, and λ1,λ2,λ3,λ4,λ5 represent the weights of the corresponding optimization items. In complex or dynamic environments, especially when facing emergencies or unexpected situations, manual guidance plays a vital role in the functionality and safety of unmanned systems. The purpose of manual guidance is to use human decision-making to deal with uncertainty, quickly perceive environmental changes, identify potential risks, and take appropriate measures to deviate the trajectory from the direction guided by the human operator.

[0016] Since the shape of a B-spline curve is only affected by local control points, changing the position of a specific control point affects only the nearby portion of the curve, not the entire curve. A human operator can simply manipulate the mouse to adjust the control points near the hazard, thereby deviating the trajectory from the hazard and intuitively affecting the movement of the drone cluster. The operator first clicks the mouse on the human-machine interface to select the starting position and set the desired direction for the movement of the control point. Then, they drag and release the mouse to complete the setting. By integrating human decision-making into the algorithm, the operator's choices are naturally incorporated into the trajectory optimization process, enabling effective human-machine collaboration. In the human-machine interface, the location where the operator clicks the mouse is p start , the position where the operator releases the mouse is p end . Control point Q i(i represents the index of the position control point Q) is affected by the human operator and is located at p start is a sphere with radius r at its center.

[0017] f h As shown below:

[0018]

[0019] Among them, c hg =||p start -p end ||2 indicates the strength of artificial guidance, Represents the desired guiding position of the control point.

[0020] To avoid attracting control points to the same position, it is necessary to choose In order to give full play to the role of artificial guidance. start and the end point p end and perpendicular to line segment p start p end The plane of the starting point. The control point Q within the manual guidance range i The projection onto plane π is expressed as

[0021] Collision penalty term f c :

[0022]

[0023] Among them, d(Q i ) represents the control point Q i The distance to the nearest obstacle,

[0024]

[0025] Control input penalty term f u :

[0026]

[0027] Smoothness penalty f s :

[0028]

[0029] Dynamic feasibility penalty term f d : The dynamic feasibility penalty can prevent the planned trajectory from exceeding the physical limitations of the drone. This soft constraint is designed as a smooth function with continuous second-order derivatives.

[0030] f d =f v +f a(6)

[0031] in,

[0032] δ=0.5,n m =4, x, y represent the horizontal and vertical coordinates respectively, and z represents the height. The above optimization problem is solved by the limited memory Peloton-Fletcher-Goldforb-Senner algorithm.

[0033] Step 2: Design a cluster control method based on a local interaction mechanism, including: designing a formation-keeping control quantity that enables individuals to maintain a relative distance from each other, an obstacle avoidance control quantity, a control quantity to prevent individuals from colliding with each other, a speed alignment control quantity, a control quantity to reach a desired height, a control quantity to converge the speed of the UAV cluster, and a control quantity to prevent the cluster from being overly dispersed, based on the kinematics and dynamics models of the lower-level UAVs. The specific process is as follows:

[0034] Compared with lower-level drones, human-guided drones have a wider airborne communication range and stronger anti-interference ability, which effectively ensures the stability and safety of the cluster in emergency situations. Human operators can make quick decisions using intuitive perception and rich experience. The present invention divides the drone cluster into the highest-level human-guided drone and multiple lower-level drones to form a hierarchical structure. The human-guided drone ranks first, while the lower-level drones rank from the second to the nth.

[0035] In a drone swarm, subordinate drones achieve cooperative flight through an artificial potential field method under the guidance of a human-guided drone. This helps to reduce the communication burden of controlling the drone swarm. Subordinate drones follow simple local interaction rules based on the state of their peers, including cohesion, alignment, and separation. The dynamic model of subordinate drones in a drone swarm is as follows:

[0036]

[0037] Among them, m i is the mass of each lower-level UAV, which is set to 1 kg in the present invention. v i ,u i are the Euclidean space position vector, velocity vector and control input of each slave drone, respectively, i v i is the velocity damping term. Let x, y represent the horizontal and vertical coordinates respectively, and z represent the height. The control input of each subordinate drone is given by the following equation:

[0038]

[0039] u fis the formation-keeping control quantity that enables individuals to maintain relative distances between each other:

[0040]

[0041] in, k f is the control gain, represents the distance between individuals i and j, w j represents the influence weight of j on i, r comm and r d represents the horizontal communication distance and the expected distance between i and j.

[0042] u obs is the obstacle avoidance control quantity:

[0043]

[0044] in, k obs is the control gain, represents the square of the distance between individual i and the surface of obstacle j, r sense Represents the individual's perceived distance to an obstacle.

[0045] u c Represents the control amount to prevent individuals from colliding with each other:

[0046]

[0047] in, k c represents the control gain, r lim is the distance at which the potential field function affects individual j.

[0048] Alignment control amount u v_hg Align the velocity of individual i with the velocity of the human-guided drone:

[0049]

[0050] Among them, k v_hg is the control gain, Indicates the speed of the manually guided drone.

[0051] u h_hg The control amount to guide the drone subordinate drone i to reach the desired altitude:

[0052]

[0053] Among them, k h_hg To control the gain, Indicates the altitude of the manually guided drone.

[0054] u vn The control quantity that makes the speed of the lower-level drone converge is shown in the following formula:

[0055]

[0056] Among them, k vn To control the gain.

[0057] Aggregate control amount u co The lower-level drones are kept at an appropriate distance from each other to prevent the cluster from being too dispersed, as shown in the following formula:

[0058]

[0059] in, k co represents the control gain, r lim2 Indicates the minimum interaction distance of the aggregation control component, beyond which the subordinate drones begin to aggregate with each other.

[0060] In order to achieve effective control of the lower-level UAVs, each lower-level UAV is equipped with a first-order Mach hold autopilot, a first-order heading hold autopilot, and a second-order altitude hold autopilot. The time constants of the three autopilots are τ v , τ λ , τ h In order to make the UAV more consistent with the characteristics of the actual system, V is defined max ,V min ,n max ,λ max ,λ min , g are respectively the maximum horizontal speed, the minimum horizontal speed, the maximum lateral overload, the maximum height change rate, the minimum height change rate, and the gravity acceleration g = 9.8m / s 2 .

[0061] Step 3: Generate the initial trajectory of the artificially guided drone based on the current environment, including: using the fast traversal random tree algorithm to find a collision-free path from the starting point to the target in a static environment containing convex obstacles such as cuboids and cylinders, then solving the quadratic programming problem from the obtained path to generate the initial minimum jerk trajectory, and then optimizing the initial trajectory. The specific process is as follows:

[0062] The present invention uses a path planning method that uses Rapidly-exploring Random Trees* sampling. The key advantage of the Rapidly-exploring Random Trees algorithm is that it can efficiently handle high-dimensional, complex, unstructured environments. Through random sampling and rewiring strategies, it can not only find connected paths, but also gradually optimize the quality of the paths. At the same time, it can also naturally integrate kinematic constraints, have stronger adaptability and robustness, and is particularly suitable for scenarios where drone clusters need to plan paths in complex dynamic environments. Search-based algorithms are limited by grid division and computational complexity, and perform poorly in unstructured environments.

[0063] The present invention finds a collision-free path from the starting point to the target in a static environment containing convex obstacles such as cuboids and cylinders. The fast traversal random tree algorithm provides a rough but connected path, and then the initial minimum jerk trajectory is generated by solving the quadratic programming problem based on the path obtained from the fast traversal random tree algorithm, which makes the generated trajectory have better continuity and smoothness, reducing mutations in the UAV motion. And this also provides a good initial value for the subsequent trajectory optimization, as shown in the following formula:

[0064]

[0065] in, and are the initial state and terminal state of the trajectory, respectively. This combined method fully combines global path planning and trajectory optimization, and can quickly generate high-quality initial trajectories, providing a good initial value for subsequent manual guidance trajectory replanning. For the optimization of the initial motion trajectory, the manual guidance term in formula (1) is not considered, so the trajectory optimization problem becomes:

[0066]

[0067] For accurate tracking of the reference trajectory, in order to verify the effectiveness of the method proposed in the present invention, a third-order integral nominal model is used, and on this basis, a linear model predictive control algorithm is used for trajectory tracking. This greatly simplifies the computational complexity and avoids processing complex nonlinear terms. The states and inputs in the μ-dimensional Euclidean space are as follows:

[0068]

[0069] Among them, p μ , v μ and a μ Respectively represent the position, velocity and acceleration of the artificially guided UAV. Using the forward Euler method, the equation of the system discretization form is as follows:

[0070]

[0071] in, dt is the prediction step size. The optimization problem in model predictive control involves minimizing the position, velocity, and acceleration errors between the reference trajectory and the system state.

[0072] Step 4: Rapid replanning of the trajectory of the person in the loop, including: if the trajectory needs to be replanned under human guidance, real-time and rapid replanning based on the optimization problem designed in step 1. The specific process is as follows:

[0073] In complex environments, human-guided drones can provide guidance for other subordinate drones. This approach not only improves flexibility and efficiency, but also enhances the ability to respond to emergencies, enabling drone swarms to effectively cope with dense and dynamically changing environments.

[0074] The present invention designs a human-in-the-loop trajectory replanning framework, designs a human-computer interaction interface based on the MATLAB graphical interaction interface, and captures the intentions of human operators. Even without direct intervention by human operators, the drone cluster can rely on its advanced preset algorithms and onboard sensor data to autonomously plan its trajectory based on four key indicators: safety, dynamic feasibility, energy efficiency, and trajectory smoothness. In emergency situations (such as sudden changes in the environment), human operators can quickly intervene through the human-computer interaction interface to replan the trajectory and ensure safe and stable flight of the entire drone cluster.

[0075] First initialize the identifier Flag replan and Flag optimized If Flag replan =0, Flag optimized = 0, indicating that manual guidance is not considered, let λ1 = 0, and perform trajectory optimization based on formula (17). optimized =1. If Flag replan =1, Flag optimized = 0, indicating that manual guidance is required, and trajectory optimization is performed based on formula (1). In the entire manual guidance process, the manually guided UAV needs to accurately track the planned reference trajectory. If the operator intervenes in this process, Flag replan =1, Flag optimized =0.

[0076] For the lower-level drone, if j≠i, So

[0077]

[0078] Among them, Rank represents the level of the individual, and the control amount of the relevant subordinate drone is calculated based on step 3. Among them, the position vector and velocity vector of the subordinate drone are shown as follows:

[0079]

[0080] Among them, x i ,y i ,z i Represent the individual's horizontal coordinate, vertical coordinate and height, V i ,ψ i ,λ represents the individual height change rate. If the current simulation time t is less than the maximum simulation time t max , and the manually guided UAV does not reach the target point, t = t + dt, where dt is the simulation step size.

[0081] A method for trajectory planning and control of a swarm of drones under human guidance can quickly convert the intention of human operators into control instructions for the swarm of drones through a human-machine interaction interface, improving the efficiency of human-machine collaboration and ensuring the safety of the group. Then, considering the communication burden of the swarm, the drones in the swarm are divided into human-guided drones and subordinate drones. The human-guided drones track the planned trajectory. For the subordinate drones, the corresponding control algorithm is designed using the artificial potential field method based on the local interaction mechanism. This enables the drone swarm to safely and collaboratively traverse a dynamic and dense environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 The present invention is a flow chart for realizing the trajectory planning and control method of a swarm of UAVs under human guidance.

[0083] Figure 2 It is the position description diagram of the control points of the desired trajectory.

[0084] Figure 3 This is the result graph of fast traversal random tree path planning in an environment containing only static obstacles.

[0085] Figure 4 It is the initial planning trajectory map of the manually guided UAV.

[0086] Figure 5 It is a motion trajectory diagram of a drone cluster in an environment with unknown dynamic obstacles.

[0087] The numbers and symbols in the figure are explained as follows:

[0088] ▲——Manually guided drone

[0089] O——Lower-level drone

[0090] ★——Starting point and end point DETAILED DESCRIPTION

[0091] The effectiveness of the artificially guided UAV cluster trajectory planning and control method proposed in the present invention is verified by a specific example. The simulation environment of this example is configured with an Intel i9-12900 processor, 2.40 GHz main frequency, 32 GB memory, 64-bit Win11 operating system, and the software is MATLAB R2024b version.

[0092] A method for trajectory planning and control of drone clusters under artificial guidance, the implementation flow chart is as follows: Figure 1 As shown, this embodiment is a simulation of cooperative control of UAV obstacle avoidance formation in complex dynamic scenarios, with 600 simulation iterations and a step size of 0.05s. One manually guided UAV (level 1) and nine subordinate UAVs (levels 2-4) will traverse a dense environment. The environment includes static obstacles such as cylinders and cuboids, as well as unknown dynamic obstacles. In the simulation scenario, the entire enclosed area is a square with a side length of 100 meters. The initial position of the manually guided UAV is (1,50,0) meters, and the target position is (1,50,0) meters. The maximum simulation time is set to 30 seconds. If the distance between the manually guided UAV and the target point is less than 1m, the UAV cluster is considered to have reached the target point. The specific practical steps of this example are as follows:

[0093] Step 1: Modeling the trajectory optimization problem using human-guided information

[0094] First, a UAV swarm mission area environment containing only static obstacle information is constructed as prior map information, and relevant optimization indicators are designed. The cost function of trajectory optimization uses the finite memory Peloton-Fletcher-Goldforb-Senner algorithm for trajectory optimization. The initial value x0 of the optimization problem is the initial control point obtained by solving formula (16). The initial parameters are set as follows: λ1=1, λ2=10, λ3=0.5, λ4=10, λ5=1. The optimality tolerance of the finite memory Peloton-Fletcher-Goldforb-Senner algorithm is 10 -10 , the maximum number of iterations is 1000. The sufficient descent condition parameter of the Wolfe condition is 10 -4 , the curvature condition parameter is 0.9.

[0095] Step 2: Design a cluster control method based on local interaction mechanism

[0096] Based on the local interaction mechanism and the lower-level UAV model, the control quantity of the lower-level UAV is designed. The autopilot parameters (τ v ,τ ψ ,τ λ ,τ h ) is (1,0.75,0.3,1)s, (v max ,v min) is (5,1) m / s, n max is 10g, (λ max ,λ min ) is (5,-5) m / s. Control gain k f ,k obs ,k c ,k v_hg ,k h_hg ,k co and k vn are 10, 100,000, 100,000, 1, 1, 10 and 10 respectively. In the artificial potential field method, the expected distance r between drones is desire is 5m, and the repulsion distance between individuals is r lim is 1m, and the minimum aggregation interaction range r lim2 The obstacle sensing distance r of the lower-level UAV is 5m. sense is 3m, and the communication distance between drones is r comm is 25m.

[0097] Step 3: Generate the initial trajectory of the artificially guided drone based on the current environment

[0098] A fast traversal random tree algorithm is used to find a collision-free initial path from the three-dimensional initial position of the human-guided UAV to the target position. In the process of searching the initial path, only known static convex obstacles such as cylinders and cuboids are considered. Based on this path, the minimum acceleration trajectory is generated by equation (16). Figure 3 is the result of the collision-free path obtained by the fast traversal random tree algorithm. Then, based on the solved initial value of the control point, the initial trajectory optimization is performed according to formula (17), and the parameter setting for solving the optimization problem in step 1 is adopted. Figure 4 is the initial trajectory without considering dynamic unknown obstacles.

[0099] Step 4: Rapid replanning of the trajectory of the person in the loop

[0100] In the process of manually guiding the UAV to fly based on the trajectory obtained in step 3, due to the existence of unknown dynamic obstacles, when the human operator detects obstacles using the human-machine interaction interface, it is necessary to re-plan the trajectory in real time. start and p end Always in the X,Y plane, so the normal plane π is perpendicular to the X,Y plane. Control point Q within the manual guidance range i Expected location like Figure 2 As shown in Figure 2, the trajectory of the drone cluster is as follows: Figure 5 shown.

Claims

1. A method for trajectory planning and control of a drone cluster under artificial guidance, characterized in that: The method comprises the following steps: Step 1: Model the trajectory optimization problem using the information of manual guidance, specifically including: modeling the trajectory optimization problem of the manually guided UAV, which is expressed as an unconstrained nonlinear optimization problem, including manual guidance penalty term, collision penalty term, control input penalty term, trajectory smoothness, and dynamic feasibility; Step 2: Design a cluster control method based on a local interaction mechanism, including: designing a formation-keeping control amount based on the kinematic and dynamic models of the lower-level UAVs, an obstacle avoidance control amount, a control amount to prevent individuals from colliding with each other, a speed alignment control amount, a control amount to reach a desired height, a control amount to converge the speed of the UAV cluster, and a control amount to prevent the cluster from being overly dispersed; Step 3: Generate an initial trajectory of the artificially guided UAV based on the current environment, specifically including: using a fast traversal random tree algorithm to find a collision-free path from the starting point to the target in a static environment containing convex obstacles of cuboids and cylinders, then solving a quadratic programming problem from the obtained path to generate an initial minimum jerk trajectory, and then optimizing the initial trajectory; Step 4: Rapid replanning of the trajectory of the person in the loop, specifically including: if the trajectory needs to be replanned under human guidance, real-time and rapid replanning is performed based on the optimization problem designed in step 1.

2. The method according to claim 1, characterized in that: The step 1: using the information of manual guidance to model the trajectory optimization problem, the specific process is as follows: The planned trajectories are all represented by the third-order uniform B-spline curve s(t) parameterized by time t, where Q is the position control point, V is the velocity control point, A is the acceleration control point, J is the jerk control point, and the time of each trajectory is dt; The trajectory optimization problem of a manually guided UAV consists of five aspects and is formulated as an unconstrained nonlinear optimization problem with the following cost function: Among them, Q represents the position control point, f h is the artificial guidance penalty term, f c is the collision penalty term, f u is the control input penalty term, f s represents the trajectory smoothness, f d represents the kinetic feasibility, f total represents the total cost function, λ1,λ2,λ3,λ4,λ5 represent the weights of the corresponding optimization items respectively; The operator first clicks the mouse on the human-machine interface to select the starting position and sets the desired direction for the movement of the control point; then, drags and releases the mouse to complete the setting; by integrating human decision-making into the algorithm, the operator's choice is incorporated into the trajectory optimization process, achieving human-machine collaboration; in the human-machine interface, the position where the operator clicks the mouse is p start , the position where the operator releases the mouse is p end ; Control point Q i Affected by human operators, located at p start In a sphere with a center and a radius of r, i represents the index of the position control point Q; f h As shown below: Among them, c hg =||p start -p end ||2 indicates the strength of artificial guidance, represents the desired guiding position of the control point; Collision penalty term f c : Among them, d(Q i ) represents the control point Q i The distance to the nearest obstacle, Control input penalty term f u : Smoothness penalty f s : Dynamic feasibility penalty term f d : The dynamic feasibility penalty prevents the planned trajectory from exceeding the physical limitations of the UAV. This soft constraint is designed as a smooth function with continuous second-order derivatives; f d =f v +f a (6) in, δ=0.5,n m =4, x, y represent the abscissa and ordinate respectively, z represents the height; the above optimization problem is solved by the limited memory Peloton-Fletcher-Goldforb-Senner algorithm.

3. The method according to claim 2, characterized in that: Construct a start and the end point p end and perpendicular to line segment p start p end The plane of the starting point; the control point Q within the range of manual guidance i The projection onto plane π is expressed as 4. The method according to claim 1, characterized in that: The specific process of step 2 is as follows: The drone cluster is divided into the top-level human-guided drone and multiple lower-level drones to form a hierarchical structure; the human-guided drone is ranked first, and the lower-level drones are ranked from the second to the nth; In a UAV cluster, subordinate UAVs achieve cooperative flight through the artificial potential field method under the guidance of the artificial guide UAV; subordinate UAVs follow simple local interaction rules based on the state of their companions, including cohesion, alignment and separation; the dynamic model of subordinate UAVs in a UAV cluster is as follows: Among them, m i is the mass of each subordinate drone, set to 1kg; v i ,u i are the Euclidean space position vector, velocity vector and control input of each slave drone, respectively, i v i is the velocity damping term; let x, y represent the abscissa and ordinate respectively, z represents the height, and the control input of each subordinate drone is given by the following equation: u f is the formation-keeping control quantity that enables individuals to maintain relative distances between each other: in, k f is the control gain, represents the distance between individuals i and j, w j represents the influence weight of j on i, r comm and r d represents the communication distance in the horizontal direction and the expected distance between i and j; u obs is the obstacle avoidance control quantity: in, k obs is the control gain, represents the square of the distance between individual i and the surface of obstacle j, r sense Indicates the individual's perceived distance of the obstacle; u c Represents the control amount to prevent individuals from colliding with each other: in, k c represents the control gain, r lim is the distance that the potential field function affects individual j; alignment control amount u v_hg Align the velocity of individual i with the velocity of the human-guided drone: Among them, k v_hg is the control gain, Indicates the speed of the manually guided drone; u h_hg The control amount to guide the drone subordinate drone i to reach the desired altitude: Among them, k h_hg To control the gain, Indicates the altitude of the manually guided drone; u vn The control quantity that makes the speed of the lower-level drone converge is shown in the following formula: Among them, k vn To control the gain; Aggregate control amount u co The lower-level drones are kept at an appropriate distance from each other to prevent the cluster from being too dispersed, as shown in the following formula: in, k co represents the control gain, r lim2 Indicates the minimum interaction distance of the aggregation control component, beyond which the subordinate drones begin to aggregate with each other.

5. The method according to claim 4, characterized in that: Each lower-level UAV is equipped with a first-order Mach hold autopilot, a first-order heading hold autopilot, and a second-order altitude hold autopilot; the time constants of the three autopilots are τ v , τ λ , τ h ; In order to make the UAV more consistent with the characteristics of the actual system, define V max ,V min ,n max ,λ max ,λ min , g are respectively the maximum horizontal speed, the minimum horizontal speed, the maximum lateral overload, the maximum height change rate, the minimum height change rate, and the gravity acceleration g = 9.8m / s 2 .

6. The method according to claim 1, characterized in that: The specific process of step three is as follows: Find a collision-free path from the start point to the goal in a static environment containing convex obstacles such as cuboids and cylinders; as shown below: in, and are the initial state and the final state of the trajectory respectively. For the optimization of the initial motion trajectory, the artificial guidance term in formula (1) is not considered, so the trajectory optimization problem becomes:

7. The method according to claim 6, characterized in that: The method further includes: adopting a third-order integral nominal model, and using a linear model predictive control algorithm for trajectory tracking on this basis; this greatly simplifies the computational complexity and avoids processing complex nonlinear terms; the state and input in the μ-dimensional Euclidean space are as follows: Among them, p μ , v μ and a μ Respectively represent the position, velocity and acceleration of the artificially guided UAV; using the forward Euler method, the equation of the system discretization form is as follows: in, dt is the prediction step size; the optimization problem in model predictive control involves minimizing the position, velocity, and acceleration errors between the reference trajectory and the system state.

8. The method according to claim 1, characterized in that: The specific process of step 4 is as follows: The in-the-loop trajectory replanning framework captures the intentions of human operators by designing a human-machine interface based on the MATLAB graphical interface. Even without direct intervention from human operators, the drone swarm relies on its preset algorithms and onboard sensor data to autonomously plan its trajectory based on four key indicators: safety, dynamic feasibility, energy efficiency, and trajectory smoothness. In an emergency, the operator quickly intervenes through the human-machine interface to replan the trajectory, ensuring safe and stable flight of the entire drone swarm. First initialize the identifier Flag replan and Flag optimized ; If Flag replan =0, Flag optimized = 0, indicating that manual guidance is not considered, let λ1 = 0, and perform trajectory optimization based on formula (17); after that, let Flag optimized =1; if Flag replan =1, Flag optimized =0, indicating that manual guidance is required and trajectory optimization is performed based on formula (1); During the entire manual guidance process, the manually guided drone needs to accurately track the planned reference trajectory; If the operator intervenes during this process, Flag replan =1, Flag optimized =0; For the lower-level drone, if j≠i, So Among them, Rank represents the level of the individual, and the control amount of the relevant subordinate drone is calculated based on step 3; among them, the position vector and velocity vector of the subordinate drone are shown as follows: Among them, x i ,y i ,z i Represent the horizontal coordinate, vertical coordinate and height of the individual, V i ,ψ i ,λ represents the individual height change rate; If the current simulation duration t is less than the maximum simulation duration t max , and the manually guided UAV does not reach the target point, t = t + dt, where dt is the simulation step size.

Images