An artificial guided unmanned aerial vehicle cluster trajectory planning control method

Abstract

The application discloses a kind of artificial guidance under unmanned aerial vehicle cluster trajectory planning control method, comprising: step one: the information of artificial guidance is used to model trajectory optimization problem;Step two: design cluster control method based on local interaction mechanism;Step three: based on the initial trajectory of artificial guidance unmanned aerial vehicle generated in current environment;Step four: trajectory fast re-planning in human-in-the-loop.The method of the application can quickly convert the intention of human operator into the control instruction of unmanned aerial vehicle cluster through human-computer interaction interface, improve the efficiency of man-machine cooperation, and ensure the safety of group.Then, considering the communication burden of cluster, the unmanned aerial vehicles in the fleet are divided into artificial guidance unmanned aerial vehicles and subordinate unmanned aerial vehicles.The artificial guidance unmanned aerial vehicles track the planned trajectory.For subordinate unmanned aerial vehicles, corresponding control algorithm is designed based on local interaction mechanism.This enables unmanned aerial vehicle cluster to safely and cooperatively traverse dynamic and dense environments.

Description

TECHNICAL FIELD

[0001] The application is a kind of artificial guiding unmanned aerial vehicle cluster trajectory planning control method, which belongs to the field of unmanned aerial vehicle autonomous control. BACKGROUND

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

[0003] Currently, unmanned aerial vehicle clusters face the challenge of dynamic external environment and communication constraints between unmanned aerial vehicles. Due to the local interaction within the cluster, existing methods are difficult to ensure the coordination and optimality of cluster motion. In order to solve this problem, the application 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 bird groups (such as Harris hawk groups, pigeon groups, etc.) have obvious social hierarchical structures, and the hierarchical relationship has the phenomenon of sequential transmission. For Harris hawk groups, age plays a major role in dominating the hierarchical relationship, and older individuals have higher dominance than younger individuals. If the prey escapes in the hawk group's line of sight, the hawk group usually adopts a hierarchical attack strategy, that is, the strongest hawk always leads the other hawk individuals to capture the prey. For pigeon groups, pigeon groups can form coordinated and orderly motion patterns based on topological hierarchical relationships, and can quickly traverse complex environments through reactive obstacle avoidance. Drawing on biological swarm intelligence, individuals in the unmanned aerial vehicle cluster can be divided into different levels, which can better guide the unmanned aerial vehicle cluster to fly safely in complex environments. High-level unmanned aerial vehicles are designated as artificially guided unmanned aerial vehicles to track the planned trajectory, thereby ensuring the smoothness and optimality of group motion. Low-level unmanned aerial vehicles operate based on the designed control algorithm, enabling the unmanned aerial vehicle cluster to quickly and safely traverse dense environments. This method has advantages in improving the smoothness and coordination of group motion trajectories.

[0005] Currently, in the research of unmanned aerial vehicle systems, unmanned aerial vehicle clusters and ground stations are connected through communication equipment, forming a "platform unmanned, system manned" system. Therefore, the problem of authority allocation between operators and unmanned aerial vehicle clusters arises. For example, in the process of recovering unmanned aerial vehicles, although existing planning and control algorithms can better enable unmanned aerial vehicles to dock under ideal conditions, they still highly depend on skilled pilots for control in complex environments. Especially in complex weather conditions, such as strong winds, rain, snow, or low-visibility environments, or in space-limited areas (no-fly zones), such as urban building gaps, dense forest areas, or narrow valleys, it becomes more difficult to safely recover unmanned aerial vehicles.

[0006] In addition, the dynamic external environment makes the perception and decision of the UAV system face more severe challenges. In this case, the UAV swarm needs to intelligently adapt to the changing environment while completing the task. However, due to the limitations of the perception ability or the system itself, this may lead to inaccurate identification of the surrounding environment information, thereby affecting the perception and decision of the UAV, especially in a complex environment without human guidance.

