A method and device for controlling and avoiding obstacles of unmanned aerial vehicle swarm

Abstract

The present invention relates to a method and device for controlling and avoiding obstacles of a drone cluster, which belongs to the technical field of drones and solves the problem that the perception ability, dynamic transformation ability and obstacle avoidance ability of existing drones to the external environment are insufficient to meet their own obstacle avoidance needs. The method comprises: when some drones in a drone cluster perceive an obstacle, each drone in the partial drones locates the obstacle position according to its own positioning and obstacle perception results and broadcasts the obstacle position in the drone cluster; the drone cluster makes adaptive changes according to the obstacle position and autonomously adjusts the drone's own heading so that the obstacle always remains within the drone cluster's line of sight; after the drone cluster passes the obstacle, the tail of the drone cluster keeps facing the obstacle to ensure that the drone cluster as a whole is out of the obstacle range. The drone cluster makes adaptive changes according to the obstacle and autonomously adjusts the drone's own heading to achieve the cluster's large-scale obstacle perception capability.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and in particular to a method and device for controlling and avoiding obstacles of unmanned aerial vehicle clusters. Background Art

[0002] Currently, drone swarms mostly fly in open areas. Performing missions in complex urban environments places higher demands on the swarm's dynamic transformation, collision avoidance, and obstacle avoidance capabilities. The types and numbers of sensors carried by miniaturized drone platforms are limited, resulting in the inability of individuals in the swarm to perceive the external environment to meet their own obstacle avoidance needs. Summary of the invention

[0003] In view of the above analysis, the embodiments of the present invention aim to provide a method and device for drone cluster control and obstacle avoidance, so as to solve the problem that the existing drones' ability to perceive the external environment, dynamic transformation ability, and obstacle avoidance ability are insufficient to meet their own obstacle avoidance needs.

[0004] On the one hand, an embodiment of the present invention provides a method for controlling and avoiding obstacles in a drone cluster, including: when some drones in a drone cluster sense an obstacle, each drone in the said part of the drones locates the obstacle position according to its own positioning and the obstacle perception result and broadcasts the obstacle position in the drone cluster; the drone cluster adaptively changes according to the obstacle position and autonomously adjusts the drone's own heading so that the obstacle always remains within the line of sight of the drone cluster; and after the drone cluster passes the obstacle, the tail of the drone cluster keeps facing the obstacle to ensure that the drone cluster as a whole leaves the obstacle range.

[0005] The beneficial effects of the above technical solution are as follows: the drone cluster adapts to the location of obstacles and autonomously adjusts the drone's own heading. Even when a single machine carries a small number of visual sensors, the cluster can still achieve a large-scale obstacle perception capability, effectively reducing the size and weight restrictions of the body platform.

[0006] Based on the further improvement of the above method, the drone cluster performs adaptive changes according to the obstacle position, including the drone cluster performs adaptive changes according to the obstacle position according to the aggregation criterion, the separation criterion and the speed matching criterion, wherein the aggregation criterion is that each drone in the drone cluster obtains the position information of other drones in the neighborhood, combines its own position with the control algorithm to calculate the expected state offset and feeds it back to its own controller, so that the current drone is closer and closer to the center position of the drone cluster to achieve an aggregated state; the separation criterion is to obtain the distance offset between the position of each drone and the positions of other drones in the neighborhood, and feed the distance offset back to its own controller. When the distance offset is less than the safe distance between the drones, the self-controller generates a repulsive force inversely proportional to the distance offset, and the existence of the repulsive force causes the drone cluster to separate in motion; the speed matching criterion is that each drone obtains the speed information of other drones in the neighborhood in real time, compares the speed of other drones with the speed of the current drone to obtain the speed offset, and feeds the speed offset back to its own controller for speed adjustment, so that the speed size and direction of all drones in the drone cluster are consistent.

[0007] Based on the further improvement of the above method, the drone cluster autonomously adjusts the drone's own heading according to the obstacle position, including the drone cluster adjusting the drone's own heading according to the obstacle position according to the viewing angle distribution criterion, wherein the viewing angle distribution criterion is that each drone in the drone cluster adjusts its orientation so that the line of sight angle of the drone cluster is evenly distributed with the drone cluster as the center, and by obtaining the position and orientation of other drones in the neighborhood, the expected heading angle is fed back to its own controller, so that compared with the drone cluster whose line of sight angle is set to be evenly distributed, the drone cluster with a uniformly distributed line of sight angle obtains a larger line of sight range.

[0008] Based on further improvement of the above method, the drone cluster control and obstacle avoidance method also includes using a cluster control algorithm to perform cluster control on a drone cluster randomly distributed at an initial position to achieve: the distances between the drones in the drone cluster tend to be consistent; the drone cluster moves in a desired direction with a virtual navigator as the center; the drone cluster automatically avoids obstacles when encountering obstacles; after passing through the obstacle range, the cluster re-gathers toward the center and restores the formation.

