An unmanned aerial vehicle cluster path planning method based on a particle swarm algorithm

Abstract

The application discloses a kind of unmanned aerial vehicle cluster cooperative path planning methods based on particle swarm algorithm, including the following steps: unmanned aerial vehicle cluster obtains a group of starting point under street scene, terminal and obstacle information, establishes two-dimensional grid chart;Global initial path of each unmanned aerial vehicle in unmanned aerial vehicle cluster is generated using particle swarm algorithm;Space-time conflict point existing in the initial path of each unmanned aerial vehicle is detected, and conflict resolution is carried out by obtaining conflict avoidance point on the velocity circle in a specific direction;The path of unmanned aerial vehicle cluster without space-time conflict is obtained, and the path is transmitted to unity3D simulation platform in network communication udp mode for unmanned aerial vehicle cluster flight verification;The application can provide a high safety factor, good smoothness, satisfy the cooperation of unmanned aerial vehicle cluster, effectively complete the conflict resolution path solution.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of multi-unmanned aerial vehicle (UAV) cooperative planning, and particularly relates to a UAV cluster path planning method based on a particle swarm algorithm. BACKGROUND

[0002] In recent years, UAVs are widely used due to their small size, good concealment, easy maintenance, low deployment cost and other characteristics. With the continuous progress of chip research, material manufacturing, big data and other technologies, UAVs gradually develop towards intelligence and clustering. UAV clusters have advantages such as strong anti-destruction reconstruction, high distribution collaboration and significant economic benefits. After the clustering of UAVs, the main research directions include cooperative task allocation, cooperative path planning and cluster formation. Among them, cooperative path planning is the basis and premise for UAV clusters to perform tasks. Multi-UAV path planning is to plan the flight path of each UAV from the initial point to the target point according to the specific task of each UAV, under the premise of meeting various constraints (platform performance constraints, environmental constraints, task collaboration constraints), so that each UAV reaches the optimal or better performance under the specified performance index.

[0003] At present, the UAV cluster path planning in complex two-dimensional dynamic scenes is still a research difficulty. Based on traditional algorithms such as A* algorithm, the scene is rasterized, and then a point search algorithm is used to take each discrete point. The path planned is short and the planning time is short, but this is at the expense of smoothness and safety. The planned path is not suitable for UAV flight, and there is a tangent condition with obstacles. As an important algorithm in the classification of swarm optimization algorithms, the particle swarm algorithm has the advantages of simple implementation and few adjustable parameters, and is widely used in various optimization scenarios. It can also be well applied to path planning. Due to the diversity of particle populations, the final solution also shows diversity, and more feasible paths can be found. However, when facing UAV clusters, the particle swarm algorithm cannot well plan smooth and collision-free cluster trajectories. SUMMARY

[0004] Therefore, in order to overcome the shortcomings of the prior art, the purpose of the present application is to provide a UAV cluster path planning method based on a particle swarm algorithm, so as to obtain a smooth and safe path.

[0005] In order to achieve the above purpose, the technical scheme adopted by the present application is:

[0006] A UAV cluster path planning method based on a particle swarm algorithm, comprising the following steps:

[0007] Step 1: The UAV cluster obtains the obstacle position and volume information in the target scene, and the corresponding start and end point information of each UAV in the UAV cluster, establishes a corresponding environment two-dimensional grid map, and models the UAV as a movable circle, which is the circumscribed circle of the UAV;

[0008] Step 2: A particle swarm algorithm is used to generate the global flight path of each UAV in the UAV cluster; each UAV corresponds to a particle population, and each particle in the particle population plans a flight path. The UAV takes the optimal flight path in the corresponding particle population as the initial flight path;

[0009] Step 3: The initial flight path is segmented and smoothed by using the segmented Bezier curve method. The priority of each UAV in the UAV cluster is set according to the curve length of the entire flight path. The shorter the length, the lower the priority set;

[0010] Step 4: Detect the conflict position existing in the multiple UAV flight paths, find the avoidance position on the avoidance circle of the UAV, replace the detected conflict position, and complete the conflict resolution; the conflict refers to the distance between the centers of mass of two UAVs at the same time being less than the safety distance, and the conflict position is the intersection region of the circumscribed circles of the two UAVs; the safety distance is the sum of the radii of the circumscribed circles of the two UAVs;

[0011] Step 5: Obtain the new UAV cluster flight path, and verify the safety and time efficiency of the flight path on a simulation platform.

