Man-machine three-dimensional route planning method

Abstract

The invention discloses a three-dimensional flight path planning method for an unmanned aerial vehicle. The method comprises the following steps: acquiring a to-be-planned node set; according to the to-be-planned node set, a preset path population is determined, and the preset path population comprises preset paths corresponding to a plurality of preset node sequences respectively; using a preset optimization algorithm to optimize the preset path population according to a preset number of optimization times to obtain a target path population, the preset optimization algorithm being used for determining a plurality of elite paths whose total path distance meets a preset condition from the preset path population in each optimization process, each preset path in the preset path population is fused with the plurality of elite paths in sequence, and the target path population comprises an optimized path corresponding to each preset path after preset optimization times; and in the target path population, determining the optimal path with the shortest total path distance as the target path. The technical problem that the three-dimensional flight path planning effect of the unmanned aerial vehicle is poor is solved.

Description

Technical Field

[0001] The present invention relates to the field of data processing, and in particular to a method for human-machine three-dimensional trajectory planning. Background Art

[0002] The three-dimensional trajectory planning problem of UAV is an extension of the Traveling Salesman Problem (TSP), which means that in a limited three-dimensional space, given the starting point of the UAV and several mission points, the UAV starts from the starting point, visits each mission point and then returns to the starting point. Through a reasonable trajectory planning method, a safe and efficient path is designed, so that the UAV can navigate between multiple points and achieve goals such as the shortest total distance and the lowest cost under certain constraints.

[0003] Since it was proposed in the last century, the three-dimensional trajectory planning problem of unmanned aerial vehicles has been one of the most basic problems in network optimization. Due to its wide application and great economic value, it has always received widespread attention. It is a variant of the traveling salesman problem and a non-deterministic problem with polynomial complexity, that is, an NP-complete problem. The difficulty of solving the problem increases exponentially with the data scale, and it is difficult to obtain the optimal solution within a limited time. Therefore, the current three-dimensional trajectory planning of unmanned aerial vehicles has the problem of low planning efficiency.

[0004] Currently, no effective solution has been proposed to address the problem of low efficiency in three-dimensional trajectory planning of UAVs mentioned above. Summary of the invention

[0005] The embodiment of the present invention provides a method for human-machine three-dimensional trajectory planning, so as to at least solve the technical problem that the three-dimensional trajectory planning effect of unmanned aerial vehicles is poor.

[0006] According to one aspect of an embodiment of the present invention, a path planning method is provided, comprising: obtaining a set of nodes to be planned, wherein the set of nodes to be planned comprises: a plurality of path nodes, and the plurality of path nodes comprise: a starting point and at least one task point; determining a preset path population according to the set of nodes to be planned, wherein the preset path population comprises: preset paths corresponding to a plurality of preset node sequences, both ends of each of the preset node sequences are the starting points, and each of the preset node sequences comprises all of the task points in the set of nodes to be planned, and each of the preset paths comprises: a target node path between each pair of adjacent path nodes in the preset node sequence; optimizing the preset path population according to a preset number of optimizations using a preset optimization algorithm to obtain a target path population, wherein the preset optimization algorithm is used to determine, during each optimization process, a plurality of elite paths whose total path distances meet preset conditions from the preset path population, and sequentially merge each of the preset paths in the preset path population with the plurality of elite paths, and the target path population comprises: an optimized path corresponding to each of the preset paths after the preset number of optimizations; in the target path population, determining the optimized path with the shortest total path distance as the target path.

[0007] Optionally, after obtaining the set of nodes to be planned, the method further includes: combining any two of the path nodes in the set of nodes to be planned to obtain a path node set, wherein the path node set includes: a plurality of path node pairs, each path node pair includes: a first node and a second node; and a target node path with the shortest planning path distance between the first node and the second node.

[0008] Optionally, planning a target node path with the shortest path distance between the first node and the second node includes: using a rapidly expanding random tree to perform path planning for each pair of the first node and the second node according to a preset planning number of times to obtain multiple preset node paths, wherein the rapidly expanding random tree is used to use multiple tree nodes in a preset random tree to record the first node as the root node, the second node as the leaf node, and multiple sampling nodes randomly selected during this path planning process during each path planning process, and determine the preset node path according to the preset random tree; among the multiple preset node paths, determine the preset node path with the shortest path distance as the target node path.

[0009] Optionally, the method also includes: in each path planning process, randomly selecting a sampling node that is not in a preset obstacle area from a preset three-dimensional space, wherein the preset three-dimensional space includes at least: the first node, the second node and the preset obstacle area; among at least one of the tree nodes recorded in the preset random tree, determining that the tree node closest to the sampling node is the target node, wherein the tree node includes at least: the first node; detecting whether the tree node path between the sampling node and the target node passes through the preset obstacle area; if the tree node path does not pass through the preset obstacle area, recording the sampling node as the tree node connected to the target node.

[0010] Optionally, the method also includes: when the tree node path does not pass through the preset obstacle area, detecting whether the path distance between the sampling node and the second node meets a preset distance threshold; when the path distance between the sampling node and the second node does not meet the preset distance threshold, randomly selecting sampling nodes that are not in the preset obstacle area in the preset three-dimensional space again until the number of nodes of the sampling nodes recorded in the preset random tree reaches a first number threshold; when the path distance between the sampling node and the second node meets the preset distance threshold, recording the sampling node as the tree node connected to the target node, and recording the second node as the tree node connected to the sampling node.

[0011] Optionally, among multiple preset node paths, determining the preset node path with the shortest path distance as the target node path includes: among multiple preset node paths, determining the preset node path with the shortest path distance as the first node path; eliminating the redundant sampling nodes in the first node path to obtain a second node path, wherein the second node path includes: the second node path includes: the first node, the second node and necessary nodes, and the necessary nodes are the sampling nodes that have not been eliminated; determining the fitting curve of multiple nodes to be fitted in the second node path to obtain the target node path, wherein the nodes to be fitted include: the first node, the second node and the necessary nodes.

[0012] Optionally, removing the redundant sampling nodes in the first node path to obtain the second node path includes: placing the first node into a preset path stack; when the top node of the preset path stack changes, traversing each tree node in the random tree to be optimized in the direction from the leaf node to the root node until the necessary node is determined, wherein the random tree to be optimized is the preset random tree for determining the first node path, and the tree nodes in the random tree to be optimized include at least: the sampling node and the second node, and the stack node path between the necessary node and the top node does not pass through a preset obstacle area; placing the necessary node into the preset path stack until the second node is placed into the preset path stack; determining the second node path according to the first node, the necessary node and the second node recorded in the preset path stack.

[0013] Optionally, when the top node of the preset path stack changes, traversing each tree node in the random tree to be optimized in the direction from the leaf node to the root node until the necessary node is determined includes: when the top node of the preset path stack changes, selecting the tree node as the node to be detected in the direction from the leaf node to the root node; detecting whether the stack node path between the node to be detected and the top node in the preset path stack passes through a preset obstacle area; when the stack node path does not pass through the preset obstacle area, determining the node to be detected of the stack node path as the necessary node; when the stack node path passes through the preset obstacle area, selecting the next tree node as the node to be detected in the direction from the leaf node to the root node until the necessary node is determined.

[0014] Optionally, determining the fitting curve of multiple nodes to be fitted in the second node path to obtain the target node path includes: detecting whether the number of fitting nodes of the nodes to be fitted in the second node path reaches a second number threshold; when the number of fitting nodes reaches the second number threshold, using the curve to fit adjacent nodes to be fitted in the second node path in sequence to obtain the target node path.

[0015] Optionally, the method further includes: inserting an expansion node in the second node path when the number of fitting nodes does not reach the second number threshold; and determining the expansion node as the node to be fitted until the number of fitting nodes reaches the second number threshold.

[0016] Optionally, when the number of fitting nodes does not reach the second number threshold, inserting the expansion node in the second node path includes: when the number of fitting nodes does not reach the second number threshold, determining a pair of nodes to be fitted with the farthest adjacent distance in the second node path, wherein the pair of nodes to be fitted includes: two nodes to be fitted; and inserting the expansion node at a middle position between the two nodes to be fitted in the pair of nodes to be fitted.

[0017] Optionally, determining the preset path population based on the set of nodes to be planned includes: using a population initialization based on a greedy strategy to serialize the set of nodes to be planned according to a preset number of serialization times to obtain a plurality of the preset node sequences, wherein the population initialization based on the greedy strategy is used to determine the preset processing strategy for serialization processing this time during each serialization processing, and serialize the set of nodes to be planned according to the preset processing strategy to obtain the preset node sequence, and the preset processing strategy at least includes: individual initialization and random scrambling based on the greedy strategy; connecting the target node paths between multiple pairs of adjacent path nodes in each of the preset node sequences to generate the preset paths corresponding to the preset node sequences; and placing the plurality of preset node sequences and the preset paths corresponding to each of the preset node sequences into the preset path population.

[0018] Optionally, the method also includes: generating a first random number during each serialization process; detecting whether the first random number is less than a first greediness; when the first random number is less than the first greediness, using individual initialization based on a greedy strategy to put each of the path nodes in the set of nodes to be planned into a first initial node sequence in sequence to obtain the preset node sequence, wherein the first position of the first initial node sequence is the starting point; when the first random number is not less than the first greediness, randomly scrambling the set of nodes to be planned to obtain the preset node sequence.

[0019] Optionally, when the first random number is less than the first greediness, each of the path nodes in the node set to be planned is placed into a first initial node sequence in sequence using individual initialization based on a greedy strategy, and obtaining the preset node sequence includes: when the first random number is less than the first greediness, the last path node in the first initial node sequence is determined as the target path node; according to the node set to be planned, a candidate node set of the target path node is determined, wherein the candidate node set includes: a plurality of candidate path nodes, and a selection probability of each candidate path node, the candidate path node is the path node in the node set to be planned that is not placed in the first initial node sequence, the selection probability is determined according to the path distance between the candidate path node and the target path node, and the path distance is inversely proportional to the selection probability; from the plurality of candidate path nodes in the candidate node set, a candidate path node is selected and placed in the first initial node sequence; after the candidate path node is selected and placed in the first initial node sequence, the candidate path node is determined as the target path node until the preset node sequence is obtained.

[0020] Optionally, selecting a candidate path node from the multiple alternative path nodes in the candidate node set and placing it in the first initial node sequence includes: generating a second random number corresponding to the target path node; when the second random number is less than a second greediness, selecting the alternative path node with the largest selection probability from the candidate node set as the selected candidate path node; when the second random number is not less than the second greediness, using a roulette algorithm to randomly select the candidate path node from the multiple alternative path nodes in the candidate node set, wherein the roulette algorithm is used to scale the sum of the selection probabilities of the multiple alternative path nodes in the candidate node set to 1, and randomly select the alternative path node as the candidate path node according to the scaled selection probability; connecting the candidate path node after the target path node in the first initial node sequence.

[0021] Optionally, the method also includes: after selecting a candidate path node and placing it into the first initial node sequence, deleting the candidate path node from the candidate node set; after deleting the candidate path node from the candidate node set, detecting whether the candidate node set is empty; if the candidate node set is empty, connecting the starting point after the candidate path node.

[0022] Optionally, when the first random number is not less than the first greediness, the set of nodes to be planned is randomly scrambled to obtain the preset node sequence, including: when the first random number is not less than the first greediness, all the path nodes in the set of nodes to be planned are randomly scrambled to obtain a second initial node sequence; determining a first node sequence and a second node sequence in each of the second initial node sequences, wherein the first node sequence is the front sequence in the initial node sequence with the starting point as the last position, and the second node sequence is the back sequence in the initial node sequence with the starting point as the first position; moving the first node sequence to the end of the second node sequence to obtain the preset node sequence corresponding to each of the second initial nodes.

[0023] Optionally, the preset path population is optimized according to a preset number of optimizations using a preset optimization algorithm to obtain a target path population, including: in each optimization process, detecting the fitness of each preset path in the preset path population, wherein the fitness is determined according to the total path distance of the preset path, and the fitness is proportional to the total path distance; selecting multiple preset paths with the largest fitness in the preset path population as multiple elite paths that meet the preset conditions; merging each preset path in the preset path population with multiple elite paths in order of fitness from high to low to obtain a target merged path corresponding to each preset path; updating the preset path population according to the multiple target merged paths, and optimizing again until the preset number of optimizations is reached.