[0009] Based on the further improvement of the above method, the cluster control algorithm includes: the control input u of the i-th UAV i and w i , where based on the control input u i Control the relative position and speed between the drones in the drone cluster based on the control input wi Control the sight angle distribution of the drones in the drone cluster.

[0010] Based on the further improvement of the above method, the UAV airborne visual sensor is fixed on the body, and the control input w of the i-th UAV is expressed by the following formula: i :

[0011] w i =k i *(2π*i / N i -γ i )

[0012] Among them, k i is the heading control gain of UAV i, γ i is the heading angle of the i-th UAV, N i is the neighborhood of the i-th drone:

[0013] N i ={||q j -q i ||<r, j≠i, j=1,...n}

[0014] i and j are the drone numbers, and r is the drone perception radius.

[0015] Based on the further improvement of the above method, the control input u of the i-th UAV is determined according to the overall potential energy V(q) of the UAV cluster expressed by the following formula: i :

[0016]

[0017] Where q is the set of all drone positions in the drone cluster, ψ α (z) is the current distance z to the expected distance d α The potential energy integral is:

[0018]

[0019] φ α For the attraction between two drones:

[0020]

[0021]

[0022]

[0023] Among them, a, b, c, h are curve parameters, r α is the sensing distance of the drone. Since the norm ||z|| is not differentiable at z=0, the norm ||z|| is rewritten as the norm ||z|| which is differentiable everywhereσ , norm ||z|| σ Used to construct a smooth artificial potential field function, ε is the norm parameter.

[0024] Based on the further improvement of the above method, the control input u of the i-th UAV is expressed by the following formula: i is the formation control input of the i-th UAV Virtual pilot feedback control input of UAV i The virtual agent feedback control input of UAV i sum:

[0025]

[0026]

[0027]

[0028]

[0029] Among them, a ij Indicates whether drones i and j are connected before, 0 means no connection, 1 means connection, n ij is the direction vector of drone i pointing to drone j, q l is the position vector of the virtual navigator, p l is the velocity vector of the virtual navigator, c 1 and c 2 are the position and speed parameters of the virtual navigator, is a set of virtual agents, and are the position vector and velocity vector of the jth virtual agent and its connectivity with drone i, n ij is the direction vector of UAV i pointing to virtual agent j.

[0030] Based on the further improvement of the above method, the virtual intelligent body is generated at the point where the obstacle is closest to the drone. When the virtual intelligent body is within the safe distance of the drone, a repulsive force is generated on the drone, and the closer the distance, the greater the repulsive force. When the drone approaches an obstacle, it will slow down under the influence of the repulsive force and move away from the obstacle. When the drone moves away from the virtual intelligent body under the action of the repulsive force, it will automatically avoid obstacles until the obstacle is at its safe distance.

[0031] On the other hand, an embodiment of the present invention provides a drone cluster control and obstacle avoidance device, including: an obstacle data acquisition module, which is used to locate the obstacle position according to its own positioning and obstacle perception results when some drones in the drone cluster sense an obstacle; a data transmission module, which is used to broadcast the obstacle position in the drone cluster; an adjustment module, which is used to adaptively change the drone cluster according to the obstacle position and autonomously adjust the drone's own heading according to the obstacle position, so that the obstacle always remains within the line of sight of the drone cluster; and a post-processing module, which is used to keep the tail of the drone cluster facing the obstacle after the drone cluster passes the obstacle to ensure that the drone cluster as a whole is out of the obstacle range.

[0032] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0033] 1. The control algorithm of the drone cluster adaptively changes the cluster formation according to the distribution of obstacles in the environment, meeting the flight requirements of the cluster in complex environments;

[0034] 2. The drone cluster can adapt to the location of obstacles and autonomously adjust its own heading. Even if a single drone carries a small number of visual sensors, it can still achieve the cluster's ability to perceive obstacles over a large range, effectively reducing the size and weight restrictions of the aircraft platform.

[0035] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0037] Figure 1 is a flow chart of a method for controlling and avoiding obstacles of a drone cluster according to an embodiment of the present invention;

[0038] Figure 2 A schematic diagram of cluster control and obstacle avoidance principles according to an embodiment of the present invention;

[0039] Figure 3 A block diagram of cluster control interaction according to an embodiment of the present invention;

[0040] Figure 4a , Figure 4b , Figure 4cand Figure 4d They are respectively aggregation, classification, speed matching and viewing angle distribution diagram according to an embodiment of the present invention;