[0012] In one embodiment, the step 1 models the target scene as a k*k grid map, with white areas representing the feasible region of the UAV cluster and black areas representing the obstacle region; thereby representing the scene information with a k*k two-dimensional matrix, with 1 representing the obstacle region and 0 representing the feasible region; the UAV cluster consists of multiple UAVs, corresponding to a set of start points and a set of end points.

[0013] In one embodiment, the step 2 initializes the number of particle populations to M, and sets the velocity and position ranges of the particles as follows: min ≤v id ≤v max , x min ≤x id ≤x max , y min ≤y id ≤y max , where v min represents the lower limit of the velocity, which is 0, v max represents the upper limit of the velocity, (x min , y min ) represents the lower limit of the particle position corresponding to the horizontal and vertical coordinates, and x min and y minAll values are 0, (x max ,y max ) represents the upper limit of the horizontal and vertical coordinates corresponding to the particle position; v id represents the current speed of the particle id, (x id ,y id ) represents the current position of the particle id;

[0014] The velocity update formula of the standard particle swarm algorithm is:

[0015] v id (t+1)=w×v id (t)+c1r1(pbest id (t)-x id (t))+c2r2(gbest id (t)-x id (t))

[0016] Where w represents the inertia weight factor, which is set as a variable that linearly decreases during iteration; v id (t) represents the speed of the particle id at time t, x id (t) represents the position of the particle id at time t, c1 represents the learning factor of self-cognition part, c2 represents the learning factor of social cognition part, r1 and r2 represent random numbers between 0 and 1;

[0017] The position update formula of the standard particle swarm algorithm is:

[0018] x id (t+1)=v id (t+1)+x id (t)

[0019] The position is updated according to the speed, and then the corresponding fitness function fitness(t) is set, which should include three parts: the distance function f1(t) of the current position of the unmanned aerial vehicle to the target point, the distance function f2(t) of the current position of the unmanned aerial vehicle to the obstacle, and the function f3(t) corresponding to the smoothness of the trajectory of the unmanned aerial vehicle, the formula is as follows:

[0020] fitness(t)=αf1(t)+βf2(t)+ηf3(t)

[0021]

[0022]

[0023]

[0024] The value of f1(t) represents the distance from the current position of the particle to the target point, the value of f2(t) represents the size of the distance from the current position of the particle to the obstacle, and the value of f3(t) represents the deflection angle between the last flight path of the unmanned aerial vehicle and the next flight path, and a, β and η are proportional factors with values between 0 and 1; obstacle represents the centroid position of the obstacle, x t represents the position of the particle at time t, x t-1 represents the position of the particle at time t-1, x t-2 represents the position of the particle at time t-2.

[0025] In one embodiment, the expression of the step 3, the Bezier curve is as follows:

[0026]

[0027]

[0028] where P i is the control point of the Bezier curve corresponding to the flight path of the unmanned aerial vehicle, is the Bernstein polynomial; i represents the order variable between 0 and n, n represents the number of control points, that is, the order of the Bezier curve, and t represents the time variable between 0 and 1;

[0029] The segmented Bezier curve method, for each unmanned aerial vehicle, takes N flight path points as a group to form a flight path, and performs smoothing processing on the flight path to obtain a smooth flight path of each unmanned aerial vehicle.

[0030] In one embodiment, the step 4, when the next time position is detected as a conflict position, the party with low priority is the avoiding party, and the party with high priority maintains the original flight path. Within the cluster task time, the unmanned aerial vehicle with low priority ensures to reach the target point within the specified time and complete the conflict avoidance.

[0031] In one embodiment, the speed circle of the avoiding party is a circle with the current position of the avoiding party as the center and the distance from the current position to the next time position as the radius r. The avoiding position is searched on the circumference, that is, the conflict flight path point is avoided. The direction is the direction pointed by the sum of the opposite direction vector of the speed direction of the unmanned aerial vehicle in conflict and the speed direction vector of the avoiding party itself. The calculation formula of the corresponding new position (x new ,y new ) is as follows:

[0032]

[0033]

