Multi-unmanned aerial vehicle motion planning method based on multivariate universe optimization algorithm

Abstract

The invention provides a multi-unmanned aerial vehicle motion planning method based on a multivariate universe optimization algorithm, and the method employs a Munkres distribution algorithm as a task distribution algorithm of unmanned aerial vehicles, selects an MVO algorithm to plan the path of each unmanned aerial vehicle, employs a parameter polynomial fitting method for track generation, and selects a PID control method as a flight control method of the unmanned aerial vehicles. According to the invention, the MVO algorithm is applied to the multi-unmanned aerial vehicle motion planning problem. Numerical experiments and actual case studies show that the algorithm has high applicability, and the algorithm is of great significance for efficiently completing motion planning of multiple unmanned aerial vehicles in a dynamic and complex environment. An algorithm system of an existing multi-unmanned-aerial-vehicle path planning problem solving method can be enriched and perfected, and meanwhile the multi-unmanned-aerial-vehicle path planning problem is solved with the MVO algorithm as a basic framework for solving the problems that a classic algorithm is low in convergence speed, low in solving precision and the like.

Description

Technical Field

[0001] The invention relates to the field of intelligent control, and in particular to a multi-UAV motion planning method. Background Art

[0002] Multi-UAV motion planning is a crucial part of multi-UAV cooperative control. The purpose of multi-UAV motion planning is to find effective motion sequences from the starting point to the target point for multiple UAVs in a three-dimensional environment that meet the UAV dynamic constraints. It includes appropriate task allocation, path planning and trajectory generation for multiple UAVs.

[0003] The path planning of UAV is to generate a path for the UAV that has only geometric properties, is independent of time, and only focuses on the position. The goal of path planning is to make the distance between the path and obstacles as far as possible while ensuring that the length of the path is as short as possible. Currently known path planning algorithms include A*, genetic algorithm, simulated annealing method, ant colony algorithm and artificial potential field algorithm. The multiverse optimization algorithm (MVO) is a new meta-heuristic algorithm with the advantages of fewer parameters and high efficiency. The MVO algorithm can be used to effectively plan the path of UAVs in a three-dimensional environment.

[0004] The MVO algorithm originates from the multiverse theory, and three main concepts of the multiverse theory are selected as inspiration for the MVO algorithm: white holes, black holes, and wormholes. Black holes have strong gravitational force and can be linked to another space-time through wormholes, which can absorb all matter and even light; white holes are matter that begins to spit out after a black hole absorbs too much matter; wormholes are holes that connect different parts of the universe together. Wormholes in the multiverse theory act as tunnels for time / space travel, where objects can be instantly transmitted between corners of the universe (even from one universe to another). The higher the expansion rate, the higher the probability of white holes and the lower the probability of black holes. Universes with higher expansion rates tend to pass matter through white holes, and universes with lower expansion rates tend to receive more matter through black holes. Regardless of the expansion rate, matter in all universes may move randomly toward the optimal universe through wormholes.

[0005] In China, many scholars have conducted preliminary research on the universe individual update strategy and model application in the multiverse optimization algorithm; abroad, the MVO algorithm is used to solve economic load scheduling, numerical calculation and engineering optimization, train feedforward neural networks, perform reactive power optimization scheduling, solve photovoltaic generator parameters, etc. However, there are relatively few studies on the application of the MVO algorithm in solving UAV motion problems at home and abroad. Summary of the invention

[0006] In order to overcome the shortcomings of the prior art, the present invention provides a multi-UAV motion planning method based on a multiverse optimization algorithm. The present invention aims to improve the efficiency of existing motion planning technology and proposes a multi-UAV motion planning method based on a multiverse optimization algorithm. The method adopts the Munkres allocation algorithm as the task allocation algorithm of the UAV, selects the MVO algorithm to plan the path of each UAV, adopts the parameter polynomial fitting method for trajectory generation, and selects the PID control method as the flight control method of the UAV. After simulation testing, each UAV can effectively reach its respective target point. And compared with the other two path planning algorithms, the MVO algorithm is more efficient in generating UAV flight paths. The present invention applies the MVO algorithm to the problem of multi-UAV motion planning. Numerical experiments and actual case studies show that the algorithm has strong applicability and is of great significance for multiple UAVs to efficiently complete motion planning in a dynamic and complex environment.