[0041] Figure 5a , Figure 5b , Figure 5c , Figure 5d , Figure 5e and Figure 5f They are respectively curve diagrams of a cluster formation process according to an embodiment of the present invention;

[0042] Figure 6 is a diagram illustrating the principle of an obstacle avoidance algorithm according to an embodiment of the present invention;

[0043] Figure 7a , Figure 7b , Figure 7c , Figure 7d , Figure 7e and Figure 7f They are respectively curve graphs of cluster control and obstacle avoidance algorithm tests according to embodiments of the present invention;

[0044] Figure 8 is a block diagram of a cluster simulation system according to an embodiment of the present invention;

[0045] Fig. 9 is a block diagram of a cluster semi-physical simulation system according to an embodiment of the present invention;

[0046] Fig.10 4 is a block diagram of a drone cluster control and obstacle avoidance device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0047] The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0048] refer to Figure 1 A specific embodiment of the present invention discloses a method for controlling and avoiding obstacles in a drone cluster, including: in step S102, when some drones in the drone cluster sense an obstacle, each drone in the partial drones locates the obstacle according to its own positioning and the obstacle perception result and broadcasts the obstacle position in the drone cluster; in step S104, the drone cluster makes adaptive changes according to the obstacle position and autonomously adjusts the drone's own heading so that the obstacle always remains within the sight range of the drone cluster; and in step S106, after the drone cluster passes the obstacle, the tail of the drone cluster keeps facing the obstacle to ensure that the drone cluster as a whole is out of the obstacle range.

[0049] Compared with the prior art, in the drone cluster control and obstacle avoidance method provided in this embodiment, the drone cluster adaptively changes according to the position of the obstacle and autonomously adjusts the drone's own heading. Even when a single machine carries a small number of visual sensors, the cluster can still achieve a large-scale obstacle perception capability, effectively reducing the size and weight restrictions of the body platform.

[0050] In the following, reference will be made to Figure 1 , each step of the drone cluster control and obstacle avoidance method according to an embodiment of the present invention is described in detail.

[0051] In step S102, when some drones in the drone cluster sense an obstacle, each drone in the drone cluster locates the obstacle according to its own positioning and the obstacle perception result and broadcasts the obstacle position in the drone cluster.

[0052] In step S104, the drone cluster performs adaptive changes according to the position of the obstacle and autonomously adjusts the drone's own heading, so that the obstacle always remains within the sight of the drone cluster. The drone cluster performs adaptive changes according to the obstacle position, including the drone cluster performs adaptive changes according to the aggregation criterion, separation criterion and speed matching criterion according to the obstacle position, wherein, according to the aggregation criterion, each drone in the drone cluster obtains the position information of other drones in the neighborhood, combines its own position with the control algorithm to calculate the expected state offset and feeds it back to its own controller, so that the current drone is closer and closer to the center position of the drone cluster to achieve an aggregated state; the separation criterion obtains the distance offset between the position of each drone and the position of other drones in the neighborhood, and feeds the distance offset back to its own controller. When the distance offset is less than the safe distance between the drones, the self-controller generates a repulsive force inversely proportional to the distance offset. The existence of the repulsive force separates the drone cluster in motion; the speed matching criterion, each drone obtains the speed information of other drones in the neighborhood in real time, compares the speed of other drones with the speed of the current drone to obtain the speed offset, and feeds the speed offset back to its own controller for speed adjustment, so that the speed size and direction of all drones in the drone cluster are consistent. The drone cluster autonomously adjusts the drone's own heading according to the obstacle position, including the drone cluster adjusting the drone's own heading according to the obstacle position according to the perspective distribution criterion, wherein the perspective distribution criterion is that each drone in the drone cluster adjusts its direction so that the sight angle of the drone cluster is evenly distributed with the drone cluster as the center, and by obtaining the position and direction of other drones in the neighborhood, the desired heading angle is fed back to its own controller, so that compared with the drone cluster whose sight angle is set to be evenly distributed, the drone cluster whose sight angle is evenly distributed obtains a larger sight range. Specifically, the sight directions of the drones in the formation of the prior art are the same, and the drone cluster control and obstacle avoidance method according to the embodiment of the present invention is improved to have a larger sight range than the prior art.

[0053] In step S106, after the drone cluster passes through the obstacle, the tail of the drone cluster remains facing the obstacle to ensure that the drone cluster as a whole is out of the obstacle range.

[0054] The drone cluster control and obstacle avoidance method also includes using a cluster control algorithm to perform cluster control on a drone cluster that is randomly distributed at an initial position to achieve: the distances between the drones in the drone cluster tend to be consistent; the drone cluster moves in a desired direction with a virtual navigator as the center; the drone cluster automatically avoids obstacles when encountering obstacles; and after passing through the obstacle range, the cluster regroups toward the center and restores the formation.