[0034] where (x cur ,ycur represents the current horizontal and vertical coordinates of the unmanned aerial vehicle, and angle represents the angle that the direction of the velocity needs to be adjusted, which is obtained by the following formula,

[0035] vector1 = (x next -x cur ,y next -y cur )

[0036] vector2 = -(x o,next -x o,cur ,y o,next -y o,cur )

[0037]

[0038] In the formula, (x cur ,y cur ) represents the current position of the evader, (x next ,y next ) represents the position of the evader at the next moment, i.e. the position of the collision, vector1 represents the velocity vector of the evader, vector2 represents the opposite direction vector of the velocity vector of the other unmanned aerial vehicle in conflict, (x o,next ,y o,next ) and (x o,cur ,y o,cur ) represent the position of the other unmanned aerial vehicle in conflict at the next moment and the current moment respectively, and finally the evading position of the unmanned aerial vehicle is obtained, and the evading position is used to replace the original position to realize conflict resolution.

[0039] Compared with the prior art, the beneficial effects of the present application are:

[0040] 1. The particle swarm algorithm is used to plan the initialization track of the unmanned aerial vehicle cluster, considering the size of the unmanned aerial vehicle, the length of the track, the dynamic constraints and many other factors, and in the dense obstacle scene, an absolutely safe track is obtained.

[0041] 2. The segmented Bezier curve retains the safety of the unmanned aerial vehicle track, and at the same time, the unmanned aerial vehicle track is smoothed to obtain a smooth and safe track.

[0042] 3. The conflict resolution method can effectively handle the conflict problem in this scene, and completes the cooperative path planning of the unmanned aerial vehicle cluster. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 It is the overall flowchart of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0044] Figure 2It is a Unity three-dimensional simulation scene corresponding to the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0045] Figure 3 It is a two-dimensional grid environment map of the modeling of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0046] Figure 4 It is a flowchart of step 2 about planning the initial path of the unmanned aerial vehicle cluster by the particle swarm algorithm of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0047] Figure 5a It is an effect diagram of optimizing the overall path by using the Bezier curve of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0048] Figure 5b It is an effect diagram of segmenting and smoothing the path by using the Bezier curve of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0049] Figure 6 It is a schematic diagram of the conflict of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0050] Figure 7 It is a conflict avoidance principle diagram of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0051] Figure 8a It is a two-dimensional simulation diagram of the unmanned aerial vehicle cluster cooperative path planning based on the particle swarm algorithm.

[0052] Figure 8b It is a top view of the three-dimensional simulation experimental verification of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm.

[0053] Figure 8c It is a side view of the three-dimensional simulation experimental verification of the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm. DETAILED DESCRIPTION

[0054] In order to make the purpose, technical scheme and advantages of the present application clearer, the embodiments of the present application will be further described below with reference to the drawings.

[0055] As Figure 1 shown, the unmanned aerial vehicle cluster path planning method based on the particle swarm algorithm includes the following steps:

[0056] Step 1: The UAV cluster obtains the information of the obstacle position, volume and the like in the target scene, and the information of the corresponding start point and end point coordinates of each UAV in the UAV cluster, and establishes a corresponding environment two-dimensional grid map. And the UAV is modeled as a movable circle, which is the circumscribed circle of the UAV.

[0057] In the present application, the UAV cluster is composed of multiple UAVs, so there is a group of start points and a group of end points. The target scene is modeled as a k*k grid map in the present application, so the scene information is represented by a two-dimensional matrix of k*k size, and 1 represents the obstacle region and 0 represents the feasible region.

[0058] In an embodiment of the present application, the target scene is selected as a street, the area size of the Unity street scene is 50m*50m, and there are some buildings distributed, and the obstacles are mainly four-sided obstacles similar to the buildings. The feasible region of the UAV is the region that is not covered by the obstacles, as shown in the specific drawing. Figure 2 From the top view, each building can be regarded as being composed of a plurality of squares, and there are many feasible streets that are not covered by obstacles, so the scene can be modeled as a 50*50 grid map, and in the two-dimensional grid map, the obstacle region is modeled as a black square, and the feasible region of the UAV cluster is a white region, as shown in the specific drawing. Figure 3 Mathematically, the scene information can be represented by a two-dimensional matrix of 50*50 size, 1 representing the obstacle region and 0 representing the feasible region. The UAV has a certain size and volume, so in this embodiment, the UAV is modeled as a movable circular region with a radius r, and for example, r=0.6.

[0059] Step 2: The particle swarm algorithm is used to generate the global flight path of each UAV in the UAV cluster. Each UAV corresponds to a particle swarm, and each particle in the particle swarm plans a flight path. The UAV takes the optimal flight path in the corresponding particle swarm as its flight path.

