Unmanned aerial vehicle cluster control design and simulation method and system based on consistency algorithm

Abstract

The invention discloses an unmanned aerial vehicle cluster control design and simulation method and system based on a consistency algorithm, and relates to the technical field of unmanned aerial vehicle cluster control, and the method comprises the steps: carrying out the global path planning according to a starting point and an end point of an unmanned aerial vehicle cluster in a current flight environment, and taking the obstacle position as a target, obtaining a plurality of global path points of each unmanned aerial vehicle; based on the global path points and a preset communication topological graph of the unmanned aerial vehicle cluster, performing cooperative control on the unmanned aerial vehicle cluster by adopting a consistent formation control algorithm, and constructing a global controller of the unmanned aerial vehicle cluster; and performing local path planning and obstacle avoidance on the global controller by constructing a potential function of the unmanned aerial vehicle, thereby realizing formation control of the unmanned aerial vehicle cluster. Efficient cooperative control of the unmanned aerial vehicle cluster is achieved through global path planning and consistent formation control, obstacles of the unmanned aerial vehicles in the moving process are effectively avoided by calculating the potential function, and the control flexibility of the unmanned aerial vehicle formation is improved.

Description

Technical Field

[0001] This application relates to the technical field of UAV swarm control, and specifically relates to a design and simulation method and system for UAV swarm control based on a consensus algorithm. Background Art

[0002] UAV swarm control technology refers to the realization of collaborative, coordinated, and efficient task execution among multiple UAVs through advanced communication, navigation, and control algorithms. With the rapid development of UAV technology, swarm flight components have become a hot topic. Especially in multiple industries such as military, agriculture, power inspection, environmental monitoring, and logistics distribution, the application scenarios of UAV swarms are continuously enriched, demonstrating great potential and value.

[0003] Current UAV swarm control technology mainly relies on traditional path planning algorithms and basic formation control algorithms (such as centralized control, distributed control, and hybrid control, etc.), all of which use a single algorithm to handle global path planning or local obstacle avoidance, and perceive the surrounding environment and the states of other UAVs through communication and sensor technologies between UAVs to avoid collision events. However, in a complex environment with frequent sudden obstacles, the existing technology shows poor collaborative optimization ability and is difficult to achieve timely obstacle avoidance. Moreover, the basic formation control algorithm faces challenges in aspects such as communication delay and data synchronization in complex situations, and it is difficult to control the overall accuracy of the UAV swarm. Summary of the Invention

[0004] This application proposes a design and simulation method and system for UAV swarm control based on a consensus algorithm, which realizes the efficient collaborative control of UAV swarms through global path planning and consensus formation control, and effectively avoids obstacles during the movement of UAVs by calculating the potential function, improving the control flexibility of UAV formations.

[0005] The first aspect of this application provides a design and simulation method for UAV swarm control based on a consensus algorithm, and the method includes:

[0006] According to the starting point and ending point of the UAV swarm in the current flight environment, perform global path planning with the goal of avoiding obstacle positions to obtain several global path points for each UAV;

[0007] Based on the global path points and the preset communication topology map of the UAV swarm, use the consensus formation control algorithm to perform collaborative control on the UAV swarm and construct a global controller for the UAV swarm;

[0008] Construct the potential function of the UAV to perform local path planning and obstacle avoidance for the global controller to achieve the formation control of the UAV swarm.

[0009] Based on the current known positions of the UAVs and their corresponding end points, a global path planning is performed for each UAV with the aim of avoiding obstacle positions, obtaining the global path points that the UAVs pass through from the starting point to the end point. Then, a communication topology graph of the UAV cluster is constructed based on the communication situation among the UAVs, and a global controller is constructed based on the communication relationship among the UAVs to coordinately control the formation switching of the UAV cluster, reducing the problems of communication delay and data asynchronization, improving the real-time performance of UAV cluster control, and also enhancing the flexibility of formation control through the global controller. Finally, the potential functions between UAVs, the end point, and obstacles are calculated to measure the gravitational and repulsive forces between these nodes, enabling the UAVs to effectively avoid obstacles based on the planned global path in a complex environment.

[0010] In a possible implementation method of the first aspect, based on the starting point and the end point of the UAV cluster in the current flight environment, a global path planning is carried out with the aim of avoiding obstacle positions, obtaining several global path points for each UAV, specifically:

[0011] Taking the current position of the UAV as the starting point, determine the global planning range through the starting point and the end point;

[0012] Based on the current position of the UAV, search for nodes within the global planning range with the aim of avoiding obstacle positions to find the forced neighbors of each node within the global planning range; where the forced neighbor is a node that forces the algorithm to change the moving direction during global path planning;

[0013] According to the forced neighbors, the end point and the starting point of the UAV, find the nodes belonging to the jump points within the global planning range;

[0014] Based on the jump points and the obstacle positions, perform global path planning starting from the current position of the UAV to obtain several global path points for each UAV.

[0015] The above solution first plans the global planning range of the UAVs based on the starting point and the end point of the UAVs, providing data support for the path planning of the UAVs. Then, the moving direction during the movement of the UAVs towards the end point is determined by finding the forced neighbors of each node within the global planning range. Then, global path planning is performed through the jump points and the obstacle positions to quickly find the global path of the UAVs to the end point.

[0016] In a possible implementation method of the first aspect, based on the current position of the UAV, search for nodes within the global planning range with the aim of avoiding obstacle positions to find the forced neighbors of each node within the global planning range, specifically:

[0017] Starting from the current position of the drone, with the goal of reaching the end point, determine the search status of each node within the global planning range through a preset node judgment rule; wherein, the search status includes unobstructed horizontal movement status, unobstructed diagonal movement status, obstructed horizontal movement status, and obstructed diagonal movement status;

[0018] Perform path selection based on the search status and the obstacle positions to determine the forced neighbors of each node.

[0019] In a possible implementation method of the first aspect, performing path selection based on the search status and the obstacle positions to determine the forced neighbors of each node is specifically as follows:

[0020] Based on the search status, combine the obstacle positions to perform path selection from a node to its corresponding adjacent node, and determine the path trajectory between each node and its corresponding adjacent node;