[0055] The cluster control algorithm includes: the control input u of the i-th UAV i and wi , where based on the control input u i Control the relative position and speed between drones in the drone cluster based on the control input w i Control the line of sight angle distribution of drones in a drone swarm.

[0056] The visual sensor on the drone is fixed on the body. The control input w of the i-th drone is expressed by the following formula: i :

[0057] w i =k i *(2π*i / N i -γ i )

[0058] Among them, k i is the heading control gain of UAV i, γ i is the heading angle of the i-th UAV, N i is the neighborhood of the i-th drone

[0059] N i ={||q j -q i ||<r, j≠i, j=1,...n}

[0060] i and j are the drone numbers, and r is the drone perception radius.

[0061] The overall potential energy is the sum of the potential energies between drones. Mathematically, it can be proved that the overall potential energy will converge to the minimum under the designed input, corresponding to the stable formation of the cluster. The control input u of the i-th drone is determined according to the overall potential energy V(q) of the drone cluster expressed by the following formula: i :

[0062]

[0063] Among them, q is the set of all drone positions in the drone cluster, ψ α (z) is the current distance z to the expected distance d α The potential energy integral is:

[0064]

[0065] φ α For the attraction between two drones:

[0066]

[0067]

[0068]

[0069] Among them, a, b, c, and h are curve parameters, and the curve parameters are used to comprehensively determine the curve of the virtual force between UAVs as the distance between UAVs changes. α To sense the distance, specifically, the distance that a drone tracks other drones, since the norm ||z|| is not differentiable at z=0, the norm ||z|| is rewritten as the norm ||z|| which is differentiable everywhere σ , norm ||z|| σ Used to construct a smooth artificial potential field function, ε is the norm parameter.

[0070] The control input u of the i-th drone is expressed by the following formula i is the formation control input of the i-th UAV Virtual pilot feedback control input of UAV i The virtual agent feedback control input of UAV i sum:

[0071]

[0072]

[0073]

[0074]

[0075] Among them, a ij Indicates whether drones i and j are connected before, 0 means no connection, 1 means connection, n ij is the direction vector of UAV i pointing to UAV j, q l is the position vector of the virtual navigator, p l is the velocity vector of the virtual navigator, c 1 and c 2 are the position and speed parameters of the virtual navigator, is a set of virtual agents, and are the position vector and velocity vector of the jth virtual agent and its connectivity with drone i, n ij is the direction vector of UAV i pointing to virtual agent j.

[0076] The virtual intelligent body is generated at the point where the obstacle is closest to the drone. When the virtual intelligent body is within the safe distance of the drone, a repulsive force is generated on the drone, and the closer the distance, the greater the repulsive force. When the drone approaches an obstacle, it will slow down under the influence of the repulsive force and move away from the obstacle. When the drone moves away from the virtual intelligent body under the action of the repulsive force, it will automatically avoid the obstacle until the obstacle is at its safe distance.

[0077] refer to Fig.10 A specific embodiment of the present invention discloses a drone cluster control and obstacle avoidance device, including: an obstacle data acquisition module 1002, used for locating the obstacle position according to its own positioning and obstacle perception results when some drones in the drone cluster perceive an obstacle; a data transmission module 1004, used for broadcasting the obstacle position in the drone cluster; an adjustment module 1006, used for adaptively changing the drone cluster according to the obstacle position and autonomously adjusting the drone's own heading, so that the obstacle is always kept within the line of sight of the drone cluster; and a post-processing module 1008, used for, after the drone cluster passes the obstacle, the tail of the drone cluster keeps facing the obstacle to ensure that the drone cluster as a whole is out of the obstacle range.

[0078] In the following, reference Figures 2 to 9 , the drone cluster control and obstacle avoidance method and device according to an embodiment of the present invention are described in detail by way of specific examples.

[0079] Individual drones in a swarm perceive the local environment based on onboard sensors and computing units, and individuals share and process environmental information through data links. By increasing the number of individuals in the swarm, a large-scale environmental perception capability emerges, enabling the swarm to adapt to formation changes and autonomous obstacle avoidance. Figure 2 As mentioned above, when a drone cluster encounters an obstacle, due to the limited perception range, only some drones can detect the obstacle. These drones locate the obstacle based on their own positioning and obstacle perception results and broadcast it in the cluster. Therefore, the cluster makes adaptive changes according to the obstacle distribution to cope with narrow passages. When the cluster passes through the passage, the drones autonomously adjust their heading according to the distribution of surrounding obstacles, so that the obstacle is always within the cluster's line of sight. After the cluster passes the obstacle, the tail of the cluster keeps facing the obstacle to ensure that the entire cluster is out of the obstacle range.