[0024] Optionally, the multiple elite paths include at least a first elite path, a second elite path and a third elite path arranged in descending order of fitness, and each of the preset paths in the preset path population is merged with the multiple elite paths in descending order of fitness to obtain a target merged path corresponding to each preset path, including: fusing the preset paths in the preset path population with the first elite path to obtain a first merged path; fusing the first merged path with the second elite path to obtain a second merged path; fusing the second merged path with the third elite path to obtain a third merged path; and determining the third merged path as the target merged path corresponding to the preset path.

[0025] Optionally, the method further includes: when each of the preset paths is fused with each of the elite paths, randomly selecting the preset paths from the preset path population as paths to be optimized; detecting whether the paths to be optimized belong to the elite paths; when the paths to be optimized do not belong to the elite paths, using a preset encirclement operation to generate the target merged path based on the paths to be optimized, wherein the preset encirclement operation is used to randomly select the path nodes in the paths to be optimized as the first target nodes; determining the path nodes adjacent to the first target nodes in the elite paths as the second target nodes; intercepting a plurality of the path nodes between the first target nodes and the second target nodes and including the second target nodes in the paths to be optimized to obtain a first preset node subsequence; reversing the first preset node subsequence and inserting it into the paths to be optimized so that the second target nodes are adjacent to the first target nodes; when the paths to be optimized belong to the elite paths, using a preset attack operation to generate the target merged path based on the paths to be optimized, wherein the preset attack operation is used to randomly intercept a second preset node subsequence in the paths to be optimized, and reversing the second preset node subsequence and inserting it into the original position.

[0026] In an embodiment of the present invention, a set of nodes to be planned is obtained, wherein the set of nodes to be planned includes: a plurality of path nodes, and the plurality of path nodes include: a starting point and at least one task point; a preset path population is determined according to the set of nodes to be planned, wherein the preset path population includes: preset paths corresponding to a plurality of preset node sequences, both ends of each preset node sequence are starting points, and each preset node sequence includes all task points in the set of nodes to be planned, and each preset path includes: a target node path between each pair of adjacent path nodes in the preset node sequence; a preset optimization algorithm is used to optimize the preset path population according to a preset optimization number of times to obtain a target path population, wherein the preset optimization algorithm is used to optimize the preset path population in each optimization process , multiple elite paths whose total path distances meet preset conditions are determined from the preset path population, and each preset path in the preset path population is merged with the multiple elite paths in turn, the target path population includes: the optimized path corresponding to each preset path after the preset optimization times; in the target path population, the optimized path with the shortest total path distance is determined as the target path, so that the gray wolf optimization algorithm can be used to select the elite path in the preset path population, and the preset path in the preset path population can be optimized based on the elite path, which can improve the path optimization efficiency, achieve the purpose of quickly optimizing the planned path, and achieve the technical effect of improving the path planning efficiency, thereby solving the technical problem of poor three-dimensional trajectory planning effect of UAV. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of this application. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0028] Figure 1 is a flow chart of a path planning method according to an embodiment of the present invention;

[0029] Figure 2 is a schematic diagram of a UAV flight according to an embodiment of the present invention;

[0030] Figure 3 is a schematic diagram of shortest three-dimensional trajectory planning with load constraints according to an embodiment of the present invention;

[0031] Figure 4 is a schematic diagram of a method for three-dimensional trajectory planning of a single UAV in a complex environment according to an embodiment of the present invention;

[0032] Figure 5 is a schematic diagram of performing three-dimensional trajectory planning based on an RRT algorithm according to an embodiment of the present invention;

[0033] Figure 6 is a schematic diagram of path comparison before and after removing redundant nodes according to an embodiment of the present invention;

[0034] Figure 7 is a schematic diagram of a redundant node removal operation according to an embodiment of the present invention;

[0035] Figure 8 is a schematic diagram of a path smoothing operation according to an embodiment of the present invention Figure 1 ;

[0036] Fig. 9 is a schematic diagram of a node expansion operation according to an embodiment of the present invention;

[0037] Fig.10 is a schematic diagram of a path smoothing operation according to an embodiment of the present invention Figure 2 ;

[0038] Fig.11 is a schematic diagram of an encoding according to an embodiment of the present invention;

[0039] Fig.12 is a schematic diagram of decoding according to an embodiment of the present invention;

[0040] Fig.13 is a schematic diagram of a preset enclosing operation according to an embodiment of the present invention;

[0041] Fig.14 is a schematic diagram of a preset attack operation according to an embodiment of the present invention;

[0042] Fig.15 is a schematic diagram of three-dimensional space environment information according to an embodiment of the present invention;

[0043] Fig.16 is a schematic diagram of initially randomly generating individual routes according to an embodiment of the present invention;

[0044] Fig.17 It is a schematic diagram of an optimal individual route after convergence of a grey wolf optimization algorithm according to an embodiment of the present invention. DETAILED DESCRIPTION

[0045] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0046] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0047] First, some nouns or terms that appear in the description of the embodiments of the present application are subject to the following explanations:

[0048] Traveling Salesman Problem (TSP): is a classic combinatorial optimization problem. It can be described as: a salesman has to go to several cities to sell goods. The salesman starts from one city and needs to go through all cities before returning to the starting point. How should the route be selected to make the total journey shortest? From the perspective of graph theory, the problem is essentially to find a Hamilton circuit with the smallest weight in a weighted completely undirected graph. Since the feasible solution to this problem is the full arrangement of all vertices, as the number of vertices increases, a combinatorial explosion will occur, and it is an NP-complete problem. Due to its wide application in transportation, circuit board design, logistics and distribution, scholars at home and abroad have conducted a lot of research on it.

[0049] Rapidly-exploring Random Tree (RRT): is a classic random sampling search algorithm for path planning problems. The algorithm constructs a tree by random sampling and rapid expansion to find a feasible path from the starting point to the target point. The main steps of the RRT algorithm include: 1. Randomly sample a point; 2. Expand the tree, generate new nodes and connect to the nearest node; 3. Repeat the first two steps until the target point is found or the maximum number of iterations is reached. The RRT algorithm is simple and efficient, and can perform path planning in complex environments, but it may generate some less smooth trajectories.

[0050] Genetic Algorithm (GA): A type of randomized search method that evolved from the evolutionary laws of the biological world (survival of the fittest, genetic mechanism of survival of the fittest). It was first proposed by Professor J. Holland of the United States in 1975. Its main features are that it directly operates on structural objects, without the limitation of derivatives and function continuity; it has inherent implicit parallelism and better global optimization capabilities; it uses a probabilistic optimization method, can automatically obtain and guide the optimization search space, and adaptively adjust the search direction without the need for definite rules. These properties of genetic algorithms have been widely used in combinatorial optimization, machine learning, signal processing, adaptive control, artificial life and other fields. It is one of the key technologies in modern intelligent computing.

[0051] Grey Wolf Optimizer (GWO): A classic evolutionary algorithm proposed by Mirjalili S in 2014 for solving real number optimization problems.

[0052] Digital Elevation Model (DEM): A digital simulation of ground terrain (i.e., a digital expression of terrain surface morphology) achieved through limited terrain elevation data. It is a physical ground model that represents ground elevation in the form of a set of ordered numerical arrays. It is a branch of the digital terrain model, and various other terrain characteristic values ​​can be derived from it.

[0053] According to an embodiment of the present invention, a path planning method embodiment is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0054] Figure 1 is a flow chart of a path planning method according to an embodiment of the present invention. Figure 1 As shown, the method comprises the following steps:

[0055] Step S102, obtaining a set of nodes to be planned, wherein the set of nodes to be planned includes: a plurality of path nodes, and the plurality of path nodes includes: a starting point and at least one task point;

[0056] Step S104, determining a preset path population according to the set of nodes to be planned, wherein the preset optimization algorithm is used to determine multiple elite paths whose total path distance meets preset conditions from the preset path population during each optimization process, and sequentially merge each preset path in the preset path population with the multiple elite paths, and the target path population includes: the optimized path corresponding to each preset path after a preset number of optimizations;

[0057] Step S106, using a preset optimization algorithm to optimize the preset path population according to a preset optimization number of times to obtain a target path population, wherein the preset optimization algorithm is used to determine, in each optimization process, a plurality of elite paths whose total path distances meet preset conditions from the preset path population, and sequentially merge each preset path in the preset path population with the plurality of elite paths, and the target path population includes: an optimized path corresponding to each preset path after the preset optimization number of times;

[0058] Step S108: In the target path population, determine the optimized path with the shortest total path distance as the target path.

[0059] In an embodiment of the present invention, a set of nodes to be planned is obtained, wherein the set of nodes to be planned includes: a plurality of path nodes, and the plurality of path nodes include: a starting point and at least one task point; a preset path population is determined according to the set of nodes to be planned, wherein the preset path population includes: preset paths corresponding to a plurality of preset node sequences, both ends of each preset node sequence are starting points, and each preset node sequence includes all task points in the set of nodes to be planned, and each preset path includes: a target node path between each pair of adjacent path nodes in the preset node sequence; a preset optimization algorithm is used to optimize the preset path population according to a preset optimization number of times to obtain a target path population, wherein the preset optimization algorithm is used to optimize the preset path population in each optimization process , multiple elite paths whose total path distances meet preset conditions are determined from the preset path population, and each preset path in the preset path population is merged with the multiple elite paths in turn, the target path population includes: the optimized path corresponding to each preset path after the preset optimization times; in the target path population, the optimized path with the shortest total path distance is determined as the target path, so that the gray wolf optimization algorithm can be used to select the elite path in the preset path population, and the preset path in the preset path population can be optimized based on the elite path, which can improve the path optimization efficiency, achieve the purpose of quickly optimizing the planned path, and achieve the technical effect of improving the path planning efficiency, thereby solving the technical problem of poor three-dimensional trajectory planning effect of UAV.

[0060] As an optional example, the above path planning method can be used for path planning of UAVs. In the set of nodes to be planned, the starting point is the take-off position of the UAV, and the mission point is the location where the UAV needs to land / stay. After the UAV takes off from the starting point, it needs to pass through each mission point in the set of nodes to be planned before returning to the starting point.

[0061] Figure 2 is a schematic diagram of a UAV flight according to an embodiment of the present invention, such as Figure 2 As shown, the drone needs to be within the minimum flight altitude limit H during flight. min and the maximum flight altitude limit H max Fly between them so that the drone's flight altitude does not fall below the altitude of the obstacle, ensuring that the drone does not touch the obstacle.

[0062] In the above step S104, the preset path population may include: multiple preset node sequences, each preset node sequence is a random combination of all path nodes in the node set to be planned, wherein the first and last positions of the preset node sequence are both starting points in the node set to be planned, indicating starting from the starting point and passing through multiple task points before returning to the starting point.

[0063] In the above step S104, the preset node sequence is a combination of each path node in the node set to be planned, and the path between adjacent path nodes in the preset node sequence is the target node path. Then, the target node paths of adjacent path nodes in the preset node sequence are accumulated to obtain the preset path corresponding to the preset node sequence.

[0064] In the above step S104, the preset path population may include: a preset number of preset paths, wherein the more preset paths there are in the preset path population, the better the effect of path optimization, and the optimized target path may be closer to the most ideal path with the shortest total path distance. However, the more preset paths there are, the longer the optimization time is, and the optimization efficiency is not high. Therefore, pre-setting the preset number corresponding to the preset paths in the preset path population can further improve the optimization efficiency while ensuring the optimization result.

[0065] Figure 3 is a schematic diagram of a shortest three-dimensional trajectory planning with load constraints according to an embodiment of the present invention, such as Figure 3 As shown in the figure, the "★" point is the starting point of the drone, the "●" point is the mission point, the curve is the three-dimensional trajectory of the drone, the sphere is the no-fly zone, and the wavy line area is the actual mountain terrain, that is, the obstacle. In a three-dimensional space, suppose there are N points, numbered from 1 to N. The drone needs to find the shortest path that passes through each point once and returns to the starting point. The drone cannot enter the no-fly zone, cannot touch obstacles, and maintain a certain safe distance from obstacles during flight. The drone has a maximum flight altitude limit H max , which cannot be exceeded at any time during the flight.

[0066] As an optional embodiment, the three-dimensional trajectory planning problem of a UAV is essentially an integer programming model. To simplify the complexity of the model, a grid method is used to model a limited three-dimensional map. The spatial information of the map is represented by an n×m H matrix:

[0067]

[0068] Among them, each element in the matrix corresponds to the terrain (obstacle) height at that location.

[0069] Optionally, define a variable X ij Indicates whether there is a path from point i to point j. If there is a path, X ij The value of X is 1, otherwise ij The value of D is 0. ij Indicates the shortest flight distance of the drone from point i to point j. Point P t =(x t ,y t ,z t) represents the flight position of the UAV at time t, where z t It represents the flight altitude of the UAV at time t, and T is the total flight time.

[0070] Optionally, the decision variables for determining the three-dimensional trajectory of the UAV are:

[0071]

[0072] Optionally, the objective function of the three-dimensional trajectory of the UAV is determined as:

[0073] Indicates the total flight distance of the drone.

[0074] Optionally, the constraints for determining the three-dimensional trajectory of the UAV include:

[0075] This means that each mission point must be left exactly once;

[0076] This means that each mission point must be entered exactly once;

[0077] z t ≤H max t∈[0,T], indicating that the UAV cannot exceed the upper limit of flight altitude at any time during flight;

[0078] z t >H(x t ,y t )t∈[0,T], which means that the drone cannot touch obstacles at any position during flight.

[0079] Through the above model analysis, it can be seen that the three-dimensional trajectory planning problem of UAV consists of two parts. The first part is the trajectory planning between any two points, and the second part is the traveling salesman problem TSP.

[0080] As an optional example, the solution method of the three-dimensional trajectory planning problem of unmanned aerial vehicles can be divided into exact algorithms and heuristic algorithms. Exact algorithms include depth-first, breadth-first, branch-and-cut, set-covering, etc.; heuristic solutions include A* algorithm, RRT algorithm, genetic algorithm, particle swarm algorithm, ant colony algorithm, gray wolf algorithm, jellyfish algorithm, artificial potential field method, etc. Exact algorithms are feasible for small-scale problems, but as long as the data scale is large, its operation time is unacceptable and difficult to apply in real production and life. Although heuristic algorithms cannot guarantee that the optimal solution can be searched, the suboptimal solution they find is often similar to the global optimal solution, but the operation time is greatly reduced, so they are widely used. Swarm intelligence algorithm is a kind of heuristic algorithm. It is inspired by the laws of nature (biology) and imitates the algorithm for solving problems according to its principles. It is also called "soft computing". It has the advantages of strong robustness and wide adaptability, and is used to solve NP problems. It can be used to solve the shortest route planning problem.

[0081] It should be noted that the gray wolf optimization algorithm is a leader among heuristic algorithms. This algorithm is an evolutionary search algorithm based on the behavior of gray wolf groups in nature. It simulates the social behavior and group collaboration between individual gray wolves and finds the optimal solution by imitating the predation behavior of gray wolves.

[0082] In the above step S106, the preset optimization algorithm may be determined based on the gray wolf optimization algorithm, and the preset optimization algorithm may regard the preset path corresponding to each preset node sequence in the preset path population as a gray wolf individual.

[0083] In the above step S106, the preset conditions can be set according to the total path distance. The multiple elite paths that meet the preset conditions can be the first three preset paths with the shortest total path distance. The three elite paths with the shortest total path distance can be used as the elite gray wolf individuals Alpha gray wolf, Beta gray wolf and Delta gray wolf in the preset path population.

[0084] In the above step S106, the more times the preset path population is optimized using the preset optimization algorithm, the more likely it is that there will be a path with the shortest total path distance in the obtained target path population. However, as the number of optimizations increases, the effect of each optimization on shortening the path distance gradually becomes worse. Therefore, setting a reasonable preset number of optimizations can balance the optimization effect and optimization efficiency, and can maximize the optimization efficiency while ensuring that the optimization results are sufficiently good.

[0085] As an optional example, the gray wolf optimization algorithm includes the following steps:

[0086] Step S11, initializing the gray wolf group: setting the initial number of gray wolf individuals and randomly initializing the position of each individual.

[0087] Step S12, calculating the fitness value: according to the objective function of the problem, calculating the fitness value of each individual gray wolf.

[0088] Step S13, aggregate the gray wolf group: sort the gray wolf individuals according to their fitness values ​​to obtain an ordered gray wolf group.

[0089] Step S14, follow the elite gray wolves Alpha, Beta and Delta: select the elite gray wolf individual with the best fitness value as the Alpha gray wolf, followed by the Beta gray wolf and the Delta gray wolf.

[0090] Step S15, searching for a new position: using the position information of the Alpha gray wolf, the Beta gray wolf and the Delta gray wolf, updating the new positions of other gray wolf individuals and using them for the next iteration.

[0091] Step S16, termination condition determination: loop through steps S12 to S15 until a preset termination condition is reached.

[0092] In the above embodiment of the present application, the gray wolf optimization algorithm simulates the social behavior between gray wolves and uses group collaboration and individual competition to achieve the search for the global optimal solution. Compared with other optimization algorithms, the gray wolf optimization algorithm has a better convergence speed and global search capability, and has achieved good results in practical applications. The algorithm performs well on the CEC2005 test set and effectively avoids falling into the local optimum, but it is only applicable to real number optimization problems, and there is little research on combinatorial optimization problems.

[0093] As an optional embodiment, the path planning method includes: planning the order of each path node, and determining the moving path between each path node. Then, when path planning is performed with the shortest total path as the goal, the target node path with the shortest distance between any two path nodes in the set of nodes to be planned can be determined first, and then based on the target node path between adjacent nodes, multiple preset node sequences recorded in the preset path population are optimized to obtain the target path with the shortest total path distance.

[0094] In the above step S106, after each optimization of the preset path population, it can be detected whether the total path distance of the optimized path with the shortest total path distance in the preset path population after this optimization is less than the preset total distance threshold. If it is determined that the total path distance of the optimized path is less than the preset total distance threshold, the optimized path is determined as the target path and the optimization is terminated; otherwise, the optimization of the preset path population will continue until the preset optimization times are reached.

[0095] As an optional embodiment, after obtaining the set of nodes to be planned, the method also includes: combining any two path nodes in the set of nodes to be planned to obtain a path node set, wherein the path node set includes: multiple path node pairs, each path node pair includes: a first node and a second node; between the first node and the second node, planning a target node path with the shortest path distance.

[0096] In the above-mentioned embodiment of the present invention, the node set to be planned includes multiple path nodes. After obtaining the node set to be planned, any two path nodes in the node set to be planned can be combined, and each combination is used as a path node pair. Multiple path node sets can be obtained. Then, based on the path node sets, a target node path with the shortest path distance between each path node pair can be planned, thereby realizing the planning of the path between the path nodes.

[0097] As an optional embodiment, planning a target node path with the shortest path distance between a first node and a second node includes: using a rapidly expanding random tree to perform path planning for each pair of the first node and the second node according to a preset planning number of times to obtain multiple preset node paths, wherein the rapidly expanding random tree is used to use multiple tree nodes in the preset random tree to record the first node as the root node, the second node as the leaf node, and multiple sampling nodes randomly selected during this path planning process during each path planning process, and determine the preset node path according to the preset random tree; among the multiple preset node paths, determine the preset node path with the shortest path distance as the target node path.

[0098] In the above-mentioned embodiment of the present invention, the fast-expanding random tree constructs a tree by random sampling and fast expansion to find a feasible path from the starting point to the target point. When planning the target node path between the first node and the second node, the first node can be used as the starting point of the fast-expanding random tree, that is, the root node, and the second node can be used as the target point of the fast-expanding random tree, that is, the leaf node. When using the fast-expanding random tree to perform a path planning, multiple sampling nodes can be randomly planned between the first node and the second node to obtain a preset random tree with the first node as the root node, multiple sampling nodes as branch nodes, and the second node as the leaf node. The fast-expanding random tree is used to perform path planning according to the preset planning times, and multiple preset random trees that meet the preset planning times can be obtained. Each preset random tree is a preset node path. By comparing the path distances of multiple preset node paths, the target node path with the shortest path distance between the first node and the second node can be determined, thereby determining the target node path.

[0099] It should be noted that the rapidly expanding random tree is a heuristic algorithm that searches for feasible paths by randomly expanding the tree. However, due to its random nature, it can only find one feasible path and cannot guarantee that the found path is the global optimal path. Therefore, by using a repeated algorithm, multiple preset node paths can be generated according to the preset planning times, and the target node path with the shortest distance can be selected from the generated multiple preset node paths, so that the selected target node path can be closer to the global optimal path.

[0100] As an optional embodiment, the method also includes: in each path planning process, randomly selecting a sampling node that is not in a preset obstacle area from a preset three-dimensional space, wherein the preset three-dimensional space at least includes: a first node, a second node and a preset obstacle area; among at least one tree node recorded in a preset random tree, determining that the tree node closest to the sampling node is the target node, wherein the tree node at least includes: a first node; detecting whether the tree node path between the sampling node and the target node passes through the preset obstacle area; if the tree node path does not pass through the preset obstacle area, recording the sampling node as a tree node connected to the target node.

[0101] In the above embodiment of the present invention, each path planning can construct a corresponding preset random tree, the root node of the preset random tree is the first node, so the tree nodes of the preset random tree at least include the first node, and then in the process of each path planning, a sampling node that is not in the preset obstacle area can be randomly selected in the preset three-dimensional space including the first node and the second node, and then in the preset random tree corresponding to this path planning, the tree node closest to the sampling node is detected as the target node, and then the straight line connecting the sampling node and the target node is used as the tree node path, and it is detected whether the tree node path passes through the preset obstacle area. If the tree node path does not pass through the preset obstacle area, the sampling node is recorded as a tree node in the preset random tree, and the sampling node is connected to the target node in the preset random tree, and then multiple tree nodes are gradually determined to achieve the establishment of the preset random tree.

[0102] As an optional example, if the tree node path passes through a preset obstacle area, the establishment of the sampling nodes of the tree node path is abandoned.

[0103] Optionally, the preset obstacle areas include: no-fly zones that cannot be entered, and obstacles that cannot be touched.

[0104] As an optional example, the tree nodes of the preset random tree include: a first node as a root node, and a sampling node recorded in the preset random tree each time. Therefore, after each random selection of a sampling node, the first node or the sampling node recorded in the preset random tree can be used as the target node of the sampling node.

[0105] As an optional embodiment, the method also includes: when the tree node path does not pass through a preset obstacle area, detecting whether the path distance between the sampling node and the second node meets a preset distance threshold; when the path distance between the sampling node and the second node does not meet the preset distance threshold, randomly selecting sampling nodes that are not in the preset obstacle area in the preset three-dimensional space again until the number of sampling nodes recorded in the preset random tree reaches a first number threshold; when the path distance between the sampling node and the second node meets the preset distance threshold, recording the sampling node as a tree node connected to the target node, and recording the second node as a tree node connected to the sampling node.

[0106] In the above-mentioned embodiment of the present invention, when the tree node path does not pass through the preset obstacle area, the sampling node for constructing the tree node path can be recorded as a new tree node in the preset random tree, and it is detected whether the current path planning meets the end condition. If the end condition is met, it is determined that the current path planning is completed; otherwise, after recording the sampling node, a new sampling node needs to be selected again.

[0107] As an optional example, the end condition can be determined based on the path distance between the sampling node and the second node. By detecting the path distance between the sampling node and the second node that construct the tree node path, if the path distance between the sampling node and the second node meets a preset distance threshold, it is indicated that the sampling node meets the end condition, and then the sampling node is recorded as a new tree node after the preset random tree. The second node can also be used as a leaf node of the preset random tree, and the second node is connected to the sampling node in the preset random tree to end this path planning.

[0108] Optionally, when the path distance between the sampling node and the second node meets a preset distance threshold, it is also possible to detect whether the straight line connecting the sampling node and the second node passes through a preset obstacle area. If the straight line connecting the sampling node and the second node does not pass through the preset obstacle area, the termination condition is met, and then after recording the sampling node as a tree node in the preset random tree, the second node is recorded in the preset random tree, and the straight line connecting the sampling node and the second node is determined as the path between the sampling node and the second node.

[0109] Optionally, if the straight line connecting the sampling node and the second node does not pass through the preset obstacle area, the termination condition is not met, and after the sampling node is recorded as a tree node in the preset random tree, it is necessary to reselect the sampling node until the straight line connecting the reselected sampling node and the second node does not pass through the preset obstacle area.