[0060] In an embodiment of the present application, the speed of each particle in the randomly initialized particle swarm is calculated, the initial position of the swarm particle is set at the start point, and the cycle is entered; the fitness function value of each particle is calculated, the historical optimal value pbest id (t) of each particle is obtained according to comparison, and then the minimum fitness function value in this generation is found as the optimal value gbest id(t), the velocity and position of the particle are updated according to the velocity and position update formula, the position of the particle in each generation is saved, that is, a track point, it is judged whether the position reaches the target point and the fitness function value is less than the threshold value or the iteration number reaches the maximum iteration number, and any one of the two conditions is met, and the loop is exited; the track points of one particle are connected to form a polyline, that is, the track of one particle; in one particle population, a plurality of particles are contained, thereby corresponding to a plurality of tracks, and the optimal track in each particle population is the global track of each unmanned aerial vehicle.

[0061] The flow of global track planning of the application by using the particle swarm algorithm can refer to Figure 4 The particle swarm algorithm takes the two-dimensional grid map modeled in step 1 as the environment of the particle search point. Firstly, the range of the particle selecting the track point needs to be limited, the number of the particle population is initialized as M, and the position range of the particle is set, and the corresponding constraint is:

[0062] x min ≤x id ≤x max

[0063] y min ≤y id ≤y max

[0064] Wherein (x min ,y min ) represents the lower limit of the horizontal and vertical coordinates corresponding to the position of the particle, (x max ,y max ) represents the upper limit of the horizontal and vertical coordinates corresponding to the position of the particle; v id represents the current speed of the particle id, (x id ,y id ) represents the current position of the particle id.

[0065] Since the embodiment is to find the track point on the 50m*50m grid map, x min is 0, x max is 50, y min is 0, and y max is 50.

[0066] The upper and lower limits of the speed of the particle also need to be set to avoid too large difference between the positions of the previous and next generations, and the corresponding constraint is:

[0067] v min ≤v id ≤v max

[0068] Wherein v min represents the lower limit of the speed, v max represents the upper limit of the speed, and in the embodiment, v minis set to 0, v max is set to 4.

[0069] The velocity and position update formula of the standard particle swarm algorithm is:

[0070] v id (t+1) = w x v id (t) + c1 r1 (pbest id (t) - x id (t)) + c2 r2 (gbest id (t) - x id (t))

[0071] x id (t+1) = v id (t+1) + x id (t)

[0072] where w represents the inertia weight factor, the greater the value, the better the global optimization ability of the particle, the faster the convergence, but there are also search rough, not easy to find accurate solution and other problems, the smaller the value of w, the better the local optimization ability of the particle, can find accurate solution, but slow convergence. Considering that the particle needs to explore a wide range in the early iterations, it should have a good global optimization ability, and in the later iterations, it needs to find accurate solutions, i.e. it needs good local optimization ability. Therefore, the inertia weight factor w is set to a variable that decreases linearly during iteration, i.e. a quantity that decreases linearly with the increase of the number of iterations, ensuring the global optimization ability of the initial large-scale search and the local optimization ability of the accurate solution in the later period. The specific formula is as follows:

[0073]

[0074] where w max represents the upper limit of the inertia weight factor, generally taken as 0.9, w min represents the lower limit of the inertia weight factor, generally taken as 0.4, G max represents the maximum number of iterations, and g represents the current number of iterations. In the velocity update formula, c1 represents the learning factor of the self-cognition part, and c2 represents the learning factor of the social cognition part. Studies have shown that when c1 + c2 ≤ 4, better convergence results can usually be obtained; r1 and r2 are any random numbers between 0 and 1; v id (t) represents the speed of particle id at time t, and x id (t) represents the position of particle id at time t.

[0075] The velocity update formula includes three parts: the inertia term for the previous speed, the self-cognition term, and the group cognition term. It shows that the current speed of the particle is not only affected by the previous generation and the historical optimal value of itself, but also affected by the optimal value of the entire population.

[0076] The present application updates the position according to the speed and sets a corresponding fitness function fitness(t). The fitness function fitness(t) set by the present application should include three parts: a distance function f1(t) of the current position of the UAV to the target point, a distance function f2(t) of the current position of the UAV to the obstacle, and a function f3(t) corresponding to the smoothness of the trajectory of the UAV. The optimization purpose of the particle swarm algorithm is to make the fitness function value reach a minimum value.

[0077] fitness(t) = αf1(t) + βf2(t) + ηf3(t)

[0078] α, β and η are proportional factors.

[0079] f1(t) is a distance function related to the target, which provides a guide for the particle to continuously approach the target point, so that the function value becomes smaller. The value represents the distance from the current position of the particle to the target point. The specific formula is:

[0080]

[0081] f2(t) is a function related to obstacle avoidance, which ensures that the track point selected by the particle is far away from the centroid of the obstacle. The value represents the distance from the current position of the particle to the obstacle. The specific formula is:

[0082]

