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

Abstract

The application provides a multi-unmanned aerial vehicle motion planning method based on a multi-universe optimization algorithm, adopts Munkres distribution algorithm as a task distribution algorithm of the unmanned aerial vehicle, selects MVO algorithm to plan paths of each unmanned aerial vehicle, adopts a parameter polynomial fitting method for trajectory generation, and selects a PID control method as a flight control method of the unmanned aerial vehicle, and the application applies the MVO algorithm to a multi-unmanned aerial vehicle motion planning problem. Numerical experiments and actual case studies show that the algorithm has strong applicability, is important for the multi-unmanned aerial vehicle to efficiently complete motion planning in a dynamic and complex environment, can enrich and perfect an algorithm system of existing multi-unmanned aerial vehicle path planning problem solving methods, and solves the multi-unmanned aerial vehicle path planning problem based on the MVO algorithm as a basic framework in view of problems such as slow convergence speed and low solution accuracy of classical algorithms.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of intelligent control, and in particular to a multi-unmanned aerial vehicle motion planning method. BACKGROUND

[0002] Multi-unmanned aerial vehicle motion planning is a crucial part of multi-unmanned aerial vehicle cooperative control. The purpose of multi-unmanned aerial vehicle motion planning is to find an effective motion sequence for each unmanned aerial vehicle from the starting point to the target point in a three-dimensional environment that meets the unmanned aerial vehicle dynamics constraints. It includes appropriate task allocation, path planning and trajectory generation for multiple unmanned aerial vehicles.

[0003] The path planning of unmanned aerial vehicles is to generate a path for the unmanned aerial vehicle that only has geometric properties, is independent of time and only concerns position. The goal of path planning is to make the distance between the path and the obstacle 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, ant colony algorithm and artificial potential field algorithm. Multi-universe optimization algorithm (MVO) is a new type of meta-heuristic algorithm, which has the advantages of fewer parameters and high efficiency. The MVO algorithm can effectively plan the path of the unmanned aerial vehicle in a three-dimensional environment.

[0004] The MVO algorithm is derived from the theory of multi-universe, which selects three main concepts of the theory of multi-universe as the inspiration of the MVO algorithm: white hole, black hole and wormhole. Black holes have a strong gravitational force and can be linked to another space-time through wormholes, and can absorb all matter and even light; white holes are the matter that is vomited out after a black hole absorbs too much matter; wormholes are those holes that connect different parts of the universe. The wormhole in the theory of multi-universe acts as a tunnel for time / space travel, where objects can travel instantly between different corners of the universe (even from one universe to another). The higher the expansion rate, the higher the probability of white hole and the lower the probability of black hole. The higher the expansion rate of the universe, the more matter tends to pass through the white hole, and the lower the expansion rate of the universe, the more matter tends to be received through the black hole. Regardless of the expansion rate, all matter in the universe can move randomly through the wormhole towards the best universe.

[0005] In China, many scholars have made preliminary research on the universe individual update strategy and model application in the multi-universe optimization algorithm; abroad, the MVO algorithm has been used to solve economic load dispatch, numerical calculation and engineering optimization, train feedforward neural networks, conduct reactive power optimization scheduling, and solve photovoltaic generator parameters. However, the application of the MVO algorithm in solving unmanned aerial vehicle motion problems is relatively less studied at home and abroad. SUMMARY

[0006] In order to overcome the deficiencies of the prior art, the present application provides a multi-unmanned aerial vehicle motion planning method based on a multi-universe optimization algorithm. The present application aims to improve the efficiency of existing motion planning techniques and proposes a multi-unmanned aerial vehicle motion planning method based on a multi-universe optimization algorithm. The method uses the Munkres assignment algorithm as the task assignment algorithm for the unmanned aerial vehicle, selects the MVO algorithm to plan the path of each unmanned aerial vehicle, uses the parameter polynomial fitting method for trajectory generation, and selects the PID control method as the flight control method for the unmanned aerial vehicle. Through simulation testing, each unmanned aerial vehicle can effectively reach the target point. Compared with two other path planning algorithms, the MVO algorithm generates the unmanned aerial vehicle flight path with higher efficiency. The present application applies the MVO algorithm to the multi-unmanned aerial vehicle motion planning problem. Numerical experiments and actual case studies show that the algorithm has strong applicability and is of great significance for the efficient completion of motion planning by multiple unmanned aerial vehicles in a dynamic and complex environment.

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