[0110] As an optional example, the end condition can also be determined based on the number of sampling nodes. When the path distance between the sampling node and the second node does not meet the preset distance threshold, it means that the sampling node needs to be selected again. In this case, after recording the sampling node record, the sampling node needs to be reselected until the number of sampling nodes recorded in the preset random tree reaches the first number threshold, thereby terminating this path planning.

[0111] As an optional embodiment, among multiple preset node paths, determining a preset node path with the shortest path distance as a target node path includes: among multiple preset node paths, determining a preset node path with the shortest path distance as a first node path; eliminating redundant sampling nodes in the first node path to obtain a second node path, wherein the second node path includes: the second node path includes: a first node, a second node and necessary nodes, and the necessary nodes are sampling nodes that have not been eliminated; determining a fitting curve of multiple nodes to be fitted in the second node path to obtain a target node path, wherein the nodes to be fitted include: a first node, a second node and necessary nodes.

[0112] In the above-mentioned embodiment of the present invention, a preset random tree can be determined each time path planning is performed, and a preset node path can be determined based on multiple tree nodes in each preset random tree. Among the multiple preset node paths, the preset node path with the shortest path distance can be selected as the first node path, and then optimization is performed based on the first node path to obtain the target node path with the shortest path distance, thereby realizing the determination of the target node path.

[0113] As an optional example, a first node path is used to connect a first node and a second node, and the first node path passes through multiple sampling nodes between the first node and the second node. When optimizing the first node path, redundant sampling nodes in the first node path can be eliminated to obtain a second node path including a first node, a necessary node, and a second node. Then, by fitting the first node, the necessary node, and the second node in the second node path using a curve, the target node path can be determined based on the second node path.

[0114] The above-mentioned embodiment of the present invention can achieve path smoothing by eliminating redundant sampling nodes and using curves for path fitting. The smoothed path can help reduce turns, reduce navigation difficulty, and can also make the planned path closer to an actually feasible path.

[0115] As an optional embodiment, eliminating redundant sampling nodes in the first node path to obtain the second node path includes: placing the first node into a preset path stack; when the top node of the preset path stack changes, traversing each tree node in the random tree to be optimized in the direction from the leaf node to the root node until the necessary node is determined, wherein the random tree to be optimized is a preset random tree for determining the first node path, and the tree nodes in the random tree to be optimized include at least: a sampling node and a second node, and the stack node path between the necessary node and the top node of the stack does not pass through a preset obstacle area; placing the necessary node into the preset path stack until the second node is placed into the preset path stack; determining the second node path according to the first node, the necessary node, and the second node recorded in the preset path stack.

[0116] In the above embodiment of the present invention, the preset random tree is established in the direction from the root node to the leaf node. The same preset random tree may have multiple forked paths, but only one forked path can connect the root node and the leaf node. Therefore, after determining the preset random tree, it can be traversed in the direction from the leaf node to the root node to obtain the preset node path corresponding to the preset random tree, thereby realizing the determination of the preset node path.

[0117] In the above-mentioned embodiment of the present invention, when determining the preset node path according to the preset random tree, redundant sampling nodes can also be simultaneously eliminated, and then by constructing a preset path stack and traversing each tree node in the random tree to be optimized in the direction from the leaf node to the root node, a necessary node can be determined in each round of traversal, and then the necessary node is placed in the preset path stack to update the preset path stack, and the preset path stack is traversed again after the preset path stack is updated, and then multiple rounds of traversal are repeated until the second node is placed in the preset path stack, so that redundant sampling nodes can be eliminated from the first node path.

[0118] As an optional embodiment, when the top node of the preset path stack changes, traversing each tree node in the random tree to be optimized in the direction from the leaf node to the root node until the necessary node is determined includes: when the top node of the preset path stack changes, selecting a tree node as a node to be detected in the direction from the leaf node to the root node; detecting whether the stack node path between the node to be detected and the top node in the preset path stack passes through a preset obstacle area; when the stack node path does not pass through the preset obstacle area, determining the node to be detected of the determined stack node path as a necessary node; when the stack node path passes through the preset obstacle area, selecting the next tree node as a node to be detected in the direction from the leaf node to the root node until the necessary node is determined.

[0119] In the above-mentioned embodiment of the present invention, in each round of traversal, each tree node in the random tree to be optimized can be traversed in the direction from the leaf node to the root node, and the stack node path between each tree node and the top node in the preset path stack can be detected one by one to see whether it passes through the preset obstacle area. Then, when it is first discovered that the stack node path does not pass through the preset obstacle area, the tree node that constructs the stack node path is determined as a necessary node, and the necessary node is placed in the preset path stack to update the preset path stack, so that the top node of the preset path stack is changed to the necessary node. Then, each tree node in the random tree to be optimized is retraversed in the direction from the leaf node to the root node until the second node as a leaf node in the random tree to be optimized is also placed in the preset path stack to complete the construction of the preset path stack. Then, according to the first node, the necessary node, and the second node recorded in the preset path stack, the second node path can be determined to eliminate redundant sampling nodes.

[0120] Optionally, when the stack node path passes through the preset obstacle area, it means that the tree node is not a necessary node, then the next tree node is selected in the direction from the leaf node to the root node to reconstruct the stack node path for detection until the necessary node is determined.

[0121] As an optional embodiment, determining the fitting curve of multiple nodes to be fitted in the second node path to obtain the target node path includes: detecting whether the number of fitting nodes of the nodes to be fitted in the second node path reaches a second number threshold; when the number of fitting nodes reaches the second number threshold, using the curve to fit adjacent nodes to be fitted in the second node path in sequence to obtain the target node path.

[0122] In the above-mentioned embodiment of the present invention, the fitted curve is affected by the number of nodes to be fitted in the second node path. The more nodes to be fitted, the smaller the difference between the fitted curve and the original path; the fewer nodes to be fitted, the larger the difference between the fitted curve and the original path, and the larger the difference, the greater the probability that a legal path becomes illegal. Therefore, when using the curve for fitting, only the route at the corner is expected to be smooth, and the overall difference between the paths before and after fitting is expected to be small. Therefore, before fitting multiple nodes to be fitted in the second node path, it is necessary to detect whether the number of fitting nodes of the nodes to be fitted reaches the second number threshold. Then, when the number of fitting nodes of the nodes to be fitted reaches the second number threshold, the curve can be used to fit the nodes to be fitted of the second number threshold in sequence, so as to obtain a target node path that is closer to the actual feasibility.

[0123] As an optional embodiment, the method further includes: inserting an expansion node in the second node path when the number of fitting nodes does not reach a second number threshold; determining the expansion node as a node to be fitted until the number of fitting nodes reaches the second number threshold.

[0124] In the above embodiment of the present invention, when the number of fitting nodes of the node to be fitted does not reach the second number threshold, an expansion node is inserted in the second node path until the number of fitting nodes of the node to be fitted reaches the second number threshold, thereby expanding the node to be fitted.

[0125] As an optional embodiment, when the number of fitting nodes does not reach a second number threshold, inserting an expansion node in the second node path includes: when the number of fitting nodes does not reach the second number threshold, determining a pair of nodes to be fitted with the farthest adjacent distance in the second node path, wherein the pair of nodes to be fitted includes: two nodes to be fitted; and inserting the expansion node in the middle position between the two nodes to be fitted in the pair of nodes to be fitted.

[0126] In the above embodiment of the present invention, when expanding the nodes to be fitted in the second node path, a pair of nodes to be fitted with the farthest adjacent distance can be determined in the second node path, and then an expanded node can be inserted in the middle position of the pair of nodes to be fitted with the farthest adjacent distance to achieve the expansion of the nodes to be fitted.

[0127] It should be noted that the node to be fitted in the second node path is a necessary node, or a first node, or the second node. According to the method of determining the necessary node, it can be known that the straight path between adjacent nodes to be fitted does not pass through the preset obstacle area. Therefore, the expanded node inserted based on the adjacent nodes to be fitted is also not in the preset obstacle area.

[0128] As an optional embodiment, determining a preset path population based on a set of nodes to be planned includes: using a population initialization based on a greedy strategy to serialize the set of nodes to be planned according to a preset number of serialization times to obtain multiple preset node sequences, wherein the population initialization based on the greedy strategy is used to determine the preset processing strategy for serialization processing during each serialization process, and serialize the set of nodes to be planned according to the preset processing strategy to obtain a preset node sequence, and the preset processing strategy at least includes: individual initialization and random scrambling based on a greedy strategy; connecting target node paths between multiple pairs of adjacent path nodes in each preset node sequence to generate a preset path corresponding to the preset node sequence; and placing multiple preset node sequences and the preset path corresponding to each preset node sequence into the preset path population.

[0129] In the above-mentioned embodiment of the present invention, the process of determining the preset path population according to the set of nodes to be planned is equivalent to performing multiple serialization processes on the path nodes in the set of nodes to be planned to obtain a preset node sequence obtained by each serialization process. The multiple path nodes arranged in the preset node sequence can be decomposed into multiple adjacent path nodes, and then the target node paths between adjacent path nodes are connected in sequence according to the order of each path node recorded in the preset node sequence, so that the preset path corresponding to each preset node sequence can be obtained, and then multiple preset paths can be determined based on multiple preset node sequences to realize the determination of the preset path population.

[0130] It should be noted that there are multiple ways to serialize the path nodes in the planned node set. When the preset path population is determined, multiple serialization processing methods can be used to generate a preset node sequence. Each time a preset node sequence is generated, a greedy strategy can be used to select a serialization processing method for generating the preset node sequence.

[0131] As an optional embodiment, the method also includes: generating a first random number during each serialization process; detecting whether the first random number is less than a first greediness; when the first random number is less than the first greediness, using individual initialization based on a greedy strategy to put each path node in the set of nodes to be planned into a first initial node sequence in turn to obtain a preset node sequence, wherein the first position of the first initial node sequence is the starting point; when the first random number is not less than the first greediness, randomly scrambling the set of nodes to be planned to obtain a preset node sequence.

[0132] In the above-mentioned embodiment of the present invention, each serialization process can generate a preset node sequence. In the case of each serialization process, a first random number can be generated first, and then the first random number can be compared with a preset first greediness. When the first random number is less than the first greediness, the preset node sequence is generated by individual initialization based on the greedy strategy. When the first random number is not less than the first greediness, the preset node sequence is generated by random scrambling. This realizes the determination of the preset node sequence of the preset path population by using multiple serialization processing methods.

[0133] As an optional example, the first and last positions of the preset node sequence are both starting points. When the preset node sequence is determined by individual initialization based on a greedy strategy, the starting point can be first placed at the first position of the first initial node sequence, and then each path node can be placed in the first initial node sequence from the first position to the last position in sequence to obtain the preset node sequence.

[0134] As an optional embodiment, when the first random number is less than the first greediness, each path node in the node set to be planned is placed in turn into the first initial node sequence using individual initialization based on the greedy strategy, and obtaining the preset node sequence includes: when the first random number is less than the first greediness, the last path node in the first initial node sequence is determined as the target path node; according to the node set to be planned, a candidate node set of the target path node is determined, wherein the candidate node set includes: multiple candidate path nodes, and the selection probability of each candidate path node, the candidate path node is a path node in the node set to be planned that is not placed in the first initial node sequence, the selection probability is determined according to the path distance between the candidate path node and the target path node, and the path distance is inversely proportional to the selection probability; among the multiple candidate path nodes in the candidate node set, a candidate path node is selected and placed in the first initial node sequence; after the candidate path node is selected and placed in the first initial node sequence, the candidate path node is determined as the target path node until the preset node sequence is obtained.

[0135] In the above-mentioned embodiment of the present invention, a preset node sequence is determined based on individual initialization of a greedy strategy. It is necessary to place the path nodes in the node set to be planned one by one into the first initial node sequence. Then, when the path nodes are placed into the first initial node sequence, the path nodes that are not placed into the first initial node sequence can be determined as candidate path nodes based on the node set to be planned, and the selection probability of placing each candidate path node after the last target path node of the current first initial node sequence is determined to obtain a candidate node set. Then, the candidate path nodes to be placed into the first initial node sequence can be selected from the candidate node set according to the selection probability to achieve an update of the first initial node sequence, and then the candidate node set is re-determined according to the updated first initial node sequence until all the path nodes in the node set to be planned are placed into the first initial node sequence to achieve the determination of the preset node sequence.