[0083] f3(t) is a function related to the deflection angle of the track, which ensures that the particle does not have too large a deflection angle when selecting a track point, so as to ensure that the overall track is in one direction as much as possible. The value represents the deflection angle from the previous track of the UAV to the next track. The specific formula is:

[0084]

[0085] In the formula, α, β and η are proportional factors with values between 0 and 1; obstacle represents the centroid position of the obstacle, x t represents the position of the particle at time t, x t-1 represents the position of the particle at time t-1, x t-2 represents the position of the particle at time t-2.

[0086] Experiments show that a population size of 20 can find more feasible tracks. In the embodiment of the present application, the total process of step 2 is: first, set the population size to 20, and then randomly initialize the initial speed of each particle in the particle population in the range of [0, v max ] and set the position of the particle population at the starting point. The historical optimal value pbest id(t) The fitness function value calculated at the starting position is taken as the initial value, and the optimal value gbest of the current generation of the population id (t) is set to infinity. After initialization, the loop is entered, and the fitness function value of each particle is calculated. If the current fitness function value of the particle is less than the historical optimal value pbest id (t) of the particle, the historical optimal value pbest id (t) of each particle is updated, the minimum value of the fitness function in the current generation of the population is found as the global optimal value gbest id (t), and then pbest id (t) and gbest id (t) are brought into the speed formula to update the speed and position of the particle, and the position of each generation is saved. The position of each particle in each generation corresponds to a waypoint of the flight path. In each generation, it is judged whether the position reaches the target point or the number of iterations reaches the maximum number of iterations. If either of the two conditions is met, the loop is exited. Then it is judged whether the number of iterations is equal to the maximum number of iterations and the distance between the last generation of the particle and the target point is less than 1. If so, it means that the target point has been found, i.e. a flight path has been found. If the number of iterations is equal to the maximum number of iterations and the last generation of the particle is far away from the target point, it means that the target point has not been found, and the algorithm needs to be run again. Experiments show that when the population size is greater than 20 and the learning factors c1 and c2 satisfy the conditions c1≤c2 and c1+c2≤4, a collision-free flight path from the starting point to the end point can always be found. c1≤c2 is because in a dense obstacle scene, the particle needs to accept more guidance from the particle population to avoid being trapped in a local optimum by following its own speed and self-awareness, i.e. the particle is stuck in a certain obstacle corner of the scene.

[0087] Step 3: The segmented Bezier curve method is used to segment and smooth the flight path. After step 2, a series of waypoints can be obtained, and these points can be connected to form a broken line, which is a particle flight path. Such a flight path is not smooth enough and has a large deflection angle, which does not meet the constraints of unmanned aerial vehicle dynamics. Therefore, after obtaining the initial flight path planned by the particle swarm optimization algorithm in step 2, the Bezier curve is used to smooth the flight path. The priority of each unmanned aerial vehicle in the unmanned aerial vehicle swarm is set according to the curve length of the entire flight path. The shorter the length, the lower the priority.

[0088] The Bezier curve is derived from the Bernstein formula. The corresponding formula of the n-order Bezier curve is as follows:

[0089]

[0090]

[0091] where P icontrol points of a Bezier curve corresponding to a UAV flight path, Bernstein polynomial, i represents a degree variable between 0-n, n represents the number of control points, that is, the order of the Bezier curve, and t represents a time variable between 0-1.

[0092] The Bezier curve is derived from the Bernstein polynomial and is a curve for global optimization, and the control points in the middle generally do not lie on the curve except that the start point and the end point coincide with the first control point and the last control point. The order of the Bezier curve is related to the number of control points. A first-order Bezier curve is controlled by two points and is a straight line, and a second-order Bezier curve is controlled by three points and is a parabola. Therefore, the order of the Bezier curve is determined by the number of path points possessed by the flight path.

[0093] Experiments show that, in the dense obstacle scene provided by the present application, if the path points of the whole flight path are taken as the control points of the Bezier curve, a Bezier curve fitting the whole flight path is obtained, and there is a tangent condition with an obstacle at a certain angle, as shown in Figure 5a . Figure 5a The area marked by the middle circle is a dangerous area for the UAV flight, and in the simulation experiment, the wings of the UAV will collide with the obstacles, and the safety cannot be guaranteed. The reason is that the initial flight path planned by the particle swarm algorithm takes into account the size of the UAV, but when the Bezier curve is smoothed, the curve is required to contain the smallest polygon of all the vertices, and the control points are not arranged in order, so especially at the corner, the Bezier curve may cross the obstacle in order to contain all the path points.