[0008] The technical solution adopted by the present application to solve its technical problems has the following path generation steps:

[0009] Step 1. Initialize the number of iterations, determine the target points that the unmanned aerial vehicle can reach, set the starting point of the unmanned aerial vehicle as the current node, and set the maximum number of iterations;

[0010] Step 2. As shown in Figure 2 , determine the six neighboring points of the current node. Select six points at a distance of 1 from the origin in the x-axis, y-axis, and z-axis directions, and select the neighboring nodes that are relatively closest in orientation and do not have the same coordinate axis as the target point as important neighboring nodes. The remaining three neighboring nodes are ordinary neighboring nodes; Figure 2 The blue points are the three points that are closer to the target point, and are selected as important neighboring nodes,

[0011] Step 3. Collision detection is performed on the paths from the ordinary neighboring nodes to the current node and the paths from the important neighboring nodes to the current node. The parts with collisions between the ordinary neighboring nodes and the important neighboring nodes are removed, i.e. it is determined whether the path from the current node to the neighboring node has a collision with the path from the current node to the important neighboring node. If there is a collision, the path is removed;

[0012] Step 4. After removing the path in step 3, if the important neighboring nodes still exist, go to step 5; if the important neighboring nodes do not exist, go to step 6;

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

[0014] Step 6. Randomly select a node from the common adjacent points and take it as the current node, and go to Step 7;

[0015] Step 7. Add 1 to the iteration number, and if the current iteration number reaches the maximum iteration number or the current node reaches the target point, go to Step 8, otherwise, jump to Step 2;

[0016] Step 8. End the loop, and all selected nodes are the path.

[0017] The step 8 adopts the principle of the multiverse optimization algorithm in the whole path generation process, and the specific steps are as follows:

[0018] First, n universes, i.e. path solutions, need to be created, and the performance parameters of the multiverse optimization algorithm, wormhole existence probability (WEP) and wormhole travel distance rate (TDR), are initialized. The parameter TDR is concave decreasing through iteration, which is quickly reduced at first and then slowly reduced, and the parameter WEP is linearly increasing:

[0019] Then, the path cost of each universe is calculated;

[0020] According to the roulette principle, the universes are sorted from high to low according to the cost of the cost-standardized universe expansion rate, the universe with the maximum fitness is selected according to the roulette white hole serial number, the black hole and the white hole exchange dimension information, and the dimension information is the path information of the unmanned aerial vehicle;

[0021] In order to obtain the optimal motion trajectory, continuous iteration is carried out, and the maximum iteration number is set to 500, and the optimization boundary ub and lb of the problem are set;

[0022] When the random number r_2 is less than WEP, the black hole travels through the wormhole to the surrounding of the universe with the maximum fitness, and the black hole dimension is updated near the universe with the maximum fitness through the TDR parameter; the obtained optimal universe is the optimal trajectory of the unmanned aerial vehicle motion.

[0023] The maximum iteration number is 500.

[0024] The TDP and WEP parameter iteration expressions are as follows:

[0025]

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

[0027] The min=0.2, max=1.

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

[0029] cost i = the number of path nodes of the ith path + 100*(the distance between the end of the path and the target point).

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

[0031] lb = [0 0 0]

[0032] ub = [20 5 6].

[0033] The application has the beneficial effect that the multi-unmanned aerial vehicle motion planning method based on MVO proposed by the application has strong applicability through numerical experiments and actual case studies; the invention can enrich and perfect the algorithm system of the existing multi-unmanned aerial vehicle path planning problem solving method, and simultaneously solve the problems of slow convergence speed and low solution accuracy of the classical algorithm, and solve the multi-unmanned aerial vehicle path planning problem based on the MVO algorithm as a basic framework.