[0136] Optionally, the preset node sequence includes two starting points, which are respectively at the first and last positions of the preset node sequence. After all the path nodes in the node set to be planned are placed in the first initial node sequence, the starting point needs to be placed at the last position of the first initial node sequence, so as to obtain the preset node sequence.

[0137] Optionally, the candidate path nodes in the candidate node set do not include a starting point.

[0138] Optionally, the selection probability of each alternative path node is determined based on the path distance between each alternative path node and the last target path node in the current first initial node sequence, wherein, when path planning is performed with the shortest total distance path as the goal, the closer the path distance between the alternative path node and the target path node, the greater the probability of the alternative path node being selected, and therefore the path distance is inversely proportional to the selection probability.

[0139] As an optional embodiment, selecting a candidate path node from multiple alternative path nodes in the candidate node set and placing it into the first initial node sequence includes: generating a second random number corresponding to the target path node; when the second random number is less than the second greediness, selecting the alternative path node with the largest selection probability from the candidate node set as the selected candidate path node; when the second random number is not less than the second greediness, using a roulette algorithm to randomly select a candidate path node from multiple alternative path nodes in the candidate node set, wherein the roulette algorithm is used to scale the sum of the selection probabilities of multiple alternative path nodes in the candidate node set to 1, and randomly select an alternative path node as a candidate path node according to the scaled selection probability; and connecting the candidate path node after the target path node in the first initial node sequence.

[0140] In the above-mentioned embodiment of the present invention, when selecting candidate path nodes to be placed in the first initial node sequence from multiple alternative path nodes in the candidate node set, the strategy for selecting the path node can be determined by using the greedy strategy each time the candidate path node is selected, thereby ensuring that there are differences in the multiple preset node sequences generated by multiple individual initializations based on the greedy strategy, thereby achieving the purpose of differentiated selection of candidate path nodes.

[0141] As an optional example, based on the roulette algorithm, scaling the sum of the selection probabilities of multiple alternative path nodes in the candidate node set to 1 is equivalent to: arranging the alternative areas corresponding to the multiple alternative path nodes in the candidate node set in the roulette wheel, the area of ​​each alternative path node in the roulette wheel is determined according to the selection probability of the alternative path node, and then randomly determining a selection point in the roulette wheel, and then the alternative area where the selection point is located can complete the selection of the candidate path node.

[0142] It should be noted that since the area of ​​each alternative path node in the roulette wheel is determined according to the selection probability of the alternative path node, the greater the selection probability of the alternative path node after scaling, the larger the area occupied by the alternative path node in the roulette wheel, and thus the greater the probability of the alternative path node being selected during the random selection process.

[0143] In the above-mentioned embodiment of the present invention, although the candidate path node with a greater selection probability has a greater probability of being selected, since the candidate path nodes are randomly selected using the roulette algorithm, the candidate path nodes with a lower selection probability may also be selected as candidate path nodes.

[0144] As an optional embodiment, the method also includes: after selecting a candidate path node and placing it into the first initial node sequence, deleting the candidate path node from the candidate node set; after deleting the candidate path node from the candidate node set, detecting whether the candidate node set is empty; if the candidate node set is empty, connecting the starting point after the candidate path node.

[0145] In the above-mentioned embodiment of the present invention, before placing the candidate path nodes in the candidate node set into the first initial node sequence, the candidate path nodes placed this time may be deleted from the candidate node set, and the number of remaining alternative path nodes in the candidate node set may be detected. If the number of remaining alternative path nodes in the candidate node set is zero, that is, the remaining alternative path nodes in the candidate node set are empty, then all path nodes in the node set to be planned have been placed into the first initial node sequence, and then the starting point is placed again at the end of the first initial node sequence to obtain a preset node sequence, thereby realizing the determination of the preset node sequence.

[0146] As an optional example, if the number of remaining candidate path nodes in the candidate node set is not zero, that is, the remaining candidate path nodes in the candidate node set are not empty, the candidate node set can be re-determined based on the remaining candidate path nodes in the candidate node set.

[0147] As an optional embodiment, when the first random number is not less than the first greediness, the set of nodes to be planned is randomly scrambled to obtain a preset node sequence, including: when the first random number is not less than the first greediness, all path nodes in the set of nodes to be planned are randomly scrambled to obtain a second initial node sequence; determining a first node sequence and a second node sequence in each second initial node sequence, wherein the first node sequence is the front sequence with the starting point as the last position in the initial node sequence, and the second node sequence is the back sequence with the starting point as the first position in the initial node sequence; moving the first node sequence to the end of the second node sequence to obtain a preset node sequence corresponding to each second initial node.

[0148] In the above-mentioned embodiment of the present invention, the first and last path nodes in the preset node sequence are both starting points. Therefore, when the preset node sequence is determined by random scrambling, all path nodes in the node set to be planned can be randomly scrambled first to generate a second initial node sequence, and the position of the starting point in the second initial node sequence can be identified. Then, the first node sequence and the second node sequence can be intercepted from the second initial node sequence according to the starting point, so that the first node sequence is a sequence segment including the first position to the starting point in the second initial node sequence, and the second node sequence is a sequence segment including the starting point to the last position in the second initial node sequence. Then, the positions of the first node sequence and the second node sequence are interchanged, so that the first node sequence and the second node sequence are spliced ​​into a preset node sequence with the first and last positions as starting points, thereby realizing the determination of the preset node sequence.

[0149] As an optional embodiment, a preset optimization algorithm is used to optimize the preset path population according to a preset optimization number of times to obtain a target path population, including: in each optimization process, detecting the fitness of each preset path in the preset path population, wherein the fitness is determined according to the total path distance of the preset path, and the fitness is proportional to the total path distance; selecting multiple preset paths with the largest fitness in the preset path population as multiple elite paths that meet preset conditions; merging each preset path in the preset path population with multiple elite paths in order of fitness from high to low to obtain a target merged path corresponding to each preset path; updating the preset path population according to the multiple target merged paths, and optimizing again until the preset optimization number is reached.

[0150] In the above-mentioned embodiment of the present invention, when the preset path population is optimized by using the preset optimization algorithm, each optimization process can detect the fitness of each preset path in the preset path population. The greater the fitness of the preset path, the shorter the total path distance of the preset path. Therefore, in each optimization process, multiple preset paths with the largest fitness can be selected from the preset path population as elite paths, and then each preset path in the preset path population is merged with multiple elite paths in turn. The merging order can be performed in the order of high to low fitness. After each preset path is optimized, a corresponding target merged path can be obtained. After the multiple preset paths of the preset path population are optimized, a preset path population reconstructed by multiple target merged paths can be obtained, thereby realizing the preset path population. Then, by optimizing the preset path population according to the preset optimization times, the preset path population obtained by the last optimization is the target path population, thereby realizing high-efficiency optimization of the preset path population.

[0151] Optionally, multiple preset paths in the preset path population may be sorted in descending order of fitness, and then multiple preset paths may be selected from the first in the queue as elite paths according to preset conditions.

[0152] As an optional embodiment, the multiple elite paths include at least a first elite path, a second elite path, and a third elite path arranged in descending order of fitness, and each preset path in the preset path population is merged with the multiple elite paths in descending order of fitness to obtain a target merged path corresponding to each preset path, including: fusing the preset path in the preset path population with the first elite path to obtain a first merged path; fusing the first merged path with the second elite path to obtain a second merged path; fusing the second merged path with the third elite path to obtain a third merged path; and determining the third merged path as the target merged path corresponding to the preset path.

[0153] In the above embodiment of the present invention, each optimization process of the preset path population can arrange multiple elite paths from large to small according to fitness, and obtain an elite path queue whose sequence is a first elite path, a second elite path and a third elite path, and then randomly select a preset path from the preset path population to merge with the first elite path to obtain a first merged path, and then merge the first merged path with the second elite path to obtain a second merged path, and finally merge the second merged path with the third elite path to obtain a third merged path, then the third merged path is the target merged path corresponding to the preset path, thereby achieving one-time optimization of the preset path.

[0154] As an optional embodiment, the method also includes: when each preset path is fused with each elite path, randomly selecting a preset path from the preset path population as a path to be optimized; detecting whether the path to be optimized belongs to an elite path; when the path to be optimized does not belong to an elite path, using a preset encirclement operation to generate a target merged path based on the path to be optimized, wherein the preset encirclement operation is used to randomly select a path node in the path to be optimized as a first target node; determining a path node adjacent to the first target node in the elite path as a second target node; intercepting multiple path nodes between the first target node and the second target node and including the second target node in the path to be optimized to obtain a first preset node subsequence; reversing the first preset node subsequence and inserting it into the path to be optimized so that the second target node is adjacent to the first target node; when the path to be optimized belongs to an elite path, using a preset attack operation to generate a target merged path based on the path to be optimized, wherein the preset attack operation is used to randomly intercept a second preset node subsequence in the path to be optimized, and reversing the second preset node subsequence and inserting it into the original position.

[0155] The above-mentioned embodiment of the present invention utilizes the gray wolf optimization algorithm to merge each preset path with each elite path, which can simulate the preset encirclement operation and preset attack operation of the gray wolf. The preset encirclement operation is used to enable the ordinary gray wolf (that is, the preset path) to cooperate with the elite wolf (that is, the elite path) to perform a global search to achieve global optimization. The preset attack operation is used to enable the elite wolf (that is, the elite path) to perform a local search to achieve local optimization. Therefore, the preset encirclement operation and the preset attack operation can balance the global search and local optimization, improve the search efficiency, and then improve the optimization efficiency of the path.

[0156] The present invention also provides an optional embodiment, which provides a method for three-dimensional trajectory planning of a single UAV in a complex environment. Aiming at the problems existing in the traditional gray wolf optimization algorithm in solving the three-dimensional trajectory planning of a single UAV in a complex environment, an improved gray wolf optimization algorithm is proposed to solve such a combination optimization problem to reduce the flight cost of the UAV, comprising the following steps:

[0157] Step S21, using the RRT algorithm to plan the route: a random sampling-based method is used to quickly generate sampling nodes, and the tree structure is expanded to adapt to complex environments, which has wide applicability.

[0158] Step S21, select the shortest route and optimize: by running the RRT algorithm multiple times, select the route with the shortest path distance (i.e., the target node path) and optimize it to retain the optimal solution and increase the search probability of the global optimal solution.

[0159] Step S23, deleting redundant sampling nodes for optimization: By deleting redundant sampling nodes, the route is made more compact, unnecessary turns and straight segments are avoided, the path efficiency is improved, and time and energy are saved.

[0160] Step S24, curve smoothing: smoothing the route to reduce the mechanical pressure of the drone during flight and make the system more stable. Inserting expansion nodes through the node expansion algorithm can achieve a balance between smoothness and feasibility while keeping close to the original route.

[0161] Step S25, population initialization algorithm based on greedy strategy: Introducing a population initialization algorithm based on a greedy strategy, considering the selection probability of path nodes, generating an initial population close to the optimal solution (such as a preset path population), which can improve local search capabilities, reduce the risk of falling into a local optimal solution, accelerate convergence speed, improve the quality of the initial solution, and enhance the feasibility of the problem space.

[0162] Step S26, new population evolution strategy: by adjusting the evolution parameters and drawing on the strategies of the best, second-best, and third-best individuals (such as the first elite path, the second elite path, and the third elite path), a dynamic balance between global search and local search is achieved. The collaborative behavior of gray wolves in nature is simulated, with extensive exploration and deep optimization capabilities.

[0163] Figure 4 is a schematic diagram of a method for three-dimensional trajectory planning of a single UAV in a complex environment according to an embodiment of the present invention. Figure 4 As shown, the steps include:

[0164] Step S41, initializing parameters, reading data, and loading models, specifically includes: loading data from a data set and initializing a model.

[0165] Step S42, using the RRT algorithm to plan a route between any two points (such as planning a target node path).

[0166] Step S43, three-dimensional track optimization.

[0167] Step S44, encoding and decoding.

[0168] Step S45, population initialization based on the greedy strategy.

[0169] Step S46, calculating the fitness of the population, specifically includes: performing fitness evaluation to determine whether the termination condition is met, if the termination condition is met, determining the optimal individual; if the termination condition is not met, executing step S47.