[0021] According to the search value of the nodes in the path trajectory, divide the adjacent nodes into natural neighbors or forced neighbors; wherein, the search value is used to measure whether a certain node has search significance when performing path selection.

[0022] The above solution first determines the search status of each node, and then determines whether the adjacent nodes around the node have search significance according to the search status. The adjacent nodes without search significance will not be considered as nodes to be considered during path planning. Therefore, the retrieval efficiency can be improved by excluding nodes without search value.

[0023] In a possible implementation method of the first aspect, find the nodes belonging to the jump points from within the global planning range according to the forced neighbors, the end point, and the start point of the drone, specifically as follows:

[0024] If a certain node within the global planning range is the start point or the end point, determine that the node is a jump point;

[0025] If a certain node within the global planning range has the forced neighbors, determine that the node is a jump point;

[0026] If the search status of a certain node within the global planning range is unobstructed diagonal movement status or obstructed diagonal movement status, and the node can reach a certain jump point through horizontal or vertical movement, determine that the node is a jump point.

[0027] The above solution determines which points need to change the movement direction during the path planning process by judging the jump points, so as to achieve accurate path planning.

[0028] In a possible implementation method of the first aspect, based on the global path points and the preset communication topology map of the UAV cluster, a consensus formation control algorithm is used to cooperatively control the UAV cluster, and a global controller of the UAV cluster is constructed. Specifically:

[0029] According to the global path points, a motion model of each UAV is constructed by the current position of the UAV and the preset next moment position of the UAV.

[0030] According to the communication relationship between UAVs, a communication topology map of the UAV cluster is constructed.

[0031] Through the communication topology map of the UAV cluster and the motion model, considering the formation switching of the UAV cluster, a global controller of the UAV cluster is constructed.

[0032] The above solution takes into account that the behaviors and interactions between UAVs affect the entire UAV cluster. Therefore, a communication topology map of the UAV cluster is constructed according to the communication relationship between UAVs to represent whether there is a connection between UAVs. Then, considering the formation switching of the UAV cluster, a global controller of the UAV cluster is constructed to record the states of each UAV in the UAV cluster. Through the global controller, the formation of the UAV can be flexibly switched, greatly improving the flexibility of formation control.

[0033] In a possible implementation method of the first aspect, the global controller is specifically:

[0034]

[0035] In the formula, \(x(t)\) is the current position of the UAV at time \(t\), is the flight speed of the UAV obtained by taking the derivative of \(x(t)\), \(D\) is the preset degree matrix, \(A\) is the adjacency matrix corresponding to the communication topology map of the UAV cluster, and \(L\) is the Laplacian matrix.

[0036] In a possible implementation method of the first aspect, a potential function of the UAV is constructed to perform local path planning and obstacle avoidance for the global controller. Specifically:

[0037] According to the starting point and the ending point of the UAV cluster in the current flight environment, and the positions of obstacles, a potential function of each UAV is constructed by the artificial potential field method; wherein, the potential function includes a gravitational potential function and a repulsive potential function.

[0038] Based on the repulsive potential function between the drones, the repulsive potential function between the drones and the obstacles, and the attractive potential function between the drones and the corresponding end points, perform local path planning and obstacle avoidance for the global controller to obtain the moving direction and moving speed of the drone swarm at the next moment;

[0039] Based on the moving direction and moving speed of the drone swarm at the next moment, complete the formation control of the drone swarm.

[0040] The above solution constructs potential functions between drones, obstacles, and end points respectively through the artificial potential field method to measure the repulsive and attractive forces between the drones and each node. Through the combined action of the repulsive and attractive forces, obstacle avoidance planning is carried out during the movement of the drones along the global path, enabling them to effectively avoid obstacles based on the planned global path in a complex environment.

[0041] In a possible implementation method of the first aspect, the repulsive potential function and the attractive potential function are specifically:

[0042] The repulsive potential function between the drones, the specific expression is:

[0043]

[0044] In the formula, U rep (p) is the repulsive potential function between the drones at node p, q i and q j are the positions of the i-th and j-th drones respectively, K r is the repulsive coefficient, r o is the repulsive range parameter;

[0045] The repulsive potential function between the drones and the obstacles, the specific expression is:

[0046]

[0047] In the formula, U rep (p) is the repulsive potential function between the drones and the obstacles at node p, q i is the drone position, q k is the position of the k-th obstacle, K ro is the repulsive coefficient, r o is the repulsive range parameter;

[0048] The attractive potential function between the drones and the corresponding end points, the specific expression is:

[0049]

[0050] In the formula, U att(q) is the gravitational potential function between the drone at node q and the corresponding said end point, q i is the drone position, q target is the position of the said end point, K a is the gravitational coefficient.

[0051] The second aspect of this application provides a drone swarm control design and simulation system based on a consensus algorithm. The system includes: a global path planning module, a global controller generation module, and a local obstacle avoidance module;

[0052] Among them, the global path planning module is used to perform global path planning based on the starting point and end point of the drone swarm in the current flight environment, with the goal of avoiding obstacle positions, to obtain several global path points for each drone;

[0053] The global controller generation module is used to perform cooperative control on the drone swarm based on the global path points and the preset communication topology graph of the drone swarm, and adopt a consensus formation control algorithm to construct a global controller for the drone swarm;

[0054] The local obstacle avoidance module is used to perform local path planning and obstacle avoidance on the global controller by constructing the potential function of the drone, so as to achieve the formation control of the drone swarm. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solutions of this application, the following will briefly introduce the drawings required for the implementation. Obviously, the drawings in the following description are only some implementations of this application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0056] Figure 1 is a specific process schematic diagram of a drone swarm control design and simulation method based on a consensus algorithm provided by a certain embodiment of this application;

[0057] Figure 2 is a search state display diagram of a drone swarm control design and simulation method based on a consensus algorithm provided by a certain embodiment of this application;

[0058] Figure 3 is the communication topology graph of the drone swarm of a drone swarm control design and simulation method based on a consensus algorithm provided by a certain embodiment of this application;

[0059] Figure 4 is the drone swarm path graph of a drone swarm control design and simulation method based on a consensus algorithm provided by a certain embodiment of this application;