[0007] Assume that each drone path solution is equivalent to a universe, and each node in the solution is an object in that universe.

[0008] The steps for generating each path of the technical solution adopted by the present invention to solve its technical problem are as follows:

[0009] Step 1. Initialize the number of iterations, determine the target point that the drone can reach, set the starting point of the drone to the current node, and set the maximum number of iterations;

[0010] Step 2. Figure 2 As shown, the six neighboring points of the current node are determined. With the current node as the origin, six points with a distance of 1 from the origin in the three directions of x-axis, y-axis and z-axis are selected. According to the direction of the current node to the target point, the neighboring nodes with the closest direction and not on the same coordinate axis as the target point are selected as important neighboring points; the remaining three neighboring points are used as ordinary neighboring points; Figure 2 The blue points in the middle are the three points that are closer to the target point and are selected as important neighboring points.

[0011] Step 3. Perform collision check on the paths from common neighboring points to the current node and the paths from important neighboring points to the current node, and remove the parts where common neighboring points and important neighboring points collide, that is, determine whether the path from the current node to the neighboring point collides with the path from the current node to the important neighboring point. If there is a collision, remove the path;

[0012] Step 4. After removing the path in step 3, if it is determined that the important adjacent points still exist, proceed to step 5; if the important adjacent points do not exist, proceed to step 6;

[0013] Step 5. Randomly select a node from the important neighboring points and use it as the current node, and go to step 7;

[0014] Step 6. Randomly select a node from the ordinary neighboring points and use it as the current node, then proceed to Step 7;

[0015] Step 7. Increment the iteration count by 1. If the current iteration count reaches the maximum iteration count or the current node reaches the target point, then proceed to Step 8; otherwise, jump back to Step 2.

[0016] Step 8. End the loop. All the selected nodes form the path.

[0017] In Step 8, the principle of the Multiverse Optimization Algorithm is adopted during the entire path generation process. The specific steps are as follows:

[0018] First, it is necessary to create n universes, i.e., path solutions, and initialize the performance parameters of the Multiverse Optimization Algorithm: the Wormhole Existence Probability (WEP) and the Wormhole Travel Distance Rate (TDR). The parameter TDR decreases in a concave shape through iteration, first rapidly and then slowly, while the parameter WEP increases linearly.

[0019] After that, calculate the path cost of each universe.

[0020] According to the roulette wheel principle, sort the universes from high to low based on the universe expansion rate standardized by the cost, and select the universe with the highest fitness. According to the white hole serial number of the roulette wheel, exchange the dimension information between the black hole and the white hole. The dimension information is the path information of the drone.

[0021] To obtain the optimal motion trajectory, continuous iteration is carried out. Set the maximum iteration count to 500, and at the same time set the optimization boundaries ub and lb for this problem.

[0022] When the random number r_2 < WEP is satisfied, the black hole穿越到适应度最大的宇宙周围,黑洞维度在适应度最大的宇宙附近通过TDR参数进行更新;得到的最优宇宙即为无人机运动的最优轨迹。

[0023] The maximum iteration count is 500.

[0024] The iterative expressions for the TDP and WEP parameters are as follows:

[0025]

[0026] T is the maximum iteration count, and t is the current iteration count; min and max take empirical values.

[0027] min = 0.2 and max = 1.

[0028] The path cost of each universe, cost i is

[0029] cost i = Number of nodes on the i-th path + 100*(the distance from the end point of the path to the target point).

[0030] The ub and lb are defined as follows:

[0031] lb=[0 0 0]

[0032] ub=[20 5 6].

[0033] The beneficial effect of the present invention lies in that the multi-UAV motion planning method based on MVO proposed in the present invention has strong applicability shown through numerical experiments and actual case studies; the invention results can enrich and improve the algorithm system of existing multi-UAV path planning problem solving methods, and at the same time, in view of the problems of slow convergence speed and low solution accuracy of classical algorithms, the multi-UAV path planning problem is solved with the MVO algorithm as the basic framework.