[0170] Step S47, based on the population update strategy of gray wolf optimization, specifically includes: obtaining the first three elite wolves (such as the first elite path, the second elite path and the third elite path), improving the wolf pack update operation, and performing fitness evaluation.

[0171] Step S48, elite strategy.

[0172] Step S49, update the optimal individual (i.e. update the target path with the shortest total path distance), then return to step S46, evaluate whether the updated optimal individual meets the termination condition, if the termination condition is met, determine the optimal individual; if the termination condition is not met, execute step S47 again.

[0173] As an optional embodiment, Figure 4 The three-dimensional trajectory planning method for a single UAV in a complex environment shown in the figure can be implemented by solving the three-dimensional trajectory planning problem of a single UAV in a complex environment by using the Grey Wolf Optimization Algorithm (GWO) shown in Algorithm 1. The pseudo code of Algorithm 1 is shown below.

[0174]

[0175] Algorithm 1 of the present application initializes the model and plans the route between any two path nodes through the RRT algorithm to obtain a distance matrix; then initializes the population through the importance and greediness of the greedy factor; next, obtains the elite wolves through loop iteration, and updates the population according to the evolutionary algorithm; finally, determines the optimal final population through the elite selection strategy and population update.

[0176] In the above step S41, the initialization parameters include: determining the population size popSize (scalar), the maximum evolution generation maxGen (scalar), the data set dataFile (set), the greed factor importance beta (scalar), and the greediness greedyEpsilon (scalar).

[0177] In the above step S41, reading data includes: reading the data set dataFile, including the coordinates of the starting point of the drone (x1, y1, z1), the coordinates of each task point (x i ,y i ,z i )(i=2,3,...,N), elevation data of the UAV flight space (dem file, n×m two-dimensional matrix), no-fly zone, size of the UAV, and maximum flight altitude.

[0178] In the above step S41, loading the model includes: calculating the decision variable dimension D=N, drawing a terrain map, a no-fly zone, a starting point of the UAV and a mission point of the UAV.

[0179] In the above step S42, the RRT algorithm is used to plan the route between any two points, including: through two layers of loops, traversing all the two-by-two combinations of the three-dimensional coordinates of N nodes (including the starting point and the task point), each group includes the first node and the second node, a total of N (N-1) combinations, and then using the RRT algorithm to plan the route between the two path nodes respectively, including the following steps:

[0180] Step S51, determining the first node and the second node: that is, the above-mentioned combined coordinate pair.

[0181] Step S52, initializing RRT: setting the first node as the root node of the tree, and setting an empty tree structure.

[0182] Step S53, random sampling: randomly selecting a sampling node in the three-dimensional space for constructing the RRT.

[0183] Step S54, finding the nearest node: finding a tree node closest to the sampling node from the tree as the target node, wherein the tree node includes at least one path node, and the at least one path node includes at least the first node.

[0184] Step S55, expand the tree: create a new sampling node between the first node and the second node, generate a route, and check whether the route has a collision with an obstacle, whether it passes through a no-fly zone, and whether it exceeds the maximum flight altitude. If yes, abandon the sampling node and return to step S53; otherwise, add the sampling node to the tree.

[0185] Step S56, judging whether the target is reached: checking whether the new sampling node is close to the second node, if so, proceeding to step S57, otherwise returning to step S53.

[0186] Step S57, constructing the optimal path: starting from the second node, backtracking to the root node (such as the first node) through the parent pointer of the node, and constructing the optimal path from the first node to the second node (that is, determining the preset node path).

[0187] Step S58, repeatedly executing steps S53 to S57 until an optimal path that meets the requirements is found or a set number of iterations is reached (that is, until the number of sampling nodes recorded in the preset random tree reaches a first number threshold).

[0188] Figure 5 is a schematic diagram of performing three-dimensional trajectory planning based on the RRT algorithm according to an embodiment of the present invention, such as Figure 5 As shown in the figure, the RRT algorithm searches for feasible solutions in space through random sampling and tree expansion, and gradually builds a tree structure to approach the target point, and finally obtains a collision-free route planning path. The "★" point is the starting point, the "●" point is the target point, the gray line is the route generated during the random exploration of the RRT algorithm (such as the preset node path), and the white line is the successful route finally searched out (such as the target node path).

[0189] Alternatively, the RRT algorithm is a heuristic search algorithm that searches for feasible paths by randomly expanding the tree. Due to its random nature, it cannot guarantee to find the global optimal path, but can only find one feasible path, making it difficult to guarantee the optimal solution.

[0190] It should be noted that the RRT algorithm is a heuristic search algorithm that searches for feasible paths by randomly expanding trees. Due to its random nature, it cannot guarantee that the global optimal path will be found, and can only find one feasible path. Therefore, there is a problem that it is difficult to guarantee the optimal solution. In the RRT algorithm, the line between the randomly sampled node and the nearest neighbor node (such as the path node closest to the sampling node) is usually a straight line, which may cause the generated path to contain many redundant straight line segments, resulting in the problem of generating redundant paths, which increases the length of the path and the execution time. Due to the random sampling and random connection of nodes, the path generated by the RRT algorithm is usually non-smooth, with many turns and corners, and the problem of non-smooth planned paths, which does not meet the actual motion requirements.

[0191] In the above step S43, for problems that are difficult to ensure the optimal solution, the algorithm can be repeated to generate routes multiple times (such as generating multiple preset node routes), and the route with the shortest distance (such as the target node path) is retained. The number of repetitions k in the actual algorithm (that is, the number of sampling nodes recorded in the preset random tree) is 10.

[0192] In the above step S43, for the problem of "generating redundant paths", a redundant node elimination algorithm is used to add the first node to the shortest path list (such as a preset path stack), and then the entire path node is traversed from the second node forward through a loop (such as traversing each tree node in the random tree to be optimized in the direction from the leaf node to the root node until the necessary node is determined). If the top node of the stack can be directly connected to the current traversal node (no collision, no passing through a no-fly zone, and no exceeding the maximum flight altitude), the current node is pushed into the stack (that is, the necessary node is determined), and the current loop is terminated, and the entire path node is traversed from the second node forward to repeat the above operation until the second node is pushed into the stack.

[0193] Table 1 is a schematic table of path nodes planned by using the RRT algorithm according to an embodiment of the present invention. As shown in Table 1, the path between the first node and the second node includes 103 path nodes.

[0194] Table 1

[0195]

[0196]

[0197] Table 2 is a schematic table of path nodes after redundant nodes are eliminated according to an embodiment of the present invention. As shown in Table 2, after redundant nodes are eliminated based on the 103 path nodes shown in Table 1, 9 path nodes are obtained.

[0198] Table 2

[0199]

[0200] Figure 6 is a schematic diagram of path comparison before and after removing redundant nodes according to an embodiment of the present invention, such as Figure 6 As shown, the red line is the path before removing redundant sampling nodes, and the black line is the path after removing redundant sampling nodes.

[0201] Figure 7 is a schematic diagram of a redundant node removal operation according to an embodiment of the present invention, such as Figure 7 As shown, in order to facilitate the comparison of the paths before and after removing redundant sampling nodes, you can delete Figure 6Obstacles in the three-dimensional map shown, where the red line is the path before removing redundant sampling nodes, and the black line is the path after removing redundant sampling nodes.

[0202] In the above step S43, for the problem of uneven path, the path can be smoothed using curve fitting, interpolation or Bezier curve techniques, etc. Smoothing the path helps reduce turning and navigation difficulty, and can also make the path closer to an actual feasible path.

[0203] It should be noted that the fitted curve is affected by the number of initial path nodes. The more initial path nodes, the smaller the difference between the fitted curve and the original path; the fewer initial path nodes, the greater the difference between the fitted curve and the original path. The greater the difference, the greater the probability that a legal path becomes illegal. Therefore, it is hoped that the difference between the path before and after fitting should not be too large, and only smoothing is desired at the corners of the route.

[0204] Figure 8 is a schematic diagram of a path smoothing operation according to an embodiment of the present invention Figure 1 ,like Figure 8 As shown, a path smoothing operation is performed on the 9 path nodes (ie, nodes to be fitted) shown in Table 2, wherein the black line is the path before smoothing and the red line is the path after smoothing.

[0205] Fig. 9 is a schematic diagram of a node expansion operation according to an embodiment of the present invention, such as Fig. 9 As shown, the 9 path nodes (i.e., nodes to be fitted) shown in Table 2 are expanded to 20 path nodes (i.e., nodes to be fitted), wherein the triangular points are the 9 path nodes (i.e., nodes to be fitted) before the expansion, and the circular points are the 20 path nodes (i.e., nodes to be fitted) after the expansion.

[0206] Table 3 is a schematic table of expanded path nodes according to an embodiment of the present invention. As shown in Table 3, the 9 path nodes (i.e., nodes to be fitted) shown in Table 2 are expanded to 20 path nodes (i.e., nodes to be fitted).

[0207] Table 3

[0208]

[0209] Fig.10 is a schematic diagram of a path smoothing operation according to an embodiment of the present invention Figure 2 ,like Fig.10 As shown, a path smoothing operation is performed on the 20 path nodes (ie, nodes to be fitted) shown in Table 3, wherein the blue line is the path before smoothing and the red line is the path after smoothing.

[0210] The above embodiments of the present invention, Figure 8 and Fig.10 The paths before smoothing are the same, only the number of path nodes (i.e., nodes to be fitted) during smoothing is different. Fig.10 Compared with the smooth curve Figure 8 The smooth curve in the middle fits the original path better.

[0211] As an optional embodiment, a minimum sampling node threshold (such as a second number threshold) needs to be set before smoothing the route. When the current number of path nodes is lower than this threshold, the path nodes need to be expanded until the threshold is met. The expansion of path nodes must follow two principles: (1) the path cannot change before and after the path node expansion, that is, only path nodes on the original path can be selected; (2) the newly added path nodes are as far away from adjacent path nodes as possible, that is, to ensure that all path nodes are as evenly distributed as possible on the original path.

[0212] As an optional example, in the case of expanding the path nodes, it can be implemented by the path node expansion algorithm shown in Algorithm 2, and the pseudo code of Algorithm 2 is shown below.

[0213]

[0214] In the above-mentioned algorithm 2 of the present application, the longest line segment in the path is selected each time, and the point in the middle is taken as the newly expanded path node (i.e., the expanded node) and added to the path node list. The above operation is repeated until the total number of nodes (i.e., the number of fitting nodes) meets the minimum threshold requirement, thereby realizing the expansion of the path node.

[0215] In the above step S44, through encoding and decoding operations, a route of the drone connecting one starting point and multiple mission points can be constructed (that is, the preset paths corresponding to multiple preset node sequences are determined).

[0216] Fig.11 is a schematic diagram of an encoding according to an embodiment of the present invention, such as Fig.11 As shown in the figure, for a problem with 1 starting point and N-1 task points, we need to search for a sequence of visiting these N points in order to make the total distance the shortest. The size of the decision space is N! Each arrangement may be the shortest path sequence. Therefore, the encoding process can scramble "1~N" through a random function to obtain an individual.

[0217] Fig.12 is a schematic diagram of decoding according to an embodiment of the present invention, such as Fig.12 As shown in the figure, since the drone must start from the starting point, and the randomly generated series cannot be guaranteed to start from the starting point, adjustments need to be made during decoding. The position of the starting point "1" is found from the individual, and the sequence before "1" is transferred to the end of the sequence. Fig.12 Since we have to return to the starting point in the end, we need to add "1" at the end. Fig.12 The "decoding" phase.

[0218] In the above step S45, the individuals in the initial population of the traditional grey wolf optimization algorithm are generated by random scrambling, which is effective in dealing with small-scale problems, but obviously ineffective in solving large-scale problems. Therefore, this application adopts population initialization based on a greedy strategy.

[0219] As an optional example, the above step S45 can be implemented using the greedy strategy-based population initialization shown in Algorithm 3, and the pseudo code of Algorithm 3 is shown below.

[0220]