[0060] Figure 5 It is the specific structure diagram of an unmanned aerial vehicle (UAV) cluster control design and simulation system based on a consensus algorithm provided by an embodiment of the present application. Specific Embodiment

[0061] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present application without creative efforts shall fall within the protection scope of the present application.

[0062] It should be understood that the step numbers used in the text are only for convenience of description and are not used to limit the order of execution of the steps.

[0063] First Embodiment

[0064] The existing UAV cluster control method is to select a leader in the UAV formation, and other UAVs follow its movement trajectory and fly at a certain relative distance. This method is simple to implement and has high formation accuracy, but it has a high dependence on the leader, and the performance of the leader directly affects the performance of the entire formation. Therefore, how to control the overall UAV cluster, clarify the state characteristics of each UAV, and improve the overall control of the UAV cluster is the main research direction of the embodiments of the present application. Moreover, the real-time performance and accuracy of path planning and obstacle avoidance of UAV clusters in the prior art still need to be improved in complex environments, and the ability to respond to sudden obstacles is insufficient.

[0065] As Figure 1 shown, Figure 1 This is the specific flowchart of a UAV cluster control design and simulation method based on a consensus algorithm provided by an embodiment of the present application. The UAV cluster control design and simulation method based on the consensus algorithm in this embodiment includes steps S1 to S3, which are described in detail as follows:

[0066] Step S1, based on the starting point and the ending point of the UAV cluster in the current flight environment, perform global path planning with the goal of avoiding the positions of obstacles, and obtain several global path points for each UAV.

[0067] In the embodiments of the present application, the JPS jump point algorithm is used to perform global path planning for the flight of the UAV cluster. First, the corresponding ending point is cut within 100 m of the current position of the UAV to determine the global planning range, and then several global path points that can avoid obstacles and reach the ending point are calculated through the starting point, the ending point, and the positions of the obstacles.

[0068] Among them, the JPS jump point algorithm is a graph-based search algorithm, which is an improvement on the basis of the A* algorithm. It retains the framework of the A* algorithm, and the cost function can still be expressed as f(n) = g(n) + h(n), but it optimizes the operation of finding successor nodes in the A* algorithm. Different from the strategy of the A* algorithm to traverse all reachable neighbor nodes of the current node, the JPS jump point algorithm pre-selects the jump points with computational value logically as the child nodes of the current node through the jump point search strategy, reducing the cost evaluation operation of intermediate nodes and further improving the algorithm efficiency. The implementation of the jump point search strategy is mainly based on the neighbor node pruning rule and the jump rule.

[0069] The neighbor node pruning rule determines the natural neighbors and forced neighbors with search value of a node according to the search state of the nodes within the global planning range, which can greatly improve the path planning efficiency. First, starting from the current position of the UAV and aiming to reach the end point, the search state of each node within the global planning range is determined through a preset node judgment rule. The node judgment rule defines four search states, including unobstructed horizontal movement state, unobstructed diagonal movement state, obstructed horizontal movement state, and obstructed diagonal movement state. If there are obstacles within the global planning range, it is considered obstructed, specifically as Figure 2 shown. In Figure (a), the movement from point p(x) to point x is unobstructed and can be reached through horizontal movement, so it shows the unobstructed horizontal movement state of point x; in Figure (b), the movement from point p(x) to point x is diagonal movement and there are no obstacles in the figure, so it shows the unobstructed diagonal movement state of point x; in Figure (c), the movement from point p(x) to point x is horizontal movement and there are black nodes representing the positions of obstacles in the figure, so it shows the obstructed horizontal movement state of point x; in Figure (d), the movement from point p(x) to point x is diagonal movement and there are black nodes representing the positions of obstacles around it, so it shows the obstructed diagonal movement state of point x.

[0070] Then, based on the search state, combined with the obstacle positions, path selection is performed from the node to the corresponding adjacent node to determine the path trajectory between each node and the corresponding adjacent node. When determining the path trajectory, the search value of the nodes within the global planning range is also judged, and the adjacent nodes of the nodes are divided into natural neighbors or forced neighbors according to the search value.

[0071] Among them, the natural neighbors are the adjacent nodes that can be directly reached from the current node within the global planning range, that is, the nodes adjacent to the current node horizontally, vertically, and diagonally; the forced neighbors are the nodes that force the change of the moving direction during the path planning process; in addition, pruning neighbors with no search value will also be divided in the natural neighbors.

[0072] Furthermore, by dividing natural neighbors and forced neighbors with search value and pruning neighbors without search value, it can assist the JPS jump point algorithm in global path planning not to consider the pruning neighbors, saving the calculation of these worthless nodes and greatly improving the path planning efficiency.

[0073] In addition, Figure 2 it also shows the process of path selection according to the search state and obstacle position and determining the forced neighbors of each node. Figure 2 In (a) and (b), it shows that the node x is in the obstacle-free horizontal movement state and the obstacle-free diagonal movement state respectively, where the node x is the current node, the node p(x) is the parent node of the node x, and the gray nodes are the expanded nodes; if the expanded node can reach the parent node through a shorter or equal path without passing through the current node, then the expanded node does not need to reach through the current node, that is, the red path passing through the current node x to reach the expanded node in Figures (a) and (b) can be replaced by the blue path without passing through the current node x. At this time, the expanded node can be determined as the pruning neighbor of the current node, indicating that the expanded node has no search value.

[0074] Figure 2 In (c) and (d), it shows that the node x is in the obstacle horizontal movement state and the obstacle diagonal movement state respectively, where the black nodes represent obstacles here and the blue nodes represent forced neighbors. If the shortest path from the node p(x) to the node m must pass through the node x, then the node m is called the forced neighbor of the node x.

[0075] According to the above neighbor node pruning rules, the forced neighbors and natural neighbors starting from the current position of the drone within the global planning range can be found. Then, several jump points are found from within the global planning range according to the jump rules to assist the JPS jump point algorithm in global path planning.

[0076] The jump rule stipulates how to find jump points from all nodes within the global path planning. Specifically: for any node n, if the node n is a location or an end point, then the node n is a jump point; if the node n has forced neighbors, then the node n is a jump point; if the movement from the parent node p(n) of the node n to the node n is a grid diagonal movement (that is, the search state of the node n is the obstacle-free diagonal movement state or the obstacle diagonal movement state), and the node n can reach a certain jump point through horizontal or vertical movement, then the node n is also a jump point.