[0080] The drones in the cluster communicate with each other and with the ground station through data links. Each individual drone carries a differential positioning system, a visual sensor and an onboard computing unit, which can accurately obtain its own position information and obstacle position information. On this basis, the design scheme is as follows: Figure 3As shown in the figure. The input of the formation control module of individual drones consists of three parts: 1) ground station data, including formation control instructions, such as take-off, landing, formation and disbanding, and formation parameters, such as formation and formation spacing; 2) status data of other drones in the cluster; 3) obstacle data obtained by all drones in the cluster, and a global obstacle database formed by obstacle matching. The formation control module outputs the expected attitude, speed or position based on the input data and control algorithm. The collision avoidance optimization module corrects the expected value according to the status of other individuals around it and passes it to the autopilot to control the individual movement to meet the cluster movement requirements.

[0081] Finally, a UAV swarm flight test was conducted to integrate the swarm control and obstacle avoidance algorithms into the UAV platform to further verify the effectiveness of the algorithm.

[0082] The specific implementation process is as follows:

[0083] (1) Swarm Control Algorithm with Virtual Leader

[0084] Reynolds proposed three rules for group behavior based on the behavioral consistency of bird flocks, fish schools and other biological groups in the process of foraging, as well as the phenomenon of non-collision when competing for food: Flocking Centering, Collision Avoidance and Velocity Matching. Combining them with the range and distance limitations of the UAV's onboard visual sensors, he summarized the four basic principles of UAV swarms:

[0085] ① Aggregation: All individuals in the neighborhood of each individual in the cluster maintain a compact formation, which is manifested in the consistency of individual positions and cluster cohesion. The aggregation principle is as follows Figure 4a As shown in the figure, since individuals have perception ability, each individual can obtain the position information of other individuals in the neighborhood, and calculate the expected state offset based on its own position and control algorithm and feed it back to its own controller, so that the individual is closer and closer to the center of the cluster, and finally reaches the aggregated state.

[0086] ② Separation: Each individual in the cluster can maintain a certain distance from all individuals in its area to avoid collision during the formation of the aggregation state. The separation principle is as follows Figure 4b As shown in the figure, also because the individual has the ability to perceive, it judges the distance between the positions of other individuals in the neighborhood and its own position to obtain the difference, and feeds this distance offset back to its own controller. When this offset is smaller than the safe distance between individuals, it will generate a repulsive force, and this repulsive force is inversely proportional to the size of the distance offset. The existence of this repulsive force enables the cluster to separate individuals during movement.

[0087] ③ Speed ​​matching: The speed of individuals in the cluster is consistent with that of all other individuals in the neighborhood. The speed matching principle is as follows Figure 4c As shown in the figure, also due to individual perception, individuals can obtain the speed information of other individuals in the neighborhood in real time, compare it with their own speed, and feed the speed offset calculated into their own control system for speed adjustment, so that the speed size and direction of all individuals in the cluster are consistent.

[0088] ④ View distribution: Individuals in a cluster adjust their orientation so that the view angles of the cluster are evenly distributed around the cluster. The view distribution principle is as follows: Figure 4d As shown in the figure, due to the individual perception, the position and orientation of other individuals in the neighborhood can be obtained, and the desired heading angle can be calculated according to the control algorithm and fed back to its own controller, so that the cluster can obtain a larger line of sight.

[0089] Considering that rotorcraft UAVs are usually equipped with autopilots to achieve position and attitude control and can hover in the air and turn their heading, the mathematical model of the cluster is simplified to a mass point model, as shown in the following formula, where n is the number of UAVs in the cluster, p is i ,q i and γ i are the position vector, velocity vector and heading angle of the i-th UAV, u i With w i is the control input of the ith UAV, where u i The control input is used to control the relative position and speed between individuals in the cluster, w i The control input serves to control the distribution of the viewing angles of individuals in the cluster.

[0090]

[0091] In outdoor environments, the number of other drones that a drone can sense depends on the performance of the communication system. Assuming that a drone can sense other drones within its neighborhood radius, its neighborhood (representing the neighborhood set of drone i) is defined as:

[0092] N i ={||q j -q i ||<r, j≠i, j=1,...n}

[0093] Where i and j are the drone numbers, and r is the drone perception radius. If the drones are regarded as nodes and undirected edges are used to connect each node with all nodes in its neighborhood, these nodes can be obtained:

[0094] V = {1, ..., n}

[0095] and edge

[0096] E={(i,j)∈V×V:i≠j}

[0097] The network composed of these can be represented by an undirected graph G. For a certain drone in the network, some drones that were in its neighborhood in the previous period moved out of its neighborhood at a certain moment, or some drones that were not in its neighborhood in the previous period moved into its neighborhood at a certain moment, so the system has a switching topology.