[0094] In order to ensure the safety of the UAV flight path, the present application adopts a segmented Bezier curve method for segmented optimization, divides the whole flight path into multiple segments, and fits each segment with a Bezier curve. The advantage of this is that the span of each segment is relatively small, the number of corners is small, and the reduction of control points also reduces the order of the Bezier curve, which is more stable than the high-order Bezier curve, and the tangent condition with the obstacle is reduced, and of course the smoothness is also reduced.

[0095] That is, the segmented Bezier curve method of the present application takes N path points as a group for each UAV to form a segment of the flight path, and smoothes the segment of the flight path to obtain a smoothed flight path for each UAV. In this embodiment, 20 path points are taken as a group for smoothing the segment of the flight path, which can effectively solve Figure 5a the collision with the wall, and finally ensures the safety and smoothness of the UAV flight path, Figure 5b which is the result of segmenting the flight path with the Bezier curve. It can be seen that each flight path does not have a tangent point with the obstacle.

[0096] At the same time, since the Bezier curve is also a function of time, the position points can be effectively combined with the time parameter to specify that the UAV flies the entire flight path within a fixed time. After obtaining the smooth flight path of each UAV, the priority of each UAV in the cluster is set according to the length of the entire flight path curve. The shorter the flight path length, the shorter the time to complete the flight task, and there is redundant time to avoid conflicts, so a lower priority is set.

[0097] Step 4: Detect the conflict points existing in the multiple UAV flight paths, find the avoidance position on the avoidance circle of the UAV, replace the conflict position detected, i.e., the original position coordinates of the conflict, to complete the conflict resolution.

[0098] After the processing of steps 3 and 4, the initial flight path obtained satisfies safety and smoothness, but as the number of UAVs increases in the entire region, it is impossible to guarantee that the UAVs do not collide during flight. Therefore, conflict detection and resolution are required. In the present application, a conflict refers to a situation where the distance between the centers of mass of two UAVs is less than the safety distance at the same time. Therefore, the conflict position is not a specific point, but a region where the excircles of the two UAVs intersect, as shown in the schematic. Figure 6 Similarly, since the present application models the UAV as a circle, the safety distance is the sum of the radii of the excircles of the two UAVs.

[0099] Before conflict resolution, the priority of the UAVs in the cluster needs to be specified, and the UAV flight path points that need to be adjusted are selected according to the priority. The basis for setting the priority of the UAVs is that the shorter the total flight path length, the lower the priority. Within the cluster task time, the UAV with low priority can ensure that it reaches the target point within the specified time after conflict avoidance. That is, when it is detected that the next time position is a conflict position, the party with low priority is the avoidance party, and the party with high priority maintains the original flight path. Within the cluster task time, the UAV with low priority ensures that it reaches the target point within the specified time and completes conflict avoidance.

[0100] The specific operation of conflict resolution is shown in Figure 7 Assuming that the avoidance UAV has a lower priority than the other UAV, the two UAVs predict that they will collide at the next time, and the avoidance UAV needs to take avoidance action in advance. Taking the position of the avoidance UAV at the current time as the center and the distance from the current position to the next position as the radius r to draw a circle, which is the speed circle of the avoidance UAV. Find the avoidance position on the circumference, i.e., the flight path point that avoids the conflict, according to the direction of the sum of the opposite direction vector of the speed direction of the UAV with which it conflicts and its own speed direction vector. The calculation formula of the corresponding new position coordinates (x new ,y new ) is as follows:

[0101]

[0102]

[0103] wherein (x cur ,y cur ) represents the current position of the UAV to be completed the avoidance action, r represents the distance between the next position and the current position of the UAV, i.e. the speed circle. angle represents the angle that the speed direction of the UAV needs to adjust, which is determined by the included angle between the speed vector of the avoidance party (i.e. the sum of the own speed direction vector and the corresponding direction vector) vector1 and the opposite direction vector of the speed vector of the other UAV in conflict vector2, and the corresponding formula is:

[0104] vector1 = (x next -x cur ,y next -y cur )

[0105] vector2 = -(x o,next -x o,cur ,y o,next -y o,cur )

[0106]

[0107] wherein (x cur ,y cur ) represents the current position of the avoidance UAV, (x next ,y next ) represents the next position of the avoidance UAV, vector1 represents the speed vector of the avoidance UAV. (x o,next ,y o,next ) and (x o,cur ,y o,cur ) respectively represent the next position and the current position of the other UAV in conflict, and vector2 represents the opposite direction of the speed vector of the avoidance UAV. Finally, through the formula calculation, the avoidance position coordinates (x new ,y new ) of the UAV can be obtained, and the avoidance position is used to replace the original position to realize the conflict resolution.