[0034] Using the same settings, the total time for path generation for different numbers of drones is compared with the whale optimization algorithm (WOA) and the gray wolf optimization algorithm (GWO). The multiverse optimization algorithm generates drone paths faster and more efficiently. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 Flowchart of multi-UAV motion planning based on multiverse optimization algorithm.

[0036] Figure 2 Schematic diagram of neighboring points, important neighboring points and target points in the MVO algorithm.

[0037] Figure 3 It is a quadrotor drone model with a reference frame.

[0038] Figure 4 This is the path planning diagram for eight drones based on the MVO algorithm.

[0039] Figure 5 Graphs generated for the trajectories of eight UAVs based on parametric polynomials.

[0040] Figure 6 This is the position change curve of the first drone during flight. Figure 6 (a), (b) and (c) are the curves of the change of x-axis, y-axis and z-axis positions with time respectively.

[0041] Figure 7 This is the speed curve of the first drone during its flight. Figure 7 (a), (b) and (c) are the curves of the velocity changing with time in the x-axis, y-axis and z-axis directions respectively. DETAILED DESCRIPTION

[0042] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0043] The multi-UAV motion planning method based on the multiverse optimization algorithm proposed in this invention has the following overall process: Figure 1 The following is a further clear and complete description of the technical solution in conjunction with the accompanying drawings and specific embodiments:

[0044] Step 1: Assign tasks to each drone so that the total path length of the drone swarm is as short as possible, that is, the path length is as close as possible to the straight-line distance between two points.

[0045] Step 1-1: Create a distance matrix A based on the Euclidean distance between each drone and the target point. The distance matrix is ​​an n*n matrix, where each element a in the matrix ij represents the Euclidean distance for assigning one of the i drones to one of the j targets;

[0046] Step 1-2: For each row of the distance matrix, find the minimum element value and subtract it from each element in this row to obtain the distance matrix A; go to step 1-3.

[0047] Step 1-3: Find the zero element (Z) in the resulting matrix. If there is no starred zero in the row or column where the zero element is located, add a star to Z. Repeat the operation for each element in the matrix until all elements are traversed and go to step 1-4.

[0048] Step 1-4: Cover each column containing starred zeros with a covering line;

[0049] If n columns are covered, the asterisked zeros represent a complete set of unique assignments; in this case, go to steps 1-8, otherwise, if n columns are not covered, go to steps 1-5;

[0050] Step 1-5: Find an uncovered zero and prime it;

[0051] If there are no starred zeros in the row containing the primed zero, go to step 1-6, otherwise, cover the row and find the column containing the starred zero, fill the uncovered zeros with color until there are no zeros left, save the smallest uncovered value, and then go to step 1-7;

[0052] Step 1-6: Construct a series of alternating primed and asterisked zeros;

[0053] Let z0 represent the uncovered primed zero found in step 1-5, let z1 represent the starred zero in the column where z0 is located, and let z2 represent the primed zero in the row where z1 is located; continue this operation until there are no starred zeros in the column where the primed zero is located, remove the star from the zero represented by z1, and star the primed zero, erase all the primes and uncover each row in the matrix; return to step 1-4;

[0054] Step 1-7: Add the smallest uncovered value found in step 1-5 to each element of each covered row and subtract the smallest uncovered value found in step 1-5 from each element of each uncovered column, returning to step 1-5 without changing any stars, apostrophes, or covered lines;

[0055] Step 1-8: define the matrix obtained through steps 1-1 to 1-8 as a cost matrix, and assign pairs indicated by the positions of the asterisked zeros in the cost matrix;

[0056] If the element in row i and column j is a starred zero, assign the element associated with row i to the element associated with column j;

[0057] Step 2: Generate a corresponding path solution for each drone;

[0058] Step 2-1: Initial iteration number, determine the target point, the starting point of the drone and set it as the current node, set the maximum number of iterations;