[0098] The design goal of the swarm control algorithm is to design a suitable swarm control algorithm for the above-mentioned UAV swarm system by combining the artificial potential function method. Under the condition that the initial topological connectivity of the cluster is met, the following effects should be obtained after applying control input to each UAV:

[0099] ① All agents in the system remain connected during the movement and will not split;

[0100] ② The speed of all agents in the system gradually reaches a certain value, and the central speed v of the group remains constant;

[0101] ③ The relative distance between all agents in the system gradually converges to a certain range;

[0102] ④ There will be no collision between the intelligent agents in the system during movement.

[0103] Considering that the UAV's onboard visual sensor is fixed on the body, the UAV's heading angle represents the line of sight direction. The control input w is designed i As follows, where k i is the heading control gain of UAV i:

[0104] w i =k i *(2π*i / N i -γ i )

[0105] The total potential energy is the sum of the potential energies between the drones. Mathematically, it can be proved that the designed control input u i The overall potential energy will converge to the minimum, corresponding to the stable formation of the cluster. i , the overall potential energy V(q) of the designed UAV cluster is:

[0106]

[0107] Where q is the set of all drone positions in the cluster, and the remaining terms are defined as follows:

[0108]

[0109] Where z is the current distance, d α is the expected distance, ψ α (z) is the potential energy integral from the current distance to the desired distance.

[0110]

[0111]

[0112] Where a, b, c, h are parameters, r α is the perceived distance. Since the norm ||z|| is not differentiable at z=0, it is rewritten as the norm ||z|| which is differentiable everywhere σ The purpose of constructing the norm in this way is to construct a smooth and accessible artificial potential field function, where ε is the norm parameter.

[0113]

[0114] By calculating the virtual force on the drone in the potential energy field and combining the speed consistency requirement, the control input of the drone is obtained as shown in the following formula, where is the formation control input of UAV i, a ij Indicates whether drones i and j are connected before, 0 means no connection, 1 means connection, n ij is the direction vector of UAV i pointing to UAV j. Using Lyapunov's second method, it can be proved that when the system is initially connected, the system will eventually stabilize and form the following Figure 5a , 5b , 5c, 5d, 5e and 5f. The initial positions of the individuals in the drone cluster are randomly distributed. Under the action of the cluster control algorithm, the cluster slowly gathers and eventually forms a grid formation with equal distances between drones. All drones are at a safe distance and facing the same direction.

[0115] Considering that the drone cluster needs to move according to the set trajectory, it is assumed that there is a virtual navigator in the cluster, and the navigator has a position vector q l and the velocity vector p l , design the pilot feedback control input of UAV i As shown below, where c 1 and c 2 It is the control parameter of the virtual navigator item. Its function is to ensure that the drones will form a cluster state with the virtual navigator as the center and ensure that the consistency speed of the cluster is the same as that of the navigator.

[0116]

[0117] Aiming at the obstacle avoidance requirements of drone swarms, a virtual agent generation algorithm is designed based on the fact that drones can obtain distance information of objects within the local field of view through binocular vision sensors. The onboard computer obtains the distance between the drone and the obstacle and the surface range of the obstacle based on the depth image information. The obstacle avoidance algorithm automatically generates a virtual agent on the surface of the obstacle. When the agent is within the safe distance of the drone, it will generate a repulsive force on the drone. When the drone moves away from the virtual agent under the action of the repulsive force, automatic obstacle avoidance is achieved. Figure 6 When an obstacle appears within the drone's line of sight, a virtual agent is generated at the point where the obstacle is closest to the drone using the distance information of the depth image. This agent will produce a virtual repulsive force on the drone, and the closer the distance, the greater the repulsive force. The effect of this action is that when the drone approaches the obstacle, it will slow down under its influence and move away from the obstacle until the obstacle is at a safe distance, thus achieving the effect of individual obstacle avoidance.

[0118] Therefore, the pilot feedback control input of UAV i is designed as is the following formula, where is a set of virtual agents, and are the position vector and velocity vector of the jth virtual agent and its connectivity with drone i, n ij is the direction vector of UAV i pointing to virtual agent j.

[0119]

[0120] Therefore, the UAV control input u can be obtained by synthesis i for:

[0121]

[0122] Cluster control and obstacle avoidance effects Figure 7a , Figure 7b , Figure 7c , Figure 7d , Figure 7e and Figure 7f As shown in the figure, the drone swarm can follow the virtual navigator in a grid-like distributed formation. When encountering obstacles, it can automatically avoid the obstacles and re-form the formation after crossing the obstacles. These descriptions are the actual effects of swarm control and obstacle avoidance. Figure 2 The corresponding process descriptions are examples.