[0007] Some reactive control methods, such as velocity obstacle and artificial potential field method, have been used for dynamic obstacle avoidance of UAVs. These methods only consider the current moving obstacles and only calculate one-step operations. Some other methods consider the constant speed of obstacles or use filtering-based methods to predict the motion trajectory of obstacles. This is difficult to achieve in complex and dynamic environments because many objects have various motion patterns and do not meet the constant speed or constant acceleration assumption, and sensors may not accurately perceive the danger. Due to these problems, the above-mentioned techniques lack flexibility and may lead to performance degradation in complex dynamic environments. Therefore, integrating the rich flight experience and decision-making ability of human operators into the UAV swarm can bring many benefits, and human operators only need to issue one command to change the state of the entire UAV swarm.

[0008] In order to ensure data transmission and communication within the UAV swarm, each UAV communicates with all other UAVs. Although this can enhance data sharing and decision consistency, it may cause excessive communication delay and system data redundancy. Swarm intelligence provides a feasible idea for distributed swarm control of UAVs and helps to reduce the communication burden. Bird flocks and UAV swarms have similarities in task requirements. In the hierarchical structure, individuals with more environmental information will be located at a higher level or have higher priority, which helps to maintain the consistency and stability of the UAV swarm motion and improve decision efficiency. Therefore, in the UAV swarm, the hierarchical design idea of using high-level UAVs as artificially guided UAVs can fully utilize the advantages of human decision-making, making the UAV swarm flexible to respond to dynamic and complex environments. SUMMARY

[0009] The present application proposes a trajectory planning control method for a UAV swarm under artificial guidance, which integrates the decision-making ability of human operators into the trajectory planning of the UAV swarm through a human-machine interaction interface, and designs a corresponding swarm control algorithm on this basis, aiming to solve the trajectory planning control and response to complex dynamic scenarios of the UAV swarm system.

[0010] The present application solves the problem of cooperative control of the UAV swarm system in complex environments by the following technical solution. The framework of the method is shown in Figure 1 The specific steps are as follows:

[0011] Step one: model the trajectory optimization problem with artificial guidance information, including: model the trajectory optimization problem of the artificial guidance unmanned aerial vehicle, expressed as an unconstrained nonlinear optimization problem, including artificial guidance penalty term, collision penalty term, control input penalty term, trajectory smoothness, dynamics feasibility. The specific process is as follows:

[0012] In the trajectory planning of unmanned aerial vehicle, B-spline can represent smooth and easy-to-optimize flight trajectory, and B-spline is uniquely determined by its degree, control point and node vector. The trajectory planned in the application is a 3-order uniform B-spline curve s(t) characterized by time t parameterization, wherein Q is a position control point, V is a velocity control point, A is an acceleration control point, and J is a jerk control point. The time of each trajectory is dt.

[0013] The trajectory optimization problem of the artificial guidance unmanned aerial vehicle includes five aspects, expressed as an unconstrained nonlinear optimization problem, and the cost function is as follows:

[0014]

[0015] Wherein, Q represents a position control point, f h is an artificial guidance penalty term, f c is a collision penalty term, f u is a control input penalty term, f s represents trajectory smoothness, f d represents dynamics feasibility, f total represents the total cost function, λ1, λ2, λ3, λ4, λ5 respectively represent the weight of the corresponding optimization term. In a complex or dynamic environment, especially in the face of emergency or unexpected situations, artificial guidance plays a crucial role in the function and safety of unmanned systems. The purpose of artificial guidance is to use human decision-making to deal with uncertainty, quickly perceive environmental changes, identify potential risks, and take appropriate measures, so that the trajectory deviates from the direction guided by the human operator.

[0016] Since the shape of the B-spline curve is only affected by the local control points, changing the position of a specific control point only affects the nearby part of the curve, not the entire curve. Human operators can simply manipulate the mouse to adjust the control points near the danger, thereby deviating the trajectory from the danger, intuitively affecting the motion of the unmanned aerial vehicle cluster. The operator first clicks the mouse on the human-machine interface to select the starting position, sets the required 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 choice is naturally incorporated into the trajectory optimization process, thereby achieving effective human-machine collaboration. In the human-machine interface, the position where the operator clicks the mouse is p start , and the position where the operator releases the mouse is p end . The 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 Inside a sphere with center r and radius r.

[0017] f h As shown in the following formula:

[0018]

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