[0077] After determining the jump points according to the above method, the JPS jump point algorithm will start from the starting point and search in the global planning range for 8 directions of the starting point. When a jump point or an obstacle is found, the search in this direction will stop, and the found jump point will be added to the first list. After the search for the current point is completed, the jump point that can achieve the shortest path is extracted from the first list, the jump point is set as the current node for the next search, and the jump point is added to the second list. Repeat this process until the end point is added to the second list, complete the global path planning, and find several global path points for each UAV.

[0078] Optionally, in the embodiment of the present application, the flight control board of the UAV is built-in with the PX4 open-source flight software, which is responsible for the basic attitude and position control of the UAV, has multiple flight modes and automation functions, and can be customized and extended. The on-board computer is equipped with the Ubuntu operating system and deploys the ROS robot operating system framework. ROS is used to manage the code of each functional module, improve the code reuse rate, and its distributed processing framework makes each module loosely coupled, and realizes data interaction between modules through various communication methods. The ground console realizes the task planning function based on the open-source software QGroundControl, can run on multiple devices, and provides complete flight control and parameter settings for the UAV.

[0079] Step S2, based on the global path points and the preset communication topology graph of the UAV cluster, use the consensus formation control algorithm to perform cooperative control on the UAV cluster, and construct a global controller for the UAV cluster.

[0080] In the embodiment of the present application, during the cooperative formation of the UAV cluster, the behaviors and interactions between UAVs affect the entire formation. These UAVs are connected to each other through the communication network. Therefore, the communication relationships between these UAVs are represented in a graph structure to obtain the communication topology graph of the UAV cluster. Each UAV is modeled as a node in the graph, and the connection lines between nodes represent the information flow between UAVs, specifically as Figure 3 shown.

[0081] Then, based on the communication topology graph of the UAV cluster, an adjacency matrix can be obtained. The elements in this adjacency matrix represent whether there is a communication connection between UAVs. For example, a ij is the i, j term of the adjacency matrix. A value of 1 indicates that there is a communication between UAV i and UAV j, and a value of 0 indicates that there is no communication between UAV i and UAV j.

[0082] Furthermore, based on the current position of UAV i at time t, the position that the UAV will reach at the next moment, etc., a motion model of each UAV is constructed. The specific expression is:

[0083]

[0084] where \(x(t)\) is the current position of the UAV at time \(t\), is the flight speed of the UAV obtained by differentiating \(x(t)\), and \(u\) i (t) is the control input of the UAV at time \(t\), and \(N\) is the total number of UAV clusters.

[0085] Through the control input, the flight speed of the corresponding UAV can be adjusted. Therefore, based on the above formula, a consensus controller for each UAV can be constructed, and the specific expression is:

[0086]

[0087] where \(a\) ij is the \(i,j\) term of the adjacency matrix, \(x\) j (t) is the current position of UAV \(j\) at time \(t\), and \(x\) i (t) is the current position of UAV \(i\) at time \(t\).

[0088] If the communication topology graph of the UAV cluster is an undirected graph and is connected, then according to the above consensus controller, through the communication connection between UAVs provided by the adjacency matrix, the influence of the positions of UAVs on their own flight speeds can be studied, and then the influence of their own positions can be known, and finally the UAV formation reaches a consensus balance. According to the motion model, the communication topology graph of the UAV cluster, and the consensus controller, the global controller of the UAV cluster can be obtained, and the specific expression is:

[0089]

[0090] where \(x(t)\) is the current position of the UAV at time \(t\), is the flight speed of the UAV obtained by differentiating \(x(t)\); \(D\) is the preset degree matrix, which is a diagonal matrix, where each diagonal element \(D\) ii represents the degree of node \(i\), that is, the sum of the weights of all edges connected to node \(i\); \(A\) is the adjacency matrix corresponding to the communication topology graph of the UAV cluster; \(L\) is the Laplacian matrix, which contains the connection information of the communication topology graph and is the core of the global controller. It can drive the system to reach consensus by aggregating the neighbor state differences. When the communication topology graph is connected, the properties of \(L\) ensure that the states of all UAVs finally converge to the same value.

[0091] It can be seen from the global controller that the closed-loop dynamic characteristics of the UAV formation depend on the Laplacian matrix of the formation. In discrete time, the method for updating the state parameters of each UAV in the formation is:

[0092] \(x\) i (t + 1)=x i (t)+u i (t)*dt;

[0093] In the formula, dt is the data sampling time interval. Based on the above theoretical basis, for the formation switching problem, it can be transformed into the problem of collaborative variable switching, that is, a set of collaborative variables corresponds to a formation. Design a certain collaborative variable switching mechanism, and thus the process of formation switching can be achieved.

[0094] Step S3, local path planning and obstacle avoidance are performed on the global controller by constructing a potential function of the unmanned aerial vehicle, so as to achieve formation control of the unmanned aerial vehicle cluster.

[0095] In the embodiment of the present application, collisions between unmanned aerial vehicles and between unmanned aerial vehicles and obstacles also need to be considered during the process of unmanned aerial vehicle cluster formation. Therefore, the artificial potential field method is adopted for local path planning and obstacle avoidance.

[0096] The artificial potential field method designs the movement of the unmanned aerial vehicle as the movement in a virtual force field. The target point / end point generates an attractive force on the unmanned aerial vehicle to attract the unmanned aerial vehicle to move towards it; the obstacle and other unmanned aerial vehicles generate a repulsive force on the unmanned aerial vehicle to avoid the unmanned aerial vehicle from colliding with them. The resultant force received by the unmanned aerial vehicle is the vector sum of the attractive force and the repulsive force, and the position is updated according to the resultant force, thereby achieving path planning. Among them, the virtual force field is also called the potential field, which includes the attractive force field and the repulsive force field. The attractive force field is usually proportional to the distance between the unmanned aerial vehicle and the target point. The greater the distance, the stronger the attractive force; the repulsive force field is usually inversely proportional to the distance between the unmanned aerial vehicle and the obstacle. The closer the distance, the greater the repulsive force.