[0123] The initial positions of individuals in a UAV swarm are randomly distributed. Under the action of the swarm control algorithm, the swarm achieves the following: 1) The distances between UAVs tend to be consistent; 2) The swarm moves in the desired direction with the virtual navigator as the center; 3) The swarm automatically avoids obstacles when encountering them; 4) After passing the obstacle range, the swarm gathers toward the center again and restores the formation.

[0124] (2) Cluster software simulation system

[0125] For the cluster control and obstacle avoidance algorithm proposed in this project, we first conduct research and software simulation verification on the cluster simulation system. The cluster simulation system designed in this project is as follows: Figure 8 As shown:

[0126] The system mainly includes cluster simulation software, which consists of six parts: drone module, flight control module, control module, sensor module, environment module and rendering module.

[0127] ① UAV module: Perform dynamic and kinematic calculations based on the flight control control signal and the current state information, and output the state information at the next moment;

[0128] ②Flight control module: software simulation of the flight controller, which decomposes the control instructions of the control module into control signals and transmits them to the drone module for position and attitude control;

[0129] ③Sensor module: Contains mathematical models of sensors such as accelerometers, and updates sensor data during the flight of the drone;

[0130] ④Control module: provides the sensor data acquisition interface and single-machine control interface of the drone. The cluster formation and obstacle avoidance algorithm obtains the cluster status data through this interface, and derives the control instructions of each drone in the cluster according to the algorithm to achieve closed-loop control;

[0131] ⑤Environment module: weather environment control module, to realize the weather changes in the virtual environment;

[0132] ⑥Rendering engine module: real-time rendering of objects, textures, lighting and other information in the virtual environment.

[0133] (3) Cluster semi-physical simulation system

[0134] After software simulation verification, a semi-physical simulation system was built to simulate the formation and obstacle avoidance of drone swarms in real scenarios as much as possible. Fig. 9 As shown in the figure, the effectiveness and reliability of the algorithm are further verified.

[0135] In addition to the virtual state data of drones provided by the cluster simulation software, the system also uses hardware equipment used in the physical verification test, including flight control, airborne computer and data transmission. The cluster control and obstacle avoidance algorithms are integrated in the airborne computer. The airborne computer combines the visual sensor data obtained in the cluster simulation software, the state data of other drones obtained from the data link and its own posture data to perform cluster control and obstacle avoidance calculations, and sends the control instructions to the flight control to calculate as control signals, which are then transmitted to the corresponding drones in the cluster simulation software to achieve closed-loop control.

[0136] Each set of hardware equipment in the semi-physical simulation system corresponds to a UAV in the cluster simulation software. Except for state solution, all software runs on the hardware equipment, and the communication between UAVs uses real data links. Compared with software simulation, it can verify the algorithm more effectively and provide a reference basis for physical demonstration verification.

[0137] Compared with other similar methods, the features of this technical solution are as follows: ① The traditional obstacle avoidance solution for drones is to achieve all-round obstacle perception by carrying multiple sensors, which will cause problems such as complexity and weight of the aircraft. This project uses a cluster control algorithm to achieve a large-scale obstacle perception capability of the cluster while carrying a small number of visual sensors on a single aircraft, effectively reducing the size and weight restrictions of the aircraft platform; ② This project proposes a control algorithm for drone clusters to adaptively change the cluster formation according to the distribution of obstacles in the environment, meeting the flight requirements of the cluster in complex environments.

[0138] Those skilled in the art will appreciate that all or part of the processes of the above-mentioned embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, wherein the computer-readable storage medium is a disk, an optical disk, a read-only storage memory, or a random access memory, etc.

[0139] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A method for controlling and avoiding obstacles of drone swarms, characterized in that: include: When some drones in the drone cluster sense an obstacle, each drone in the drone cluster locates the obstacle according to its own positioning and the obstacle perception result and broadcasts the obstacle position in the drone cluster; The drone cluster adaptively changes according to the position of the obstacle and autonomously adjusts the heading of the drone itself, so that the obstacle always remains within the sight range of the drone cluster; as well as After the drone cluster passes the obstacle, the tail of the drone cluster keeps facing the obstacle to ensure that the drone cluster as a whole is out of the obstacle range. The cluster control algorithm is used to perform cluster control on the drone cluster randomly distributed at the initial position to achieve: the distances between the drones in the drone cluster tend to be consistent; the drone cluster moves in the desired direction with the virtual navigator as the center; the drone cluster automatically avoids obstacles when encountering obstacles; after passing the obstacle range, the cluster regroups to the center and restores the formation; The cluster control algorithm includes: i Control input for a drone u i and w i , where based on the control input u i Control the relative position and speed of the drones in the drone cluster based on the control input w i Controlling the sight angle distribution of the drones in the drone cluster; Among them, the overall potential energy of the drone cluster is expressed by the following formula: V ( q ) Determine the i Control input for a drone u i : ； in, q is the set of positions of all drones in the drone cluster, is the current distance z To the expected distance d α The potential energy integral is: ； For the attraction between two drones: ； ； ； in, a , b , c , h is the curve parameter, is the sensing distance of the drone, due to the norm exist is not differentiable at Rewrite it as a norm that is differentiable everywhere , norm Used to construct smooth artificial potential field functions, is the norm parameter.