[0108] Step 5: Obtain the new UAV cluster track, and verify the safety and time efficiency of the track on the simulation platform.

[0109] In the embodiment of the present application, the unmanned aerial vehicle simulation experiment is carried out on the simulation platform unity3D to verify whether the unmanned aerial vehicle can avoid static obstacles and other unmanned aerial vehicles in flight and reach the target point within a limited time. After thousands of unmanned aerial vehicle flight experiments, the present application can effectively guide the unmanned aerial vehicle cluster to complete the multi-target task, and the intermediate process can successfully avoid obstacles and not collide with other unmanned aerial vehicles.

[0110] Specifically, the present application first plans the flight path of the unmanned aerial vehicle cluster to complete steps 2-4 on a two-dimensional platform, and then sends the flight path in the mavlink2.0 format to the unity3D simulation platform through the udp network communication method to verify whether the unmanned aerial vehicle cluster will collide during flight. Figure 8a 、 Figure 8b and Figure 8c The flight path planned in a certain experiment is displayed on a two-dimensional plane, and the flight top view and side view of the unmanned aerial vehicle cluster in a three-dimensional plane are listed. After thousands of simulation flight experiments, the unmanned aerial vehicle cluster does not collide during flight, and can complete the multi-target task within an average of 1 minute in a 50m*50m range. It shows that the unmanned aerial vehicle cluster cooperative path planning method based on the particle swarm algorithm proposed in the present application can effectively plan a safe and smooth flight path for the unmanned aerial vehicle cluster, and can effectively complete conflict resolution.

[0111] In summary, the present application considers safety, flight path length, inter-vehicle distance, unmanned aerial vehicle modeling size and many other factors to ensure that the initial flight path planned by the particle swarm algorithm is absolutely safe. At the same time, the whole flight path is segmented and smoothed using a Bezier curve to make it more consistent with the dynamics constraints of the unmanned aerial vehicle, and the flight path of the unmanned aerial vehicle cluster is detected for conflicts, and the conflicts are resolved for the conflict positions. The unmanned aerial vehicles in the unmanned aerial vehicle cluster can all fly without collision, safely reach the specified target, and complete the multi-target task within the specified time. The present application can provide a flight path solution that is safe, smooth, meets the cooperative requirements of the unmanned aerial vehicle cluster, and effectively resolves conflicts.

[0112] The specific embodiments described above are only used to describe the preferred mode of the present application, and do not limit the scope of the present application. Without departing from the design spirit of the present application, various modifications and improvements to the technical solutions of the present application made by those skilled in the art shall fall within the protection scope determined by the claims of the present application.

Claims

1. A method for unmanned aerial vehicle swarm path planning based on particle swarm optimization, characterized in that, The method comprises the following steps: Step 1: The UAV cluster obtains the obstacle position and volume information in the target scene, and the corresponding start point and end point information of each UAV in the UAV cluster, establishes a corresponding environment two-dimensional grid map, and models the UAV as a movable circle, which is the circumscribed circle of the UAV; Step 2: A particle swarm algorithm is used to generate the global flight path of each UAV in the UAV cluster; each UAV corresponds to a particle population, and each particle in the particle population plans a flight path, and the UAV takes the optimal flight path in the corresponding particle population as the initial flight path; Step 3: A segmented Bezier curve method is used to segment and smooth the initial flight path, and the priority of each UAV in the UAV cluster is set according to the curve length of the whole flight path; the shorter the length, the lower the priority; Step 4: The conflict position existing in the multiple UAV flight paths is detected, an avoidance position is found on the avoidance circle of the UAV, the conflict position detected is replaced, and conflict resolution is completed; the conflict refers to the distance between the centers of mass of two UAVs at the same time being less than a safety distance, and the conflict position is the intersection region of the circumscribed circles of the two UAVs; the safety distance is the sum of the radii of the circumscribed circles of the two UAVs; Step 5: A new UAV cluster flight path is obtained, and the safety and time efficiency of the flight path are verified on a simulation platform; The step 4, when detecting that the next moment position is a conflict position, the party with low priority is the avoiding party, and the party with high priority keeps the original flight path, and the unmanned aerial vehicle with low priority ensures to reach the target point within the specified time and complete conflict avoidance within the cluster task time; the speed circle of the avoiding party is a circle with the current position of the avoiding party as the center and the distance from the current position to the next moment position as the radius , and the avoiding position is searched on the circumference, that is, the flight path point avoiding the conflict; the direction according to the calculation formula of the corresponding new position is the direction in which the sum of the opposite direction vector of the speed direction of the unmanned aerial vehicle in conflict and the speed direction vector of the unmanned aerial vehicle itself points , and the calculation formula is as follows: wherein represents the current horizontal and vertical coordinates of the drone, represents the angle by which the direction of the velocity needs to be adjusted, which is obtained by the following equation, In the formula, represents the current position of the evading party, represents the position of the evading party at the next moment, i.e., the position of the collision, represents the velocity vector of the evading party, represents the opposite direction vector of the velocity vector of the other unmanned aerial vehicle in conflict, and respectively represent the position of the other unmanned aerial vehicle in conflict at the next moment and the current moment position, and finally the evading position of the unmanned aerial vehicle is obtained, and the evading position is used to replace the original position to achieve conflict resolution.