[0020] To avoid attracting control points to the same location, it is necessary to select them appropriately. To fully utilize the role of artificial guidance. Construct a path passing through the starting point p. start and the endpoint p end and perpendicular to line segment p start p end The starting plane. Control point Q within the manually guided area. i The projection onto the plane π is represented as:

[0021] Collision penalty item f c :

[0022]

[0023] Wherein d(Q) i ) represents control point Q i Distance to the nearest obstacle

[0024]

[0025] Control input penalty item f u :

[0026]

[0027] Smoothness penalty term 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 UAV. This soft constraint is designed as a smooth function with continuous second derivatives.

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

[0031] wherein,

[0032] delta = 0.5, n m = 4, x, y represent the horizontal and vertical coordinates respectively, and z represents the height. The optimization problem is solved by using the limited memory Broyden-Fletcher-Goldfarb-Shanno algorithm.

[0033] Step two: design a cluster control method based on local interaction mechanism, including: according to the kinematics and dynamics model of the subordinate unmanned aerial vehicle, design the formation keeping control quantity which enables the individual to maintain the relative distance between each other, obstacle avoidance control quantity, collision avoidance control quantity, speed alignment control quantity, control quantity to reach the expected height, control quantity to make the cluster speed converge and control quantity to prevent the cluster from excessive dispersion. The specific process is as follows:

[0034] Compared with the subordinate unmanned aerial vehicle, the artificial guide unmanned aerial vehicle has a wider on-board communication range and stronger anti-interference ability, effectively ensuring the stability and safety of the cluster in emergency. The human operator can make quick decisions with intuitive perception and rich experience. The unmanned aerial vehicle cluster is divided into the highest layer artificial guide unmanned aerial vehicle and multiple subordinate unmanned aerial vehicles in the present application, forming a hierarchical structure. The artificial guide unmanned aerial vehicle ranks first, while the subordinate unmanned aerial vehicles rank from the second to the nth.

[0035] In the unmanned aerial vehicle cluster, the subordinate unmanned aerial vehicles realize cooperative flight through the artificial potential field method under the guidance of the artificial guide unmanned aerial vehicle. This helps to reduce the communication burden of controlling the unmanned aerial vehicle cluster. The subordinate unmanned aerial vehicles follow simple local interaction rules based on the state of the peers, including cohesion, alignment and separation. The dynamics model of the subordinate unmanned aerial vehicles in the unmanned aerial vehicle cluster is as follows:

[0036]

[0037] wherein, m i is the mass of each subordinate unmanned aerial vehicle, which is set to 1 kg in the present application. v i , u i are the Euclidean space position vector, velocity vector and control input of each subordinate unmanned aerial vehicle respectively, -k i v i is the velocity damping term. Let x, y represent the horizontal and vertical coordinates respectively, and z represents the height. The control input of each subordinate unmanned aerial vehicle is given by the following equation:

[0038]

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

[0040]

[0041] wherein, k f is a control gain, represents a distance between individuals i, j, w j represents a j-to-i influence weight, r comm and r d represent a horizontal direction communication distance and a desired distance between i and j.

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

[0043]

[0044] wherein, k obs is a control gain, represents a square of a distance between individual i and a surface of an obstacle j, r sense represents a perception distance of the individual to the obstacle.

[0045] u c represents a collision avoidance control quantity that prevents individuals from colliding with each other:

[0046]

[0047] wherein, k c represents a control gain, r lim is a distance at which a potential field function has an influence on individual j.

[0048] an alignment control quantity u v_hg aligns a speed of individual i with a speed of the artificial guide drone:

[0049]

[0050] wherein, k v_hg is a control gain, represents a speed of the artificial guide drone.

[0051] u h_hg is a control quantity that guides a subordinate drone i of the guide drone to a desired altitude:

[0052]

[0053] wherein, k h_hg is a control gain, represents an altitude of the artificial guide drone.

[0054] u vn The control amount that makes the speed of the subordinate unmanned aerial vehicle converge is shown in the following formula:

[0055]

[0056] wherein k vn is a control gain.

[0057] The aggregation control amount u co The subordinate unmanned aerial vehicles maintain a proper distance to prevent the swarm from being excessively dispersed, as shown in the following formula:

[0058]