[0059] Step 2-2: If Figure 2 As shown, determine the six neighboring points of the current node (such as Figure 2 black and blue dots), and according to the distance between the current position and the target point (such as Figure 2 Yellow dot), select the neighboring nodes that are relatively close and not on the same coordinate axis as the target as important neighboring points (such as Figure 2 Blue dot)

[0060] Step 2-3: Perform collision check on neighboring points and important neighboring points, and remove the parts where the neighboring nodes and important neighboring points collide;

[0061] Step 2-4: When it is determined that an important neighboring point exists, proceed to step 2-5; otherwise, proceed to step 2-6;

[0062] Step 2-5: Randomly select a node from the important neighboring points and use it as the current node, and proceed to step 2-7;

[0063] Step 2-6: Randomly select a node from the neighboring nodes and use it as the current node, and proceed to step 2-7;

[0064] Step 2-7: The number of iterations is increased by 1. When the current number of iterations reaches the maximum number of iterations or the current node reaches the target point, go to step 2-8, otherwise repeat step 2-2;

[0065] Step 2-8: End the loop and all selected nodes are paths.

[0066] Step 3: Generate trajectory using parametric polynomial fitting:

[0067] Step 3-1: Use a polynomial formula to express the required state of the drone's trajectory at each moment:

[0068] p(t)=c1+c2t+c3t 2 +c4t 3 +c5t 4 +c6t 5

[0069] v(t)=p'(t)

[0070] a(t)=v'(t)

[0071] Among them, c1, c2, c3, c4, c5, c6 are all constant parameters; p(t) is the trajectory of the UAV at each moment; v(t) is the speed of the UAV at each moment; a(t) is the acceleration of the UAV at each moment;

[0072] Step 3-2: Find the values ​​of the parameter polynomials c1, c2, c3, c4, c5, c6. The formula for calculating the x-axis parameters is as follows:

[0073]

[0074] The y-axis and z-axis are calculated in the same way.

[0075] Step 3-3: Constrain the generated drone trajectory:

[0076] The UAV dynamic constraints are:

[0077] maxf=2.5mg

[0078] minf=0.05mg

[0079] Constrain each trajectory time, and the time interval of each segment is:

[0080]

[0081] (x n ,y n ,z n )(x n+1 ,y n+1 ,z n+1) are the coordinates of the path nodes of the drone at the current and next moment respectively;

[0082] Step 4: Establish the UAV dynamics model.

[0083] Step 4-1: Consider the quadcopter as a rigid body, and apply torque and force to it from the four rotors located at the vertices of the square. Introduce an inertial reference frame C fixed to the ground I and the body coordinate system C attached to the quadcopter B , where the center of mass of the quadcopter is the body frame C B The origin of the fuselage fixed frame C B , and The axis of the rotor is located on a plane fixed by the four rotors, and The axis points downward in the opposite direction of the total thrust. The quadrotor drone model with a reference frame is as follows Figure 3 shown.

[0084] Step 4-2: Use Euler angles to define the body frame C B To the inertial frame C I The rotation matrix R is:

[0085]

[0086] where c * is cos(*), s * is sin(*);

[0087] The dynamic characteristics of the quadrotor are written in the following equations:

[0088]

[0089] Where m is the total mass, g = [0,0,g] T is the vector of gravity acceleration (g = 9.81 m / s 2 ), v is the inertial frame C I The speed of the quadcopter is given by:

[0090] v=[v x v y v z ] T

[0091] f=[0,0,f z ] T is the total thrust vector, where f z is with The sum of all forces generated by the four axially aligned rotors is given by:

[0092] fz =f1+f2+f3+f4

[0093] Similarly, the quadcopter is relative to the body coordinate system C B The angular velocity is defined as:

[0094] ω B =[ω x ω y ω z ] T

[0095] Ω is ω B The tensor form of is given by:

[0096]

[0097] τ=[τ x ,τ y ,τ z ] T is the torque vector of the quadrotor relative to the body coordinate system CB.

[0098] J=diag(J x ,J y ,J z ) is the inertia matrix, and r = [x, y, z] T is the quadcopter in the inertial system C I The position in.