[0034] The same settings are adopted to compare the total path generation time of different numbers of unmanned aerial vehicles with the whale optimization algorithm (WOA) and the grey wolf optimization algorithm (GWO), and the multi-universe optimization algorithm generates the unmanned aerial vehicle path faster and more efficiently. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 It is a flow chart of multi-unmanned aerial vehicle motion planning based on the multi-universe optimization algorithm.

[0036] Figure 2 It is a schematic diagram of the adjacent point, important adjacent point and target point in the MVO algorithm.

[0037] Figure 3 It is a quadrotor unmanned aerial vehicle model with a reference frame.

[0038] Figure 4 It is a path planning diagram for eight unmanned aerial vehicles based on the MVO algorithm.

[0039] Figure 5 It is a trajectory generation diagram for eight unmanned aerial vehicles based on a parametric polynomial.

[0040] Figure 6 It is a position change curve of the first unmanned aerial vehicle during flight, Figure 6 (a), (b) and (c) of which are the x-axis, y-axis and z-axis position change curves over time.

[0041] Figure 7 It is a speed change curve of the first unmanned aerial vehicle during flight, Figure 7 (a), (b) and (c) of which are the x-axis, y-axis and z-axis direction speed change curves over time. DETAILED DESCRIPTION

[0042] The application will be further described in conjunction with the accompanying drawings and examples.

[0043] The multi-vehicle optimization algorithm-based multi-vehicle motion planning method proposed by the application has the overall process as shown in the figure. Figure 1 The technical solution will be further described in detail in conjunction with the accompanying drawings and specific examples:

[0044] Step 1: Assign tasks to each unmanned aerial vehicle so that the total path length of the unmanned aerial vehicle group 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: According to the Euclidean distance of each unmanned aerial vehicle from the target point, create a distance matrix A, which is an n*n matrix, wherein each element a ij represents the Euclidean distance of assigning one of the i unmanned aerial vehicles to one of the j targets;

[0046] Step 1-2: For each row of the distance matrix, find the smallest element value and subtract the smallest element value from each element in the 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, and if there is no zero with a star in the row or column where the zero element is located, add a star to the zero, 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 the zero with a star with a cover line.

[0049] If n columns are covered, the zero with a star represents a complete unique assignment set; in this case, go to step 1-8, otherwise, n columns are not covered, go to step 1-5.

[0050] Step 1-5: Find an uncovered zero and add a dash to it.

[0051] If there is no zero with a star in the row containing the dashed zero, go to step 1-6, otherwise, cover the row and find the column containing the zero with a star, fill the uncovered zeros with color until there are no remaining zeros, save the smallest uncovered value, and then go to step 1-7.

[0052] Step 1-6: Construct a series of alternating dashed and starred zeros.

[0053] Let z0 represent the uncovered plus zero found in step 1-5, let z1 represent the plus starred zero in the column where z0 is located, and let z2 represent the plus zero in the row where z1 is located; continue this operation until there is no plus starred zero in the column where the plus zero is located, remove the star from the zero represented by z1 and star the plus zero, erase all tildes 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, return to step 1-5 without changing any stars, tildes or cover lines;

[0055] Step 1-8: Define the matrix obtained through steps 1-1 to 1-8 as a cost matrix, assign elements associated with row i to elements associated with column j if the element of row i column j is a plus starred zero;

[0056] Step 1-9: If the element of row i column j is a plus 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 UAV;

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

[0059] Step 2-2: As shown in Figure 2 , determine the six neighboring points of the current node (such as Figure 2 black and blue points), and according to the orientation of the current position point from the target point (such as Figure 2 yellow point), select the neighboring nodes with relatively close orientation and without the same coordinate axis as the target as important neighboring points (such as Figure 2 blue points)