2. The method of claim 1, wherein, The step 1, modeling the target scene as k k A grid map, in which white areas represent feasible areas for the UAV cluster and black areas represent obstacle areas;​ Thus, the scene information is represented by a two-dimensional matrix of size k k 1 represents an obstacle region and 0 represents a feasible region; the UAV swarm is composed of multiple UAVs, corresponding to a set of start points and a set of end points.​ 3. The method of claim 1, wherein, The step 2, the speed of each particle in the random initial particle population is set, the initial position of the population particle is set at the starting point, and the loop is entered; the fitness function value of each particle is calculated, and the historical optimal value of each particle is obtained according to comparison Then the minimum fitness function value in the current generation is found as the optimal value of the population in the current generation The speed and position of the particle are updated according to the speed and position updating formula, and the position of each particle in each generation is saved, the position is a waypoint, it is judged whether the position reaches the target point and whether the fitness function value is less than the threshold value or whether the iteration number reaches the maximum iteration number, and any one of the two conditions is satisfied, and the loop is exited; the waypoints of one particle are connected to form a polyline, that is, the track of one particle; in one particle population, a plurality of particles are contained, and thus a plurality of tracks are correspondingly obtained, and the optimal track in each particle population is the global track of each unmanned aerial vehicle.

4. The method of claim 3, wherein, The step 2, initializing the particle population quantity is M , and setting the velocity and position range of the particle respectively as , , , wherein represents the lower limit of the velocity, and the value is 0, represents the upper limit of the velocity, represents the lower limit of the horizontal and vertical coordinates corresponding to the position of the particle, and , the values are both 0, represents the upper limit of the horizontal and vertical coordinates corresponding to the position of the particle; represents the current velocity of the particle id , represents the current position of the particle id . The position update formula of the standard particle swarm algorithm is: wherein represents an inertia weight factor, which is set as a variable linearly decreasing during iterations; represents a particle id of t momentum, represents a particle id of t position, represents a learning factor of the self-aware part, represents a learning factor of the social-aware part, , represents a random number between 0-1; The position update formula of the standard particle swarm algorithm is: According to the speed to update the position, and then set the corresponding fitness function The fitness function should include three parts: the distance function of the current position of the UAV to the target point The distance function of the current position of the UAV to the obstacle And the function corresponding to the smoothness of the UAV trajectory The formula is as follows: the value of represents the distance from the current position of the particle to the target point, the value of represents the size of the distance from the current position of the particle to the obstacle, the value of represents the angle of deviation from the last flight path to the next flight path of the UAV, , , is a scale factor with a value between [0, 1]; represents the centroid position of the obstacle, represents the t current position of the particle, represents the t -1 time position of the particle, represents the t -2 time position of the particle.

5. The method of claim 4, wherein, The inertial weight factor The formula is as follows: wherein, represents an upper limit of the inertia weight factor, represents a lower limit of the inertia weight factor, represents a maximum number of iterations, represents a current number of iterations.

6. The method of claim 1, wherein, The expression of the Bezier curve is as follows: wherein is a control point of a Bezier curve corresponding to the UAV flight path, is a Bernstein polynomial; denotes an order variable between 0 and n, denotes the number of control points, i.e. the order of the Bezier curve, denotes a time variable between 0 and 1. The segmented Bezier curve method takes N flight path points as a group to form a segment of flight path, and the segment of flight path is smoothed to obtain a smooth flight path of each UAV.

7. The method of claim 1, wherein, In step 5, the UAV simulation experiment is carried out on the simulation platform unity3D to verify whether the UAV can avoid static obstacles and other flying UAVs and reach the target point within a limited time.

Images