[0099] The quadcopter control input μ is described in terms of three torques and total thrust as follows:

[0100] μ=[f z τ x τ y τ z ] T

[0101] The state ξ of the quadrotor is:

[0102] ξ=[xyzv x v y v z φ θ ψ ω x ω y ω z ] T

[0103] The values ​​of the quadcopter parameters are as follows: total mass equals 0.176 kg, arm length is 0.086 m, and inertia matrix J = diag{0.00025 0.000232 0.0003738}.

[0104] Step 5: Use PID algorithm to control the flight of the drone:

[0105] Step 5-1: PID drone position control:

[0106]

[0107] k p is the proportionality coefficient, k i is the integration coefficient, k d is the integration coefficient.

[0108] a x (t) = a x_des +k p1 (v x_des -v x )+k d1 (pos x_des -pos x )

[0109] a y (t) = a y_des +k p2 (v y_des -v y )+k d2 (pos y_des -pos y )

[0110] a z (t) = a z_des +k p3 (v z_des -v z )+k d3 (pos z_des -pos z )

[0111] Attitude angle control

[0112] φ des =(a x (t)*sin(ψ des )-a y (t)*cos(ψ des )) / g

[0113] θ des =(a x (t)*cos(ψ des )+a y (t)*sin(ψ des )) / g

[0114] ψ des =ψ des

[0115] The real-time thrust of the drone is: F = mg + ma z (t)

[0116] Drone torque output M = [M1, M2, M3]

[0117] M1=k pφ (φ des -φ)-k dφ *ω x

[0118] M2=k pθ (θ des -θ)-k dφ *ω y