2. The drone cluster control and obstacle avoidance method according to claim 1, characterized in that: The adaptive change of the drone cluster according to the obstacle position includes the adaptive change of the drone cluster according to the aggregation criterion, the separation criterion and the speed matching criterion according to the obstacle position, wherein: The aggregation criterion is that each drone in the drone cluster obtains the position information of other drones in the neighborhood, calculates the expected state offset based on its own position and the control algorithm, and feeds it back to its own controller, so that the current drone is closer and closer to the center position of the drone cluster to achieve an aggregated state; The separation criterion obtains the distance offset between the position of each drone and the positions of other drones in the neighborhood, and feeds the distance offset back to the controller. When the distance offset is less than the safe distance between drones, the controller generates a repulsive force inversely proportional to the distance offset. The existence of the repulsive force causes the drone cluster to separate during movement. According to the speed matching criterion, each UAV obtains the speed information of other UAVs in the neighborhood in real time, compares the speed of other UAVs with the speed of the current UAV to obtain a speed offset, and feeds the speed offset back to its own controller for speed adjustment, so that the speed size and direction of all UAVs in the UAV cluster are consistent.

3. The drone cluster control and obstacle avoidance method according to claim 1, characterized in that: The drone cluster autonomously adjusts the drone's own heading according to the obstacle position, including the drone cluster adjusting the drone's own heading according to the obstacle position according to the viewing angle distribution criterion, wherein: The viewing angle distribution criterion is that each drone in the drone cluster adjusts its orientation so that the viewing angle of the drone cluster is evenly distributed with the drone cluster as the center, and obtains the positions and orientations of other drones in the neighborhood and feeds back the desired heading angle to its own controller, so that compared with the drone cluster whose viewing angle is set to be evenly distributed, the drone cluster with evenly distributed viewing angles obtains a larger viewing range.

4. The drone cluster control and obstacle avoidance method according to claim 1, characterized in that: The UAV airborne visual sensor is fixed on the body, and the following formula is used to express the first i Control input for a drone w i : ； in, For drones i The heading control gain, For the i The heading angle of the drone, N i For the i The neighborhood of drones ； i and j Number the drone. r The sensing radius of the drone.

5. The drone cluster control and obstacle avoidance method according to claim 1, characterized in that: The following formula is used to express the i Control input for a drone For the i UAV formation control input , drones i Virtual navigator feedback control input With drones i The virtual agent feedback item controls the input sum: ； ； ； ； in, Indicates drone i and j Whether it was connected before, 0 means not connected, 1 means connected, For drones i Pointing drone j The direction vector of is the position vector of the virtual navigator, is the velocity vector of the virtual navigator, and are the position and speed parameters of the virtual navigator, is a set of virtual agents, , and Respectively j The position vector and velocity vector of the virtual agent and their relationship with the drone i The connectivity situation, For drones i Pointing to Virtual Agent j The direction vector of .

6. The drone cluster control and obstacle avoidance method according to claim 5, characterized in that: The virtual intelligent body is generated at the point where the obstacle is closest to the drone. When the virtual intelligent body is within a safe distance of the drone, a repulsive force is generated on the drone, and the closer the distance, the greater the repulsive force. When the drone approaches an obstacle, it will slow down under the influence of the repulsive force and move away from the obstacle. When the drone moves away from the virtual intelligent body under the action of the repulsive force, it will automatically avoid the obstacle until the obstacle is at a safe distance.

7. A device using the drone cluster control and obstacle avoidance method according to any one of claims 1 to 6, characterized in that: include: The obstacle data acquisition module is used for locating the obstacle position according to its own positioning and the obstacle perception result when some drones in the drone cluster perceive an obstacle; A data transmission module, used for broadcasting the obstacle position in the drone cluster; An adjustment module, used to adaptively change the position of the obstacle through the drone cluster and autonomously adjust the heading of the drone itself, so that the obstacle always remains within the sight range of the drone cluster; as well as A post-processing module is used to ensure that the tail of the drone cluster remains facing the obstacle after the drone cluster passes through the obstacle, so as to ensure that the drone cluster as a whole is out of the obstacle range.

Images