[0059] wherein, k co represents a control gain, r lim2 represents the minimum mutual distance of the aggregation control component, and beyond this distance, the subordinate unmanned aerial vehicles begin to aggregate with each other.

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

[0061] Step three: generating an initial trajectory of the artificial guide unmanned aerial vehicle based on the current environment, including: using a rapid 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 a quadratic programming problem from the obtained path to generate an initial minimum jerk trajectory, and then optimizing the initial trajectory. The specific process is as follows:

[0062] The application uses a path planning method of Rapidly-exploring Random Trees* sampling. The key advantage of the Rapidly-exploring Random Trees algorithm is that it can efficiently process high-dimensional, complex unstructured environments, and through random sampling and rewiring strategies, not only can a connected path be found, but the quality of the path can also be gradually optimized. At the same time, kinematic constraints can be naturally integrated, and the adaptability and robustness are stronger, and it is particularly suitable for the scene that a UAV cluster needs to plan a path in a complex dynamic environment. The search-based algorithm is limited by grid division and computational complexity, and performs poorly in unstructured environments.

[0063] The application 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 Rapidly-exploring Random Trees algorithm provides a rough but connected path, and then generates an initial minimum jerk trajectory by solving a quadratic programming problem based on the path obtained from the Rapidly-exploring Random Trees algorithm, which makes the generated trajectory have better continuity and smoothness, and reduces the mutation in the motion of the UAV. And this also provides a good initial value for subsequent trajectory optimization, as shown in the following formula:

[0064]

[0065] wherein, and are the initial state and the terminal state of the trajectory respectively. This combination method fully combines global path planning and trajectory optimization, and can quickly generate an initial trajectory of high quality, providing a good initial value for subsequent artificial guide trajectory re-planning. For the optimization of the initial motion trajectory, the artificial guide 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 application, a third-order integral nominal model is used, and a linear model predictive control algorithm is used for trajectory tracking on this basis. This greatly simplifies the computational complexity and avoids dealing with complex nonlinear terms. The state and input in a μ-dimensional Euclidean space are as follows:

[0068]

[0069] wherein, p μ , v μ and a μ represent the position, velocity and acceleration of the artificial guide UAV respectively. The equation of the discrete form of the system is as follows by using the forward Euler method:

[0070]

[0071] where, 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: Fast trajectory re-planning in human-in-the-loop, including: if the trajectory needs to be re-planned under human guidance, real-time and fast re-planning is performed based on the optimization problem designed in step 1, the specific process is as follows:

[0073] In complex environments, the human-guided UAV can provide guidance for other subordinate UAVs. This method not only improves flexibility and efficiency, but also enhances the ability to respond to emergencies, enabling the UAV swarm to effectively deal with dense and dynamic environments.

[0074] The present application designs a trajectory re-planning framework in human-in-the-loop, designs a human-computer interaction interface based on MATLAB graphical interface, and captures the intention of human operators. Even without direct intervention of human operators, the UAV swarm can rely on its advanced preset algorithm and on-board sensor data to autonomously plan its trajectory according to 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 re-plan the trajectory, ensuring the safe and stable flight of the entire UAV swarm.

[0075] First, initialize the identifier Flag replan and Flag optimized . If Flag replan = 0, Flag optimized = 0, indicating that human guidance is not considered, set λ1 = 0, and perform trajectory optimization based on formula (17). After that, set Flag optimized = 1. If Flag replan = 1, Flag optimized = 0, indicating that human guidance is needed, perform trajectory optimization based on formula (1). During the entire human guidance process, the human-guided UAV needs to accurately track the planned reference trajectory. If the operator intervenes during this process, set Flag replan = 1, Flag optimized = 0.

[0076] For subordinate UAVs, if j ≠ i, then

[0077]

[0078] Wherein, Rank represents the rank of the individual, and the control amount of the relevant subordinate unmanned aerial vehicle is calculated based on step three. Wherein, the position vector and the velocity vector of the subordinate unmanned aerial vehicle are as shown in the following formula:

[0079]

[0080] Wherein, x i ,y i ,z i respectively represent the horizontal coordinate, the vertical coordinate and the height of the individual, V i ,ψ i ,λ represent the height change rate of the individual. If the current simulation time t is less than the maximum simulation time t max , and the artificially guided unmanned aerial vehicle has not reached the target point, t=t+dt, wherein dt is the simulation step length.