[0119] M3=k pψ (ψ des -ψ)+k dψ *(ψ' des -ω z )

[0120] The PID parameters of the drone are selected as follows:

[0121] kp_1=3.6kp_2=3.6kp_3=70

[0122] kd_1=9kd_2=9kd_3=30

[0123] kp_Φ=0.6kp_θ=0.6kp_ψ=0.02

[0124] kd_Φ=0.06kd_θ=0.06kd_ψ=0.02

[0125] The drone flight controller output is:

[0126] [F,M,φ des ,θ des ,ψ des ]

[0127] The multi-UAV motion planning method based on the multiverse optimization algorithm proposed in the present invention selects a discretized environment of 20m*5m*6m to conduct a simulation flight experiment on multiple UAVs. Node spacing: The interval between each node on the x-axis and y-axis is 0.1, and the interval between each node on the z-axis is 0.25. There are red obstacles in the environment. The starting points of the eight UAVs are (8,1,5), (5,1,5), (5,3,5), (8.1,4,4.8), (11,0.5,4), (13,2,5), (13.5,4.1,4.8), (11,3.5,4.5), and the coordinates of the eight target points are (10,2,1), (10,1,0.8), (1.8,4,5.6),

[0128] (2,3,5.9), (10,3,1), (10,4.5,0.4), (19.8,3,5), (19.8,1,5). After calculating the Euclidean distance between each UAV and the target point, the Munkres allocation algorithm is used to allocate tasks. The MVO algorithm plans the paths of each UAV as follows: Figure 4 shown.

[0129] After parametric polynomial trajectory generation, the trajectory of each UAV is as follows Figure 5 As shown:

[0130] As a comparison of path algorithms, the same settings are used to select the multiverse optimization algorithm (MVO), the whale optimization algorithm (WOA) and the gray wolf optimization algorithm (GWO) to compare the total path generation time for different numbers of UAVs.

[0131] The path generation time is shown in Table 1:

[0132] Table 1 Comparison of path times generated by different algorithms

[0133] Total time to generate the path Two drones Five drones Eight drones MVO Algorithm 1.9s 3.375s 3.69s WOA Algorithm 2.3s 3.974s 4.75s GWO Algorithm 4.6s 9.2s 15.2s

[0134] It can be seen from Table 1 that compared with the gray wolf optimization algorithm and the whale optimization algorithm, the multiverse optimization algorithm can generate drone paths more effectively, faster and more efficiently.

[0135] The above description is only a preferred embodiment of the present invention. It should be pointed out that the embodiments of the present invention are not limited to the above-mentioned implementation methods. For ordinary technicians in the technical field to which the present invention belongs, several replacements, modifications and the like can be made without departing from the principle of the present invention, and all of them should be regarded as belonging to the protection scope of the present invention.

Claims

1. A multi-UAV motion planning method based on a multiverse optimization algorithm, characterized in that It includes the following steps: Step 1. Initialize the number of iterations, determine the target points that the UAV can reach, set the starting point of the UAV as the current node, and set the maximum number of iterations; Step 2. Determine the six neighboring points of the current node. Taking the current node as the origin, select six points at a distance of 1 from the origin on the three axes of the x-axis, y-axis, and z-axis, and according to the orientation of the current node from the target point, select the neighboring node that is relatively closest in orientation and not on the same coordinate axis as the target point as the important neighboring point; the remaining three neighboring points are used as ordinary neighboring points; Step 3. Conduct a collision check on the paths from the ordinary neighboring points to the current node and the paths from the important neighboring points to the current node, and remove the parts where the ordinary neighboring points and the important neighboring points have collisions, that is, determine whether the path from the current node to the neighboring point collides with the path from the current node to the important neighboring point. If there is a collision, then remove that path; Step 4. After removing the paths in Step 3, if the important neighboring points still exist, go to Step 5; if the important neighboring points do not exist, go to Step 6; Step 5. Randomly select a node from the important neighboring points and use it as the current node, and go to Step 7; Step 6. Randomly select a node from the ordinary neighboring points and use it as the current node, and go to Step 7; Step 7. Increment the number of iterations by 1. If the current number of iterations reaches the maximum number of iterations or the current node reaches the target point, then go to Step 8; otherwise, jump to Step 2; Step 8. End the loop, and all the selected nodes are the path.

2. The multi-UAV motion planning method based on the multiverse optimization algorithm according to claim 1, wherein: The principle of the multiverse optimization algorithm is adopted in the whole path generation process in Step 8, and the specific steps are as follows: First, it is necessary to create n universes, that is, path solutions, and initialize the performance parameters of the multiverse optimization algorithm, the wormhole existence probability WEP and the wormhole travel distance rate TDR. The parameter TDR decreases in a concave shape through iteration, first rapidly and then slowly, and the parameter WEP increases linearly: After that, calculate the path cost of each universe; According to the roulette principle, sort the universes from high to low according to the universe expansion rate standardized by the cost, select the universe with the largest fitness according to the white hole serial number of the roulette, and exchange the dimension information between the black hole and the white hole. The dimension information is the path information of the UAV; To obtain the optimal motion trajectory, continuous iteration is carried out, and at the same time, the optimization boundaries ub and lb are set for this problem; When the random number r_2 < WEP is satisfied, the black hole travels through the wormhole to the vicinity of the universe with the largest fitness, and the black hole dimension is updated near the universe with the largest fitness through the TDR parameter; the obtained optimal universe is the optimal trajectory of the UAV motion.

3. The multi-UAV motion planning method based on the multiverse optimization algorithm according to claim 2, wherein: The maximum number of iterations is 500.

4. The multi-UAV motion planning method based on the multiverse optimization algorithm according to claim 2, wherein: The iteration expressions of the TDP and WEP parameters are as follows: T is the maximum number of iterations, t is the current number of iterations; the min and max are empirical values.

5. The multi-UAV motion planning method based on the multiverse optimization algorithm according to claim 4 is characterized in that: The min=0.2, max=1.

6. The multi-UAV motion planning method based on the multiverse optimization algorithm according to claim 2 is characterized in that: The path cost cost of each universe i costi = number of nodes on the i-th path + 100*(the distance from the end point of the path to the target point).

7. The multi-UAV motion planning method based on the multiverse optimization algorithm according to claim 2 is characterized in that: The ub and lb are defined as follows: lb＝[0 0 0] ub=[20 5 6].

Images