[0060] Step 2-3: Collision detection is performed on the neighboring points and important neighboring points, and the parts of the neighboring points and important neighboring points that have collisions are removed;

[0061] Step 2-4: If the important neighboring points exist, proceed to step 2-5; otherwise, proceed to step 2-6;

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

[0063] Step 2-6: Randomly select one node from the neighboring points as the current node, and proceed to step 2-7;

[0064] Step 2-7: The iteration number is incremented by 1, and the current iteration number reaches the maximum iteration number or the current node reaches the target point, then go to step 2-8, otherwise repeat step 2-2;

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

[0066] Step 3: Generate the trajectory by using the parameter polynomial fitting:

[0067] Step 3-1: Express the required state of the UAV trajectory at each time point by a polynomial formula:

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

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

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

[0071] wherein c1, c2, c3, c4, c5, and c6 are constant parameters; p(t) is the trajectory of the UAV at each time point; v(t) is the speed of the UAV at each time point; and a(t) is the acceleration of the UAV at each time point;

[0072] Step 3-2: Calculate the values of the parameter polynomials c1, c2, c3, c4, c5, and c6, and the x-axis parameter calculation formula is as follows:

[0073]

[0074] The calculation methods of the y-axis and z-axis are the same.

[0075] Step 3-3: Restrict the generated UAV trajectory:

[0076] The UAV dynamics constraint is:

[0077] maxf = 2.5mg

[0078] minf = 0.05mg

[0079] Restrict the time of each trajectory segment, 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 current and next path node coordinates of the UAV, respectively;

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

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

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

[0085]

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

[0087] Write the dynamics of the quadcopter by the following equations:

[0088]

[0089] where m is the total mass, g = [0, 0, g] T is the vector of gravitational acceleration (g = 9.81 m / s 2 ), v is the velocity of the quadcopter in the inertial frame C I 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 the sum of all forces produced by the four rotors aligned with the C axis given by:

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

[0093] Similarly, the quadcopter is positioned relative to the body coordinate system C. B Angular velocity is defined as:

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

[0095] Ω is ω B The tensor form is given by the following equation:

[0096]

[0097] τ=[τ x ,τ y ,τ z ] T It is the torque vector of the quadcopter 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 It is a quadcopter in inertial frame C I The position in the middle.

[0099] The quadcopter control input μ is described using three torques and total thrust, as shown below:

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

[0101] The state ξ of the quadcopter is:

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

[0103] The parameters of the quadcopter 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: Control the flight of the UAV using PID algorithm:

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

[0106]

[0107] k p is the proportional coefficient, k i is the integral coefficient, k d is the integral 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] Real-time thrust of UAV is: F = mg + ma z (t)

[0116] Moment output of UAV 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] PID parameters of UAV are selected as:

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

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

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

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

[0125] Output of flight controller of UAV is:

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

[0127] This invention proposes a multi-UAV motion planning method based on a multiverse optimization algorithm. A discretized environment of 20m*5m*6m is selected for simulation flight experiments on multiple UAVs. Node spacing: 0.1 for each node along the x-axis and y-axis, and 0.25 for each node along the z-axis. Red obstacles are present 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), and (11,3.5,4.5), respectively. The coordinates of the eight target points are (10,2,1), (10,1,0.8), (1.8,4,5.6), respectively.

[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, and performing task allocation using the Munkres allocation algorithm, the MVO algorithm's path planning for each UAV is as follows: Figure 4 As shown.

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

[0130] As a comparison of path generation algorithms, using the same settings, we compared the total path generation time of three algorithms—Multiverse Optimization (MVO), Whale Optimization (WOA), and Grey Wolf Optimization (GWO)—with 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 of generating 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] As can be seen from Table 1, compared with the Grey Wolf Optimization Algorithm and the Whale Optimization Algorithm, the Multiverse Optimization Algorithm can generate UAV paths more effectively, faster, and with higher efficiency.