[0221] The above algorithm 3 of the present application generates a population based on the input parameters such as the distance matrix distanceMat, the importance of the greedy factor beta, the greedy degree greedyEpsilon (also known as the first greedy degree), and calculates the access probability matrix between the path nodes (that is, used to represent the path distance between the alternative path node and the target path node) on the basis of the distance matrix (that is, used to represent the path distance between the alternative path node and the target path node); creates an empty population pop according to the population size popSize and the decision variable dimension numOfDecVariables. For each individual, a starting point startPoint is randomly selected. If the random number (that is, the first random number) is less than the greedy degree greedyEpsilon (that is, the first greedy degree), an individual (such as a preset node sequence) is generated according to the starting point and the access probability matrix using the individual greedy strategy (that is, individual initialization based on the greedy strategy), otherwise, an individual (such as a preset node sequence) is randomly generated.

[0222] As an optional example, the individual greedy strategy can be implemented by individual initialization based on the greedy strategy shown in Algorithm 4, and the pseudo code of Algorithm 4 is shown below.

[0223]

[0224] The above algorithm 4 of the present application, based on the individual greedy strategy getIndividualByGreedy (that is, individual initialization based on the greedy strategy), can generate an individual (such as a preset node sequence) according to the given starting point startPoint, the accessible node set pointSet (that is, the candidate node set), the access probability matrix proOfVisitingMat (that is, used to represent the selection probability of each candidate path node) and the greediness greedyEpsilon (that is, the second greediness); specifically including: creating an empty individual individual, and putting the starting point startPoint into the individual; according to the index of the last node of the current individual in the accessible node set pointSet, select the next node with the maximum probability from the access probability matrix proOfVisitingMat; if the random number (that is, the second random number) is less than the greediness greedyEpsilon (that is, the second greediness), select the node with the maximum probability, otherwise, select the next node according to the probability. Finally, update the individual and the accessible node set (that is, the candidate node set).

[0225] It should be noted that using the greedy algorithm to initialize the population can simply and efficiently construct an approximate optimal solution, but it cannot guarantee the global optimal solution. Therefore, when applying the greedy algorithm to initialize the population, it is necessary to weigh the size of the greediness degree greedyEpsilon (i.e., the second greediness degree) and the selection of other parameters to achieve a better algorithm effect. Usually, the greediness factor importance beta ranges from [1,5], and the greediness degree greedyEpsilon (such as the first greediness degree and the second greediness degree) ranges from [0.5,1].

[0226] In the above step S46, calculating the fitness of the population includes: taking an individual from the population as individual, decoding the individual to obtain a specific path. According to the distance matrix of distanceMat, the distance of each path is calculated respectively, and the total distance is recorded as allPathDistance, then the fitness of the individual individualFitness = -allPathDistance, the greater the fitness, the better the individual. Repeat the above steps popSize times (i.e., the preset optimization times), and calculate the fitness of all individuals in the population and record it as popFitness.

[0227] In the above step S47, the basic gray wolf optimization algorithm is used to solve the real number optimization problem. The gray wolf position is updated by moving to the three elite wolves α, β, and δ, and synthesizing the position of the new wolf through tracking, encirclement and attack operations, but it is not suitable for solving combinatorial optimization problems; therefore, this application designs a population update strategy based on gray wolf optimization.

[0228] As an optional example, based on the concept of the gray wolf optimization algorithm, the population evolution strategy shown in Algorithm 5 is designed for the characteristics of the three-dimensional trajectory planning problem of a single UAV. The pseudo code of Algorithm 5 is shown below.

[0229]

[0230] In the above algorithm 5 of this application, the population evolution strategy is the population update part, which is mainly used for the population evolution of the optimization problem. In each iteration, each individual in the current population is processed. For each individual, by calling the updateWolfStrategy function, the individual's strategy is updated based on the evolution parameter a and the optimal individual α (such as the first elite path), the suboptimal individual β (such as the second elite path) and the third optimal individual δ (such as the third elite path) of the current population; this strategy update process involves the simulation of the hunting behavior of gray wolves. The entire algorithm simulates the collaborative behavior of gray wolves in nature by constantly drawing on and integrating the excellent individual strategies in the current population, thereby guiding the entire population to evolve in the direction of a better solution. This inspiration comes from the hierarchical structure and collaborative behavior in the gray wolf society, which enables the algorithm to have certain search and convergence capabilities when solving combinatorial optimization problems.

[0231] As an optional example, the population evolution strategy is implemented based on the individual movement strategy, and the individual movement strategy is implemented by the gray wolf movement operation shown in Algorithm 6. The pseudo code of Algorithm 6 is shown below.

[0232]

[0233] In the above-mentioned algorithm 6 of this application, the gray wolf movement operation is a key step in the gray wolf optimization algorithm. The main idea is to randomly select a code position J based on the evolution parameter a, and then judge whether to perform a global search (such as a preset encirclement operation) or a local search (such as a preset attack operation) according to the size of the random number. In the global search, the individual coordinates the strategy with the elite individual through the preset encirclement operation to achieve global exploration; while in the local search, the elite makes local adjustments through the preset attack operation in the hope of finding a more local optimal solution. The dynamic adjustment of this strategy helps to balance global exploration and local optimization in the search space and improve the search efficiency of the algorithm. The evolution parameter a decreases linearly with the population iteration, so the focus is on global search in the early stage of population evolution and on local search in the later stage of evolution.

[0234] As an optional example, the global search is implemented by a preset encirclement operation shown in Algorithm 7, and the pseudo code of Algorithm 7 is shown below.

[0235]

[0236] In the above algorithm 7 of the present application, the preset encirclement operation is mainly used for global search. The algorithm takes the individual wolf to be updated (such as the preset path in the preset path population), the target individual goalWolf (such as the elite path) and the decision variable bit number j to be updated as input.

[0237] Fig.13 is a schematic diagram of a preset enclosing operation according to an embodiment of the present invention, such as Fig.13 As shown, get the length of the decision variable and copy the individual to be updated to the new individual. Then, get the value G1 on the jth dimension from the individual to be updated. By finding the position index of the value equal to G1 in the target individual, if there are multiple equal values, select one randomly. Next, determine the position of G1 in the target individual. If there is a right position, take the right side, otherwise take the left side. This obtains the value G2 adjacent to G1. Then, find the position rJ of G2 in the individual to be updated. Similarly, if there are multiple equal values, select one randomly. Next, according to the relationship between j and rJ, adjust the positions of G1 and G2 in the new individual to ensure that they are adjacent in the new individual. Finally, return the new individual.

[0238] In the above embodiments of the present application, the core idea of ​​the encirclement operation is to improve the similarity with the target individual (such as the elite path) by adjusting a certain dimension of the individual to be updated (such as the preset path in the preset path population). This can be considered as the individual to be updated (such as the preset path in the preset path population) approaching the target individual (such as the elite path), thereby prompting the individual to better explore the new solution space.

[0239] As an optional example, the global search is implemented by a preset attack operation shown in Algorithm 8, and the pseudo code of Algorithm 8 is shown below.

[0240]

[0241] In the above algorithm 7 of the present application, the attack operation is preset to perform local search. The algorithm takes the individual wolf to be updated (such as the elite path) and the decision variable bit number j to be updated as input.

[0242] Fig.14 is a schematic diagram of a preset attack operation according to an embodiment of the present invention, such as Fig.14 As shown, get the length of the decision variable and randomly get an integer rJ in the range of 1 to numOfDecVariables; sort j and rJ from small to large and record them as r0 and r1 respectively; copy the individual wolf to be updated (such as the elite path) to the new individual newWolf (such as the elite path). In the new individual, reverse the elements between positions r0 and r1. Finally, return the new individual newWolf.

[0243] In the above-mentioned embodiments of the present application, the purpose of the attack operation is to introduce changes in the local search by reversing the variables in the local area so as to better explore the local solution space, which helps the Grey Wolf Optimization Algorithm to improve the convergence and global search capabilities of the algorithm.

[0244] It should be noted that in the population evolution algorithm, the parent population generates the offspring population through evolutionary strategy selection. The offspring population is likely to be better than the parent population, but not absolutely. If the offspring population is directly used to replace the parent population for the next evolution, it may lead to the loss of the best individuals in the current population, which is specifically manifested as local oscillation of the evolution curve and difficulty in population convergence. Therefore, an elite strategy can be adopted. In each generation of the evolutionary algorithm, the parent population and the offspring population are merged, sorted according to fitness, and the excellent individuals ranked in the top half of the fitness in the merged population are selected as the new generation of population.

[0245] In the above step S48, by introducing the elite strategy, the population evolution algorithm can retain the information of excellent solutions in the search space and pass it on to future generations. This selective retention helps to avoid the degradation of solutions and pushes the algorithm towards a better solution during the evolution process. At the same time, the elite strategy can also speed up the convergence of the algorithm, especially when the search space has many local optimal solutions.

[0246] In the above step S49, through the above RRT route planning, route smoothing, population initialization, population update, and new population fitness calculation, an elite strategy is adopted to retain the population with high fitness as the offspring population. The optimal individual and optimal fitness of each new generation of population are determined and saved. When the iterative evolution of the population reaches the maximum evolutionary generation, the loop is jumped out, the optimal individual is output, and the actual route is generated by decoding.

[0247] In the above-mentioned embodiments of the present application, the algorithm converges quickly, obtains good results, and is not prone to falling into local optimum.

[0248] The technical solution provided by the present application uses RRT to plan the route in the initial stage, which has wide applicability. Based on the random sampling method, it can quickly generate sample points in the environment, and expand the tree structure through iteration, so that it can adapt to non-convex and complex environments; the RRT algorithm has probability completeness in theory, that is, the probability of finding a feasible path in a limited time is close to 1, which makes it feasible and reliable in many practical applications; repeatedly running the RRT algorithm, selecting the shortest route, and optimizing it, helps to retain the optimal solution. In this way, the probability of searching for the global optimal solution can be increased without sacrificing the quality of the path; by eliminating redundant points, the optimized route is more compact, avoiding unnecessary turns and straight segments in the original path, and such a path is more efficient, saving time and energy; by smoothing the route, the flight path presents a smoother curve when connecting adjacent nodes, which helps to reduce the mechanical pressure of the drone during flight, reduce sharp changes in flight, and improve the stability of the system; by keeping close to the original route during smoothing, the strategy achieves a balance between smoothness and feasibility. This is crucial for the flight requirements and mechanical elements that need to be considered in actual operations; the population initialization algorithm based on the greedy strategy proposed in this application, by considering the selection probability of the node, can more effectively generate an initial population close to the optimal solution in large-scale problems, improve local search capabilities, reduce the risk of falling into the local optimal solution, accelerate the convergence speed, improve the quality of the initial solution, and enhance the feasibility of the problem space; the new population evolution strategy proposed in this application, by flexibly adjusting the evolution parameter a and drawing on the strategy of the optimal, suboptimal, and third-optimal individuals (such as elite paths), the algorithm effectively updates the individuals in each iteration, simulating the collaborative behavior of gray wolves in nature. The dynamic balance between its global search (such as preset encirclement operations) and local search (such as preset attack operations) enables the algorithm to have both extensive exploration and deep optimization capabilities in the search space.

[0249] As an optional embodiment, taking an example with 1 starting point, 8 mission points, 224×331 elevation data, and 4 no-fly zones as an example, a single UAV's three-dimensional trajectory planning is performed.

[0250] Table 4 is a schematic table of starting point and task point information according to an embodiment of the present invention. As shown in Table 4, it includes 1 starting point and 8 task points.

[0251] Table 4

[0252]

[0253] Table 5 is a schematic table of no-fly zone information according to an embodiment of the present invention. As shown in Table 5, four no-fly zones are included.

[0254] Table 5

[0255]

[0256] Table 6 is a schematic table of terrain elevation data according to an embodiment of the present invention. As shown in Table 6, it includes 224×331 elevation data.

[0257] Table 6

[0258]

[0259]

[0260] Fig.15 is a schematic diagram of three-dimensional space environment information according to an embodiment of the present invention. Fig.15 As shown, based on the 1 starting point and 8 mission points shown in Table 4, the 4 no-fly zones shown in Table 5, and the 224×331 elevation data shown in Table 6, a visualization view of the 3D spatial environment information is constructed.

[0261] Fig.16 is a schematic diagram of initially randomly generating individual routes according to an embodiment of the present invention, such as Fig.16 As shown, based on Fig.15 The three-dimensional spatial environment information shown generates completely random individual routes.