[0081] A trajectory planning and control method for unmanned aerial vehicle cluster under artificial guidance can quickly convert the intention of human operators into control instructions of unmanned aerial vehicle cluster through human-computer interaction interface, improve the efficiency of human-computer cooperation, and ensure the safety of the group. Then, considering the communication burden of the cluster, the unmanned aerial vehicles in the fleet are divided into artificially guided unmanned aerial vehicles and subordinate unmanned aerial vehicles. The artificially guided unmanned aerial vehicles track the planned trajectory. For the subordinate unmanned aerial vehicles, based on the local interaction mechanism, an artificial potential field method is used to design a corresponding control algorithm. This makes the unmanned aerial vehicle cluster able to safely and cooperatively traverse dynamic and dense environments. BRIEF DESCRIPTION OF DRAWINGS

[0082] Figure 1 A trajectory planning and control method for unmanned aerial vehicle cluster under artificial guidance is a flow chart of implementation.

[0083] Figure 2 A position description diagram of the expected trajectory control point.

[0084] Figure 3 A fast-exploring random tree path planning result diagram in an environment containing only static obstacles.

[0085] Figure 4 An initial planning trajectory diagram of the artificially guided unmanned aerial vehicle.

[0086] Figure 5 A motion trajectory diagram of the unmanned aerial vehicle cluster in an environment containing unknown dynamic obstacles

[0087] The labels and symbols in the diagram are as follows:

[0088] ▲-Artificially guided unmanned aerial vehicle

[0089] O-Subordinate unmanned aerial vehicle

[0090] ★-Starting point and ending point DETAILED DESCRIPTION

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

[0092] An artificial-guided UAV cluster trajectory planning and control method, the implementation flowchart of which is shown in Figure 1 The present embodiment is directed to UAV obstacle avoidance formation cooperative control simulation in a complex dynamic scenario. The simulation iteration number is 600, and the step size is 0.05 s. One artificial-guided UAV (level 1) and nine subordinate UAVs (levels 2-4) will pass through 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 artificial-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 artificial-guided UAV and the target point is less than 1 m, the UAV cluster is considered to have reached the target point. The specific practice steps of the example are as follows:

[0093] Step one: model the trajectory optimization problem using artificial guidance information

[0094] First, construct a UAV cluster task area environment containing only static obstacle information as prior map information, and design related optimization indicators. The cost function of trajectory optimization is optimized using the limited memory BFGS algorithm. The initial value x0 of the optimization problem is the initial control point obtained by solving formula (16). Set the initial parameters: λ1 = 1, λ2 = 10, λ3 = 0.5, λ4 = 10, λ5 = 1. The optimality tolerance of the limited memory BFGS algorithm is 10 -10 , and the maximum iteration number is 1000. The sufficient decrease condition parameter of the Wolfe condition is 10 -4 , and the curvature condition parameter is 0.9.

[0095] Step two: design a cluster control method based on local interaction mechanism

[0096] Based on the local interaction mechanism and the subordinate UAV model, the control amount of the subordinate UAV is designed. The autopilot parameters (τ v , τ ψ , τ λ , τ h ) are (1, 0.75, 0.3, 1) s, and (v max , v min) is (5, 1) m / s, n max is 10 g, (λ max , λ min ) is (5, -5) m / s. Control gains 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 desired distance r desire between UAVs is 5 m, the repulsion distance r lim between individuals is 1 m, and the minimum aggregation interaction range r lim2 is 5 m. The obstacle perception distance r sense of the lower-level UAV is 3 m, and the communication distance r comm between UAVs is 25 m.

[0097] Step three: generating the initial trajectory of the artificial guide UAV based on the current environment

[0098] The rapid-exploring random tree algorithm is used to find a collision-free initial path from the three-dimensional initial position of the artificial guide UAV to the target position. In the process of searching for the initial path, only known static convex obstacles such as cylinders and cuboids are considered. Based on the path, the minimum jerk trajectory is generated by formula (16). Figure 3 is the result of the collision-free path obtained by the rapid-exploring 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 settings for solving the optimization problem in step one are adopted, Figure 4 is the initial trajectory without considering dynamic unknown obstacles.