[0097] The embodiment of the present application adopts the method of constructing a potential function to construct a potential field. The potential function is a differentiable function, and the attractive force function and the repulsive force function are the simplest potential functions. Given the starting point and end point of the unmanned aerial vehicle cluster in the current flight environment, as well as the positions of the obstacles, the potential function of each unmanned aerial vehicle is constructed, so that the unmanned aerial vehicle moves along a certain path under the combined action of the attractive force and the repulsive force and avoids obstacles until it reaches the end point.

[0098] The potential function U(q) of a certain node q within the global planning range can be expressed as:

[0099] U(q) = U att (q) + U req (q);

[0100] In the formula, U att (q) is the attractive force potential function, and U req (q) is the repulsive force potential function.

[0101] Then, according to the potential function of the node, the repulsive force potential function between unmanned aerial vehicles, the repulsive force potential function between the unmanned aerial vehicle and the obstacle, and the attractive force potential function between the unmanned aerial vehicle and the corresponding end point are constructed.

[0102] The repulsive potential function between the drones is specifically expressed as:

[0103]

[0104] Where U rep (p) is the repulsive potential function between the UAVs at node p, q i and q j are the positions of the i-th and j-th UAVs, K r is the repulsion coefficient, r o is the repulsion range parameter;

[0105] The repulsive potential function between the drone and the obstacle is specifically expressed as:

[0106]

[0107] Where U rep (p) is the repulsive potential function between the UAV and the obstacle at node p, q i is the position of the drone, q k is the position of the kth obstacle, K ro is the repulsion coefficient, r o is the repulsion range parameter;

[0108] The gravitational potential function between the drone and the corresponding end point is specifically expressed as:

[0109]

[0110] Where U att (q) is the gravitational potential function between the UAV at node q and the corresponding end point, q i is the position of the drone, q target is the position of the end point, K a is the gravitational coefficient.

[0111] According to the above-mentioned repulsive potential function and gravitational potential function, the global controller is used to perform local path planning and obstacle avoidance to obtain the moving direction and speed of the drone cluster at the next moment, so that the drones can move along a certain path and avoid obstacles under the joint action of gravity and repulsion until they reach the end point. Ultimately, the drone cluster completes formation control and switching as a whole, improves the flexibility of formation control, and enables drones to effectively avoid obstacles in complex environments.

[0112] Based on steps S1, S2, and S3 provided in the embodiments of the present application, a simulation test of obstacle avoidance for an unmanned aerial vehicle (UAV) cluster formation in a complex scenario was carried out in a simulation system. In the simulation system, a complex obstacle avoidance scenario consisting of 5 UAVs and 10 different obstacles was created. During the process of the UAV cluster moving towards the end point, 10 different obstacles were arranged. The UAV cluster used the artificial potential field method to avoid obstacles while maintaining the formation, and the minimum distance from the obstacles was kept above 1 m. After successfully avoiding the obstacles, the formation was maintained and the end point was reached.

[0113] In Figure 4 shows the path of the UAV cluster reaching the end point in the simulation system. Among them, the red squares are the global path points planned by the JPS global path planning, the yellow square is the end point of the UAV cluster, the blue small balls are the leaders of the UAV cluster, the green small balls are the followers of the UAV cluster, and the black blocks are obstacles of different sizes. The process of showing the UAV cluster starting from the starting point and reaching the end point is from the upper left figure -> the upper right figure -> the lower left figure -> the lower right figure. During the flight of the UAV cluster, the global path points will guide it to reach the end point. The lower right figure shows the flight paths of the UAV cluster represented by 5 different colors. Each color represents the flight path of one UAV. A total of 5 UAVs are shown how to avoid obstacles during the whole flight process. Finally, the UAV cluster maintains a cross formation after avoiding obstacles and reaches the end point. It can be seen from the figure of the UAV path that the UAV cluster has well avoided obstacles during the process of moving towards the end point in formation.

[0114] Implementing the embodiments of the present application has the following beneficial effects:

[0115] In the embodiments of the present application, based on the currently known current positions of the UAVs and the corresponding end points, a global path planning is carried out for each UAV based on avoiding the positions of the obstacles, and the global path points passed by the UAVs from the starting point to the end point are obtained. Then, a communication topology graph of the UAV cluster is constructed according to the communication situation between the UAVs, and a global controller is constructed based on the communication relationship between the UAVs to coordinately control the formation switching of the UAV cluster, reducing the problems of communication delay and data asynchronization, improving the real-time performance of the UAV cluster control, and also enhancing the flexibility of the formation control through the global controller. Finally, the potential functions between the UAVs, the end point, and the obstacles are calculated to measure the gravitational and repulsive forces between these nodes, so that the UAVs can effectively avoid obstacles based on the planned global path in a complex environment.

[0116] Second Embodiment

[0117] Furthermore, in order to execute the UAV cluster control design and simulation system based on the consensus algorithm corresponding to the above method embodiments to achieve the corresponding functions and technical effects, Figure 5Provided is a structural diagram of an unmanned aerial vehicle (UAV) swarm control design and simulation system based on a consensus algorithm. For the sake of illustration, only parts relevant to this embodiment are shown. The UAV swarm control design and simulation system based on the consensus algorithm provided by the embodiments of the present application includes:

[0118] A global path planning module 201, configured to perform global path planning based on the starting point and the ending point of the UAV swarm in the current flight environment, with the goal of avoiding obstacle positions, to obtain a number of global path points for each UAV.

[0119] In the embodiments of the present application, taking the current position of the UAV as the starting point, a global planning range is determined through the starting point and the ending point;

[0120] Based on the current position of the UAV, node search is performed within the global planning range with the goal of avoiding obstacle positions to find the forced neighbors of each node within the global planning range; wherein, the forced neighbor is a node that forces the algorithm to change the moving direction during global path planning;

[0121] According to the forced neighbors, the ending point and the starting point of the UAV, nodes belonging to the jump points are found from within the global planning range;

[0122] Based on the jump points and the obstacle positions, global path planning is performed starting from the current position of the UAV to obtain a number of global path points for each UAV.