[0135] The above description is only a preferred embodiment of the present invention. It should be noted that the embodiments of the present invention are not limited to the above-described implementation methods. For those skilled in the art, various substitutions and modifications can be made without departing from the principle of the present invention, and all such modifications and modifications should be considered to fall within the protection scope of the present invention.

Claims

1. A multi-unmanned aerial vehicle motion planning method based on a multi-universe optimization algorithm, characterized in that The method comprises the following steps: Step 1. Initialize the iteration number, determine the target point that the unmanned aerial vehicle can reach, set the starting point of the unmanned aerial vehicle as the current node, and set the maximum iteration number; Step 2. Determine the six adjacent points of the current node. Take the current node as the origin, select six points at a distance of 1 in the x-axis, y-axis and z-axis directions from the origin, and select the adjacent nodes with the relatively nearest orientation and without being in the same coordinate axis as the target point as important adjacent nodes according to the orientation of the current node to the target point; the remaining three adjacent nodes are ordinary adjacent nodes; Step 3. Collision detection is performed on the paths from the ordinary adjacent nodes to the current node and the paths from the important adjacent nodes to the current node, and the parts with collisions of the ordinary adjacent nodes and the important adjacent nodes are removed, that is, it is judged whether the path from the current node to the adjacent node collides with the path from the current node to the important adjacent node, and if there is a collision, the path is removed; Step 4. After removing the path in step 3, if the important adjacent nodes still exist, step 5 is entered; if the important adjacent nodes do not exist, step 6 is performed; Step 5. A node is randomly selected from the important adjacent nodes and is taken as the current node, and step 7 is entered; Step 6. A node is randomly selected from the ordinary adjacent nodes and is taken as the current node, and step 7 is performed; Step 7. The iteration number is increased by 1, and if the current iteration number reaches the maximum iteration number or the current node reaches the target point, step 8 is entered, otherwise, step 2 is jumped to; Step 8. The loop is ended, and all selected nodes are the path; The step 8 adopts the principle of the multi-universe optimization algorithm in the whole path generation process, and the specific steps are as follows: Firstly, n universes, that is, path solutions, are created, and the performance parameters of the multi-universe optimization algorithm, wormhole existence probability WEP and wormhole travel distance rate TDR, are initialized. The parameter TDR is iteratively concave decreasing, that is, it is rapidly reduced at first and then slowly reduced, and the parameter WEP is linearly increased: Then, the path cost of each universe is calculated; According to the roulette principle, the universes are sorted from high to low according to the cost cost standardization universe expansion rate, the universe with the maximum fitness is selected according to the roulette white hole serial number, the black hole and the white hole exchange dimension information, and the dimension information is the path information of the unmanned aerial vehicle; In order to obtain the optimal motion trajectory, continuous iteration is performed, and the optimization boundary ub and lb are set; When the random number r_2 is less than WEP, the black hole passes through the wormhole to the surrounding of the universe with the maximum fitness, and the dimension of the black hole is updated near the universe with the maximum fitness through the TDR parameter; the obtained optimal universe is the optimal trajectory of the unmanned aerial vehicle motion; The TDP and WEP parameter iteration expressions are as follows: T is the maximum iteration number, t is the current iteration number; min and max take empirical values.

2. The multi-unmanned aerial vehicle motion planning method based on the multi-universe optimization algorithm according to claim 1, wherein: The maximum iteration number is 500.

3. The multi-unmanned aerial vehicle motion planning method based on the multi-universe optimization algorithm according to claim 1, wherein: The min is 0.2, and the max is 1.

4. The multi-unmanned aerial vehicle motion planning method based on the multi-universe optimization algorithm according to claim 1, characterized in that: The path cost cost of each universe i is cost i = the number of path nodes of the ith path + 100*(the distance from the end of the path to the target point).

5. The multi-unmanned aerial vehicle motion planning method based on the multi-universe optimization algorithm according to claim 1, characterized in that: The ub and lb are defined as follows: lb＝[0 0 0] ub = [20 5 6].

Images