[0099] Step four: trajectory rapid replanning in the human-in-the-loop

[0100] In the process of the artificial guide UAV flying based on the trajectory obtained in step three, due to the existence of unknown dynamic obstacles, when the human operator detects the obstacles using the human-computer interaction interface, it is necessary to replan the trajectory in real time. In this process, the invention makes the points p start and p end always located in the X, Y plane, so that the normal plane π is perpendicular to the X, Y plane. The desired position of the control point Q i within the artificial guide range is shown in Figure 2 . The trajectory diagram of the UAV swarm is shown in Figure 5 .

Claims

1. A method for trajectory planning and control of unmanned aerial vehicle (UAV) swarms under manual guidance, characterized in that: The method includes the following steps: Step 1: Model the trajectory optimization problem using information from manual guidance. Specifically, this includes modeling the trajectory optimization problem of a manually guided UAV as an unconstrained nonlinear optimization problem, which includes manual guidance penalty term, collision penalty term, control input penalty term, trajectory smoothness, and dynamic feasibility. Step 2: Design a swarm control method based on local interaction mechanism, specifically including: designing formation keeping control variables, obstacle avoidance control variables, collision prevention control variables, speed alignment control variables, speed reaching control variables, speed convergence control variables, speed convergence control variables, and speed prevention control variables based on the kinematic and dynamic models of the lower-level UAVs. Step 3: Generate the initial trajectory of the manually guided drone based on the current environment. Specifically, this includes: using the fast 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 a quadratic programming problem from the obtained path to generate the initial minimum jerk trajectory; and finally optimizing the initial trajectory. Step 4: Rapid replanning of human trajectory in the loop, specifically 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; In step one, the trajectory optimization problem is modeled using manually guided information. The specific process is as follows: The planned trajectories are all represented by time-t parameterized third-order uniform B-spline curves s(t), where Q is the position control point, V is the velocity control point, A is the acceleration control point, and J is the jerk control point. The time of each trajectory segment is dt. The trajectory optimization problem for manually guided drones comprises five aspects and is formulated as an unconstrained nonlinear optimization problem, with the following cost function: Where Q represents the position control point, f h It is a manually guided penalty item, f c It is a collision penalty item, f u It is the input penalty term, f s f represents the trajectory smoothness. d Indicates the dynamic feasibility, f total Let λ1, λ2, λ3, λ4, and λ5 represent the weights of the corresponding optimization terms. 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, they drag and release 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. On the human-machine interface, the position where the operator clicks the mouse is p. start The operator releases the mouse at position p. end Control point Q i Affected by human operators, located at p start Within a sphere centered at r and with radius r, i represents the index of the position control point Q; f h As shown in the following formula: Among them, c hg =||p start -p end ||2 indicates the intensity of artificial guidance. Indicates the desired guide position of the control point; Collision penalty item f c : Where d(Q) i ) represents control point Q i Distance to the nearest obstacle Control input penalty item f u : Smoothness penalty term 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 derivatives. f d =f v +f a (6) in, x and y represent the horizontal and vertical coordinates, respectively, and z represents the height; the optimization problem is solved using the finite-memory Platoon-Fletcher-Goldforb-Senner algorithm. The specific process of step two is as follows: The drone swarm is divided into a top-level manually guided drone and multiple lower-level drones, forming a hierarchical structure; the manually guided drone is ranked first, while the lower-level drones are ranked from second to nth. In a drone swarm, lower-level drones, guided by an artificially guided drone, achieve cooperative flight through an artificial potential field method. The lower-level drones follow simple local interaction rules based on companion states, including cohesion, alignment, and separation. The dynamic model of the lower-level drones in the swarm is as follows: Where, m i This refers to the mass of each subordinate drone, set to 1 kg. v i u i These are the Euclidean space position vector, velocity vector, and control input for each subordinate UAV, -k i v i This is the velocity damping term; let x and y represent the horizontal and vertical coordinates respectively, and z represent the altitude. The control input for each lower-level UAV is given by the following equation: u f It is the formation maintenance control quantity that enables individuals to maintain relative distance from each other: in, k f It is about controlling the gain. w represents the distance between individuals i and j. j This indicates the weight of the influence of j on i, r comm and r d This represents the horizontal communication distance and the expected distance between i and j; u obs It is the obstacle avoidance control quantity: in, k obs It is about controlling the gain. r represents the square of the distance between individual i and the surface of obstacle j. sense This indicates the perceived distance of an individual to an obstacle; u c Indicates the control quantity to prevent individual collisions: in, k c Represents the control gain, r lim It is the distance at which the potential field function affects individual j; the alignment control quantity u v_hg Align the speed of individual i with the speed of the manually guided drone: Where, k v_hg It is about controlling the gain. This indicates the speed at which the drone is manually guided; u h_hg Control parameters for guiding the subordinate drone i to reach the desired altitude: Where, k h_hg To control the gain, Indicates the altitude at which the drone is manually guided; u vn The control variable that causes the speed of the lower-level UAV to converge is shown in the following formula: Where, k vn To control the gain; Aggregation control quantity u co Lower-level drones should maintain appropriate spacing to prevent the cluster from becoming overly dispersed, as shown in the following formula: in, k co Represents the control gain, r lim2 This represents the minimum interaction distance for the aggregation control components. Beyond this distance, lower-level drones begin to aggregate. The specific process of step four is as follows: The trajectory replanning framework in the loop is designed based on the MATLAB graphical interface to capture the intentions of the human operator. Even without direct intervention from the human operator, the drone swarm relies on its preset algorithm and onboard sensor data to autonomously plan its trajectory according to four key indicators: safety, dynamic feasibility, energy efficiency, and trajectory smoothness. In emergency situations, the operator can quickly intervene through the human-machine interface to replan the trajectory and ensure the 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 indicates that manual guidance was not considered. After this, let Flag... optimized =1; if Flag replan =1, Flag optimized =0 indicates that manual guidance is required, and trajectory optimization is performed based on formula (1); throughout the entire manual guidance process, the manually guided drone needs to accurately track the planned reference trajectory; if the operator intervenes during this process, the Flag will be set to 0. replan =1, Flag optimized =0; For lower-level drones, if So Where Rank represents the hierarchy of an individual, and the control variables of the relevant lower-level UAVs are calculated based on step three; the position vector and velocity vector of the lower-level UAVs are shown in the following formula: Where, x i ,y i ,z i V represents the x-coordinate, y-coordinate, and height of an individual, respectively. i ,ψ i λ represents the rate of change of an individual's height; if the current simulation duration t is less than the maximum simulation duration t... max Furthermore, the manually guided drone did not reach the target point, t = t + dt, where dt is the simulation step size.