[0123] A global controller generation module 202, configured to perform collaborative control on the UAV swarm by using a consensus formation control algorithm based on the global path points and a preset communication topology graph of the UAV swarm, and construct a global controller for the UAV swarm.

[0124] In the embodiments of the present application, according to the global path points, a motion model of each UAV is constructed through the current position of the UAV and the preset next moment position of the UAV;

[0125] According to the communication relationship between UAVs, a communication topology graph of the UAV swarm is constructed;

[0126] Through the communication topology graph of the UAV swarm and the motion model, considering the formation switching of the UAV swarm, a global controller for the UAV swarm is constructed.

[0127] A local obstacle avoidance module 203, configured to perform local path planning and obstacle avoidance on the global controller by constructing a potential function of the UAV to achieve formation control of the UAV swarm.

[0128] In the embodiments of the present application, during the process of UAV swarm formation, collisions between UAVs and between UAVs and obstacles need to be considered. Therefore, the artificial potential field method is adopted for local path planning and obstacle avoidance.

[0129] The artificial potential field method designs the movement of the UAV as movement in a virtual force field. The target point / end point generates an attractive force on the UAV to attract the UAV to move towards it; obstacles and other UAVs generate repulsive forces on the UAV to avoid collisions between the UAV and them. The resultant force received by the UAV is the vector sum of the attractive force and the repulsive force. The position is updated according to the resultant force, thereby achieving path planning. Among them, the gravitational field is usually proportional to the distance between the UAV and the target point. The greater the distance, the stronger the gravitational force; the repulsive force field is usually inversely proportional to the distance between the UAV and the obstacle. The closer the distance, the greater the repulsive force.

[0130] The embodiments of the present application adopt the method of constructing a potential function to construct a potential field. The potential function is a differentiable function, and the gravitational function and the repulsive force function are the simplest potential functions. Given the starting point and end point of the UAV swarm in the current flight environment, as well as the positions of the obstacles, the potential function of each UAV is constructed so that the UAV moves along a certain path under the combined action of the attractive force and the repulsive force and avoids obstacles until it reaches the end point.

[0131] The potential function U(q) of a certain node q within the global planning range can be expressed as:

[0132] U(q) = U att (q) + U req (q);

[0133] In the formula, U att (q) is the gravitational potential function, and U req (q) is the repulsive force potential function.

[0134] Then, according to the potential function of the node, the repulsive force potential function between UAVs, the repulsive force potential function between the UAV and the obstacle, and the gravitational potential function between the UAV and the corresponding end point are constructed.

[0135] The specific expression of the repulsive force potential function between UAVs is:

[0136]

[0137] In the formula, U rep (p) is the repulsive force potential function between UAVs at node p, q i and q j are the positions of the i-th and j-th UAVs respectively, K r is the repulsive force coefficient, and r o is the repulsive force range parameter;

[0138] The repulsion potential function between the UAV and the obstacle has the following specific expression:

[0139]

[0140] In the formula, U rep (p) is the repulsion potential function between the UAV and the obstacle at node p, q i is the position of the UAV, q k is the position of the k-th obstacle, K ro is the repulsion coefficient, r o is the repulsion range parameter.

[0141] The attraction potential function between the UAV and the corresponding end point has the following specific expression:

[0142]

[0143] In the formula, U att (q) is the attraction potential function between the UAV and the corresponding end point at node q, q i is the position of the UAV, q target is the position of the end point, K a is the attraction coefficient.

[0144] According to the above repulsion potential function and attraction potential function, local path planning and obstacle avoidance are performed on the global controller to obtain the moving direction and moving speed of the UAV swarm at the next moment, so that the UAVs move along a certain path under the combined action of attraction and repulsion and avoid obstacles until they reach the end point, ultimately enabling the UAV swarm to complete formation control and switching as a whole, enhancing the flexibility of formation control, and enabling the UAVs to effectively avoid obstacles in a complex environment.

[0145] In some embodiments, the global path planning module 201 is specifically:

[0146] The JPS jump point algorithm is used to perform global path planning for the flight of the UAV swarm. First, the corresponding end point is cut within 100 m of the current position of the UAV to determine the global planning range, and then several global path points that can avoid obstacles and reach the end point are calculated through the starting point, the end point, and the obstacle positions.

[0147] Among them, the JPS jump point algorithm is a graph-based search algorithm that improves on the basis of the A* algorithm. It retains the framework of the A* algorithm, and the cost function can still be expressed as f(n) = g(n) + h(n), but it optimizes the operation of the A* algorithm to find successor nodes. Different from the strategy of the A* algorithm to traverse all reachable neighbor nodes of the current node, the JPS jump point algorithm pre-selects the jump points with computational value logically as the child nodes of the current node through the jump point search strategy, reducing the cost evaluation operation of intermediate nodes and further improving the algorithm efficiency. The implementation of the jump point search strategy is mainly based on the neighbor node pruning rule and the jumping rule.

[0148] The neighbor node pruning rule determines the natural neighbors and forced neighbors with search value of the node according to the search state of the node within the global planning range, which can greatly improve the path planning efficiency. First, starting from the current position of the UAV and aiming to reach the end point, the search state of each node within the global planning range is determined through a preset node judgment rule. The node judgment rule defines four search states, including obstacle-free horizontal movement state, obstacle-free diagonal movement state, obstacle horizontal movement state, and obstacle diagonal movement state. If there are obstacles within the global planning range, it is an obstacle state. If the previous movement state of the node is horizontal movement, it is a horizontal movement state, and if it is diagonal movement, it is a diagonal movement state.

[0149] Then, based on the search state, combined with the obstacle position, path selection is performed from the node to the corresponding adjacent node to determine the path trajectory between each node and the corresponding adjacent node. When determining the path trajectory, the search value of the node within the global planning range is also judged, and the adjacent nodes of the node are divided into natural neighbors or forced neighbors according to the search value.

[0150] Among them, the natural neighbors are the adjacent nodes that can be directly reached from the current node within the global planning range, that is, the nodes adjacent to the current node horizontally, vertically, and diagonally; the forced neighbors are the nodes that force the change of the moving direction during the path planning process; in addition, the pruning neighbors without search value are also divided among the natural neighbors.