[0262] Fig.17 is a schematic diagram of an optimal individual route after convergence of a grey wolf optimization algorithm according to an embodiment of the present invention, such as Fig.17 As shown, the order of the route visiting the target points is determined to be [1, 2, 8, 6, 3, 5, 7, 9, 4, 1], and the driving distances are [48.96, 71.46, 64.08, 44.65, 97.51, 111.92, 62.22, 29.79, 210.23] respectively.

[0263] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A path planning method, characterized in that: include: Acquire a set of nodes to be planned, wherein the set of nodes to be planned includes: a plurality of path nodes, and the plurality of path nodes include: a starting point and at least one task point; Determine a preset path population according to the node set to be planned, wherein the preset path population includes: preset paths corresponding to a plurality of preset node sequences, both ends of each preset node sequence are the starting points, and each preset node sequence includes all the task points in the node set to be planned, and each preset path includes: a target node path between each pair of adjacent path nodes in the preset node sequence; The preset path population is optimized according to a preset optimization number of times using a preset optimization algorithm to obtain a target path population, wherein the preset optimization algorithm is used to determine, during each optimization process, a plurality of elite paths whose total path distances meet preset conditions from the preset path population, and to sequentially merge each of the preset paths in the preset path population with the plurality of elite paths, wherein the target path population includes: an optimized path corresponding to each of the preset paths after the preset optimization number of times; In the target path population, the optimized path with the shortest total path distance is determined as the target path.

2. The method according to claim 1, characterized in that After obtaining the set of nodes to be planned, the method further includes: Combining any two of the path nodes in the set of nodes to be planned to obtain a path node set, wherein the path node set includes: a plurality of path node pairs, each of the path node pairs includes: a first node and a second node; A target node path with the shortest path distance is planned between the first node and the second node.

3. The method according to claim 2, characterized in that Planning a target node path with the shortest path distance between the first node and the second node includes: Using a fast expanding random tree to perform path planning for each pair of the first node and the second node according to a preset planning number of times, to obtain a plurality of preset node paths, wherein the fast expanding random tree is used to use a plurality of tree nodes in a preset random tree to record the first node as a root node, the second node as a leaf node, and a plurality of sampling nodes randomly selected during this path planning process in each path planning process, and determine the preset node path according to the preset random tree; Among the plurality of preset node paths, the preset node path with the shortest path distance is determined as the target node path.

4. The method according to claim 3, characterized in that The method further comprises: In each path planning process, a sampling node that is not in a preset obstacle area is randomly selected from a preset three-dimensional space, wherein the preset three-dimensional space at least includes: the first node, the second node and the preset obstacle area; Among at least one of the tree nodes recorded in the preset random tree, determine the tree node closest to the sampling node as the target node, wherein the tree nodes at least include: the first node; Detecting whether a tree node path between the sampling node and the target node passes through the preset obstacle area; In the case that the tree node path does not pass through the preset obstacle area, the sampling node is recorded as the tree node connected to the target node.

5. The method according to claim 4, characterized in that The method further comprises: In the case where the tree node path does not pass through the preset obstacle area, detecting whether the path distance between the sampling node and the second node meets a preset distance threshold; When the path distance between the sampling node and the second node does not meet the preset distance threshold, randomly selecting sampling nodes that are not in the preset obstacle area in the preset three-dimensional space again until the number of the sampling nodes recorded in the preset random tree reaches a first number threshold; When the path distance between the sampling node and the second node meets the preset distance threshold, the sampling node is recorded as the tree node connected to the target node, and the second node is recorded as the tree node connected to the sampling node.

6. The method according to claim 3, characterized in that: Among the plurality of preset node paths, determining the preset node path with the shortest path distance as the target node path comprises: Among the plurality of preset node paths, determining the preset node path with the shortest path distance as the first node path; Eliminate the redundant sampling nodes in the first node path to obtain a second node path, wherein the second node path includes: the second node path includes: the first node, the second node and necessary nodes, and the necessary nodes are the sampling nodes that have not been eliminated; Determine fitting curves of a plurality of nodes to be fitted in the second node path to obtain the target node path, wherein the nodes to be fitted include: the first node, the second node and the necessary node.

7. The method according to claim 6, characterized in that The redundant sampling nodes in the first node path are removed, and the second node path is obtained, including: Put the first node into a preset path stack; In the case where the top node of the preset path stack changes, traverse each tree node in the random tree to be optimized in a direction from the leaf node to the root node until the necessary node is determined, wherein the random tree to be optimized is the preset random tree for determining the first node path, and the tree nodes in the random tree to be optimized at least include: the sampling node and the second node, and the stack node path between the necessary node and the top node does not pass through a preset obstacle area; placing the necessary nodes into the preset path stack until the second node is placed into the preset path stack; The second node path is determined according to the first node, the necessary node and the second node recorded in the preset path stack.

8. The method according to claim 7, characterized in that In the case where the top node of the preset path stack changes, each tree node in the random tree to be optimized is traversed in the direction from the leaf node to the root node until it is determined that the necessary nodes include: When the top node of the preset path stack changes, selecting the tree node as the node to be detected in the direction from the leaf node to the root node; Detecting whether a stack node path between the node to be detected and a top node in a preset path stack passes through a preset obstacle area; In the case that the stack node path does not pass through the preset obstacle area, determining the node to be detected of the stack node path as the necessary node; In the case where the stack node path passes through the preset obstacle area, the next tree node is selected as the node to be detected in the direction from the leaf node to the root node until the necessary node is determined.

9. The method according to claim 6, characterized in that Determining fitting curves of a plurality of nodes to be fitted in the second node path to obtain the target node path includes: Detecting whether the number of fitting nodes of the node to be fitted in the second node path reaches a second number threshold; When the number of the fitting nodes reaches the second number threshold, a curve is used to sequentially fit the adjacent nodes to be fitted in the second node path to obtain the target node path.

10. The method according to claim 9, characterized in that The method further comprises: When the number of the fitting nodes does not reach the second number threshold, inserting an expansion node into the second node path; The expanded nodes are determined as the nodes to be fitted until the number of the fitting nodes reaches the second number threshold.

11. The method according to claim 10, characterized in that When the number of the fitting nodes does not reach the second number threshold, inserting an expansion node into the second node path includes: When the number of the fitting nodes does not reach the second number threshold, determining a pair of nodes to be fitted with the longest adjacent distance in the second node path, wherein the pair of nodes to be fitted includes: two nodes to be fitted; The expansion node is inserted at a middle position between two nodes to be fitted in the pair of nodes to be fitted.

12. The method according to claim 1, characterized in that Determining a preset path population according to the set of nodes to be planned includes: Using a population initialization based on a greedy strategy, the set of nodes to be planned is serialized according to a preset number of serialization times to obtain a plurality of preset node sequences, wherein the population initialization based on the greedy strategy is used to determine a preset processing strategy for serialization processing this time during each serialization process, and serialize the set of nodes to be planned according to the preset processing strategy to obtain the preset node sequence, and the preset processing strategy at least includes: individual initialization based on a greedy strategy and random scrambling; Connecting the target node paths between multiple pairs of adjacent path nodes in each of the preset node sequences to generate the preset paths corresponding to the preset node sequences; Put the plurality of preset node sequences and the preset path corresponding to each preset node sequence into the preset path population.

13. The method according to claim 12, characterized in that The method further comprises: During each serialization process, a first random number is generated; Detecting whether the first random number is less than a first greediness; When the first random number is less than the first greediness, each of the path nodes in the set of nodes to be planned is sequentially placed into a first initial node sequence using individual initialization based on a greedy strategy to obtain the preset node sequence, wherein the first node of the first initial node sequence is the starting point; When the first random number is not less than the first greediness, the set of nodes to be planned is randomly scrambled to obtain the preset node sequence.

14. The method according to claim 13, characterized in that When the first random number is less than the first greediness, each of the path nodes in the set of nodes to be planned is sequentially placed into a first initial node sequence using individual initialization based on a greedy strategy, and the preset node sequence is obtained, including: When the first random number is less than the first greediness, determining the last path node in the first initial node sequence as a target path node; Determine a candidate node set of the target path node according to the set of nodes to be planned, wherein the candidate node set includes: a plurality of candidate path nodes, and a selection probability of each of the candidate path nodes, the candidate path node is the path node in the set of nodes to be planned that is not placed in the first initial node sequence, the selection probability is determined according to a path distance between the candidate path node and the target path node, and the path distance is inversely proportional to the selection probability; Selecting a candidate path node from among the plurality of candidate path nodes in the candidate node set and putting it into the first initial node sequence; After selecting a candidate path node and putting it into the first initial node sequence, the candidate path node is determined as the target path node until the preset node sequence is obtained.

15. The method according to claim 14, characterized in that Selecting a candidate path node from among the plurality of candidate path nodes in the candidate node set and putting it into the first initial node sequence comprises: Generate a second random number corresponding to the target path node; When the second random number is less than the second greediness, selecting the candidate path node with the highest selection probability from the candidate node set as the candidate path node; In the case where the second random number is not less than the second greediness, the candidate path node is randomly selected from the multiple candidate path nodes in the candidate node set using a roulette algorithm, wherein the roulette algorithm is used to scale the sum of the selection probabilities of the multiple candidate path nodes in the candidate node set to 1, and randomly select the candidate path node as the candidate path node according to the scaled selection probability; The candidate path node is connected after the target path node in the first initial node sequence.

16. The method according to claim 14, characterized in that The method further comprises: After selecting a candidate path node and putting it into the first initial node sequence, deleting the candidate path node from the candidate node set; After deleting the candidate path node from the candidate node set, detecting whether the candidate node set is empty; When the candidate node set is empty, the starting point is connected after the candidate path node.

17. The method according to claim 13, characterized in that When the first random number is not less than the first greediness, randomly scrambling the set of nodes to be planned to obtain the preset node sequence includes: When the first random number is not less than the first greediness, all the path nodes in the set of nodes to be planned are randomly scrambled to obtain a second initial node sequence; Determine a first node sequence and a second node sequence in each of the second initial node sequences, wherein the first node sequence is a front segment sequence with the starting point as the last position in the initial node sequence, and the second node sequence is a back segment sequence with the starting point as the first position in the initial node sequence; The first node sequence is moved to the end of the second node sequence to obtain the preset node sequence corresponding to each second initial node.

18. The method according to claim 1, characterized in that The preset path population is optimized according to the preset optimization times using the preset optimization algorithm to obtain the target path population including: In each optimization process, detecting the fitness of each preset path in the preset path population, wherein the fitness is determined according to the total path distance of the preset path, and the fitness is proportional to the total path distance; Selecting a plurality of the preset paths with the largest fitness from the preset path population as a plurality of the elite paths meeting the preset conditions; Merging each of the preset paths in the preset path population with the multiple elite paths in order of fitness from high to low, to obtain a target merged path corresponding to each of the preset paths; The preset path population is updated according to the plurality of target merged paths, and optimization is performed again until the preset optimization times are reached.

19. The method according to claim 18, characterized in that The multiple elite paths at least include a first elite path, a second elite path, and a third elite path arranged in descending order of fitness. Each of the preset paths in the preset path population is merged with the multiple elite paths in descending order of fitness to obtain a target merged path corresponding to each preset path, including: Merging the preset path in the preset path population with the first elite path to obtain a first merged path; The first merged path is merged with the second elite path to obtain a second merged path; Merging the second merged path with the third elite path to obtain a third merged path; The third merged path is determined as the target merged path corresponding to the preset path.

20. The method according to claim 18, characterized in that The method further comprises: When each of the preset paths is merged with each of the elite paths, the preset paths are randomly selected from the preset path population as the paths to be optimized; Detecting whether the path to be optimized belongs to the elite path; In the case that the path to be optimized does not belong to the elite path, the target merged path is generated based on the path to be optimized using a preset encirclement operation, wherein the preset encirclement operation is used to randomly select the path node in the path to be optimized as the first target node; determine the path node adjacent to the first target node in the elite path as the second target node; intercept a plurality of the path nodes between the first target node and the second target node and including the second target node in the path to be optimized to obtain a first preset node subsequence; and insert the first preset node subsequence into the path to be optimized after reversing the order, so that the second target node is adjacent to the first target node; In the case that the path to be optimized belongs to the elite path, a preset attack operation is used to generate the target merged path based on the path to be optimized, wherein the preset attack operation is used to randomly intercept a second preset node subsequence in the path to be optimized, and insert the second preset node subsequence into the original position after reversing the order.

Images