2. The method according to claim 1, characterized in that: Construct a path through the starting point p start and the endpoint p end and perpendicular to line segment p start p end The starting plane; control point Q within the manually guided area. i The projection onto the plane π is represented as:

3. The method according to claim 1, characterized in that: Each subordinate drone is equipped with a first-order Mach-keeping autopilot, a first-order heading-keeping autopilot, and a second-order altitude-keeping autopilot; the time constants of the three autopilots are τ v , τ λ , τ h To make drones more consistent with the characteristics of real-world systems, V is defined. max V min ,n max ,λ max ,λ min Let g represent the maximum horizontal velocity, minimum horizontal velocity, maximum lateral overload, maximum rate of change of height, and minimum rate of change of height, respectively. The acceleration due to gravity g = 9.8 m / s². 2 .

4. 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 starting point to the target in a static environment containing convex obstacles such as cuboids and cylinders; as shown in the following equation: in, and These are the initial and final states of the trajectory, respectively. For the optimization of the initial trajectory, the artificial guidance term in formula (1) is not considered, so the trajectory optimization problem becomes:

5. The method according to claim 4, characterized in that: The method further includes: employing a third-order integral nominal model, and then using a linear model predictive control algorithm for trajectory tracking; this greatly simplifies computational complexity and avoids dealing with complex nonlinear terms; the state and input in μ-dimensional Euclidean space are as follows: Where, p μ v μ and a μ Let represent the position, velocity, and acceleration of the manually guided drone, respectively; using the forward Euler method, the discrete-form equations of the system are 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.

Images