[0151] Furthermore, by dividing the natural neighbors and forced neighbors with search value, as well as the pruning neighbors without search value, it can assist the JPS jump point algorithm not to consider the pruning neighbors in the global path planning, saving the calculation of these worthless nodes and greatly improving the path planning efficiency.

[0152] According to the above neighbor node pruning rule, the forced neighbors and natural neighbors starting from the current position of the UAV within the global planning range can be found. Then, several jump points are found from the global planning range according to the jumping rule to assist the JPS jump point algorithm in global path planning.

[0153] The jump rule stipulates how to find jump points from all nodes within the global path planning. Specifically: for any node n, if node n is a location or the end point, then node n is a jump point; if node n has forced neighbors, then node n is a jump point; if the movement from the parent node p(n) of node n to node n is a grid diagonal movement (i.e., the search state of node n is an obstacle-free diagonal movement state or an obstacle diagonal movement state), and node n can reach a certain jump point through horizontal or vertical movement, then node n is also a jump point.

[0154] After determining the jump points according to the above method, the JPS jump point algorithm will search in 8 directions of the starting point within the global planning range starting from the starting point. When a jump point or an obstacle is searched, the search in this direction will stop, and the searched jump point will be added to the first list; after the search of the current point is completed, the jump point that can achieve the shortest path will be extracted from the first list, the jump point will be set as the current node for the next search, and the jump point will be added to the second list. Repeat this process until the end point is added to the second list, complete the global path planning, and find several global path points for each UAV.

[0155] Optionally, in the embodiment of the present application, the UAV flight control board is built-in with the PX4 open-source flight software, which is responsible for the basic attitude and position control of the UAV, has multiple flight modes and automation functions, and can be customized and extended. The on-board computer is equipped with the Ubuntu operating system and deploys the ROS robot operating system framework. ROS is used to manage the code of each functional module, improve the code reuse rate, and its distributed processing framework makes each module loosely coupled, and realizes data interaction between modules through multiple communication methods. The ground console realizes the task planning function based on the open-source software QGroundControl, can run on multiple devices, and provides complete flight control and parameter settings for the UAV.

[0156] In some embodiments, the global controller generation module 202 is specifically:

[0157] In the collaborative formation process of the UAV cluster, the behaviors and interactions between UAVs affect the entire formation. These UAVs are interconnected through the communication network link. Therefore, the communication relationships between these UAVs are represented in a graph structure to obtain the communication topology graph of the UAV cluster. Each UAV is modeled as a node in the graph, and the connection lines between nodes represent the information flow between UAVs.

[0158] Then, based on the communication topology graph of the UAV cluster, an adjacency matrix can be obtained. The elements in this adjacency matrix represent whether there is a communication connection between UAVs. For example, a ij is the i, j term of the adjacency matrix, 1 indicates that there is communication between UAV i and UAV j, and 0 indicates that there is no communication between UAV i and UAV j.

[0159] Then, based on the current position of the UAV i at time t, the position that the UAV will reach at the next moment, etc., a motion model of each UAV is constructed. The specific expression is as follows:

[0160]

[0161] In the formula, x(t) is the current position of the UAV at time t, is the flight speed of the UAV obtained by taking the derivative of x(t), and u i (t) is the control input of the UAV at time t, and N is the total number of the UAV swarm.

[0162] Through the control input, the flight speed of the corresponding UAV can be adjusted. Therefore, based on the above formula, a consensus controller for each UAV can be constructed. The specific expression is as follows:

[0163]

[0164] In the formula, a ij is the i, j term of the adjacency matrix, x[[ID=—]] j (t) is the current position of UAV j at time t, and x[[ID=—]] i (t) is the current position of UAV i at time t.

[0165] If the communication topology graph of the UAV swarm is an undirected graph and is connected, then according to the above consensus controller, through the communication connection between UAVs provided by the adjacency matrix, the influence of the position between UAVs on their own flight speed can be studied, and then the influence of their own position can be known. Finally, the UAV formation reaches a consensus balance. According to the motion model, the communication topology graph of the UAV swarm, and the consensus controller, a global controller of the UAV swarm can be obtained. The specific expression is as follows:

[0166]

[0167] In the formula, x(t) is the current position of the UAV at time t, is the flight speed of the UAV obtained by taking the derivative of x(t); D is a preset degree matrix, which is a diagonal matrix, and each diagonal element D ii represents the degree of node i, that is, the sum of the weights of all edges connected to node i; A is the adjacency matrix corresponding to the communication topology graph of the UAV swarm; L is the Laplacian matrix, which contains the connection information of the communication topology graph and is the core of the global controller. It can drive the system to reach consensus by aggregating the neighbor state differences. When the communication topology graph is connected, the properties of L ensure that the states of all UAVs finally converge to the same value.

[0168] As can be seen from the global controller, the closed-loop dynamic characteristics of the UAV formation depend on the Laplacian matrix of the formation. In discrete time, the method for updating the state parameters of each UAV in the formation is as follows:

[0169] x i (t + 1) = x i (t) + u i (t) * dt;

[0170] In the above formula, dt is the data sampling time interval. Based on the above theory, for the formation switching problem, it can be transformed into the problem of collaborative variable switching, that is, a set of collaborative variables corresponds to a formation. By designing a certain collaborative variable switching mechanism, the process of formation switching can be realized.

[0171] Implementing the embodiments of the present application has the following beneficial effects:

[0172] In the embodiments of the present application, based on the currently known current positions and corresponding end points of the UAVs, a global path planning is performed for each UAV with avoiding the obstacle positions as the basis, and the global path points that the UAVs pass through from the starting point to the end point are obtained. Then, a communication topology graph of the UAV cluster is constructed based on the communication situation between the UAVs, and a global controller is constructed based on the communication relationship between the UAVs to perform collaborative control on the formation switching of the UAV cluster, reducing the problems of communication delay and data asynchronization, improving the real-time performance of the UAV cluster control, and also enhancing the flexibility of the formation control through the global controller. Finally, the potential functions between the UAVs, the end point, and the obstacles are calculated to measure the gravitational and repulsive forces between these nodes, so that the UAVs can effectively avoid obstacles based on the planned global path in a complex environment.

[0173] The above specific embodiments have further elaborated on the purpose, technical solutions, and beneficial effects of the present application. It should be understood that the above are only specific embodiments of the present application and are not used to limit the protection scope of the present application. It is particularly pointed out that for those skilled in the art, any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A UAV swarm control design and simulation method based on a consensus algorithm, characterized by: include: Based on the starting and ending points of the drone cluster in the current flight environment, global path planning is performed with the goal of avoiding obstacles, and several global path points are obtained for each drone. Based on the global path points and the preset communication topology of the UAV cluster, a consistent formation control algorithm is used to collaboratively control the UAV cluster and build a global controller for the UAV cluster; The formation control of the UAV cluster is achieved by constructing the potential function of the UAV to perform local path planning and obstacle avoidance on the global controller.

2. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 1 is characterized in that: According to the starting point and end point of the drone cluster in the current flight environment, global path planning is performed with the goal of avoiding obstacle positions, and several global path points for each drone are obtained, specifically: Taking the current position of the UAV as the starting point, the global planning range is determined by the starting point and the end point; Based on the current position of the UAV, a node search is performed within the global planning range with the goal of avoiding obstacles, and a forced neighbor of each node within the global planning range is found; wherein the forced neighbor is a node that forces the algorithm to change the movement direction during global path planning; According to the forced neighbors, the end point and the starting point of the UAV, find the nodes belonging to the jump point from the global planning range; Based on the jump points and obstacle positions, global path planning is performed starting from the current position of the UAV to obtain several global path points for each UAV.

3. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 2 is characterized in that: Based on the current position of the UAV, a node search is performed within the global planning range with the goal of avoiding obstacle positions, and the forced neighbors of each node within the global planning range are found, specifically: Starting from the current position of the UAV and aiming to reach the destination, the search state of each node within the global planning range is determined by a preset node judgment rule; wherein the search state includes an unobstructed horizontal movement state, an unobstructed diagonal movement state, an obstructed horizontal movement state, and an obstructed diagonal movement state; Path selection is performed based on the search status and obstacle positions, and forced neighbors of each node are determined.

4. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 3 is characterized in that: The path selection is performed based on the search status and the obstacle location to determine the forced neighbor of each node, specifically: Based on the search state, a path is selected from the node to the corresponding adjacent node in combination with the obstacle position, and a path trajectory between each node and the corresponding adjacent node is determined; According to the search value of the nodes in the path trajectory, the adjacent nodes are divided into natural neighbors or forced neighbors; wherein the search value is used to measure whether a node is meaningful to search when performing path selection.

5. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 2 is characterized in that: The method of finding the nodes belonging to the jump points within the global planning range according to the forced neighbors, the end point and the starting point of the drone is as follows: If a node within the global planning range is the starting point or the end point, the node is determined to be a jump point; If a node within the global planning range has the forced neighbor, determining the node as a jump point; If the search state of a certain node within the global planning range is an unobstructed diagonal movement state or an obstructed diagonal movement state, and the node can reach a certain jump point by moving horizontally or vertically, the node is determined to be a jump point.

6. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 1 is characterized in that: Based on the global path points and the preset communication topology of the UAV cluster, the UAV cluster is collaboratively controlled using a consistent formation control algorithm to construct a global controller of the UAV cluster, specifically: Based on the global path points, a motion model of each drone is constructed using the drone's current position and the preset next moment position. Constructing a communication topology diagram of the drone cluster based on the communication relationship between drones; By using the communication topology diagram of the drone cluster and the motion model, and taking into account the formation switching of the drone cluster, a global controller of the drone cluster is constructed.

7. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 6 is characterized in that: The global controller is specifically: Where x(t) is the current position of the UAV at time t, is the flight speed of the controlled drone obtained by deriving x(t), D is the preset degree matrix, A is the adjacency matrix corresponding to the communication topology graph of the drone cluster, and L is the Laplace matrix.

8. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 1 is characterized in that: The potential function of the UAV is constructed to perform local path planning and obstacle avoidance on the global controller, specifically: Based on the starting and ending points of the drone cluster in the current flight environment, as well as the locations of obstacles, the potential function of each drone is constructed using the artificial potential field method; wherein the potential function includes the gravitational potential function and the repulsive potential function; According to the repulsive potential function between the drones, the repulsive potential function between the drones and the obstacles, and the gravitational potential function between the drones and the corresponding end points, the global controller performs local path planning and obstacle avoidance to obtain the moving direction and moving speed of the drone cluster at the next moment; The formation control of the drone cluster is completed according to the moving direction and speed of the drone cluster at the next moment.

9. The UAV swarm control design and simulation method based on the consensus algorithm according to claim 8 is characterized in that: The repulsive potential function and the attractive potential function are specifically: The repulsive potential function between UAVs is specifically expressed as: Where U rep (p) is the repulsive potential function between the UAVs at node p, q i and q j are the positions of the i-th and j-th UAVs, K r is the repulsion coefficient, r o is the repulsion range parameter; The repulsive potential function between the drone and the obstacle is specifically expressed as: Where U rep (p) is the repulsive potential function between the UAV and the obstacle at node p, q i is the position of the drone, q k is the position of the kth obstacle, K ro is the repulsion coefficient, r o is the repulsion range parameter; The gravitational potential function between the UAV and the corresponding end point is specifically expressed as: Where U att (q) is the gravitational potential function between the UAV at node q and the corresponding end point, q i is the position of the drone, q target is the position of the end point, K a is the gravitational coefficient.

10. A UAV swarm control design and simulation system based on consensus algorithm, characterized by: include: Global path planning module, global controller generation module and local obstacle avoidance module; The global path planning module is used to perform global path planning based on the starting and ending points of the drone cluster in the current flight environment, with the goal of avoiding obstacles, and obtain several global path points for each drone. The global controller generation module is used to collaboratively control the UAV cluster based on the global path points and the preset communication topology of the UAV cluster using a consistent formation control algorithm to build a global controller for the UAV cluster; The local obstacle avoidance module is used to perform local path planning and obstacle avoidance on the global controller by constructing the potential function of the drone, so as to realize the formation control of the drone cluster.

Images