Unmanned aerial vehicle three-dimensional path planning method based on hybrid ant colony algorithm

Abstract

The invention discloses an unmanned aerial vehicle three-dimensional path planning method based on a hybrid ant colony algorithm. The invention provides an unmanned aerial vehicle three-dimensional path planning method based on an ant colony algorithm combined with a particle swarm algorithm in order to solve the problems that a traditional path planning algorithm is low in search efficiency, poor in path quality, insufficient in constraint satisfaction rate and the like in unmanned aerial vehicle path planning in a three-dimensional mountainous area environment. The maximum pitch angle constraint and the maximum steering angle constraint of the unmanned aerial vehicle are comprehensively considered, and the maximum course angle constraint of the unmanned aerial vehicle is set. According to the method, global search is carried out by using a particle swarm algorithm to obtain an initial path, local fine optimization is carried out by using an improved ant colony algorithm, and efficient and high-quality path planning is realized by combining the advantages of the two.

Description

Technical Field

[0001] This invention relates to the field of UAV path planning, and is based on a method combining an improved ant colony algorithm and a particle swarm algorithm to realize 3D path planning for UAVs in 3D mountainous environments. Background Technology

[0002] In the field of UAV 3D path planning, the feasibility, length, smoothness of the path, as well as the efficiency and stability of the algorithm, directly affect the flight performance and mission completion quality of the UAV. As a core component of autonomous UAV flight, path planning provides safe and efficient flight trajectories for UAVs through algorithm search and optimization.

[0003] Traditional path planning algorithms (such as A*, RRT*, classic ant colony optimization, particle swarm optimization, and genetic algorithms) face multiple challenges in complex 3D mountainous scenarios: A* algorithm's running time increases significantly in large-scale search spaces, resulting in poor timeliness; RRT algorithm has a large standard deviation of path length and insufficient stability; while particle swarm optimization and genetic algorithms have certain global search capabilities, they are limited in satisfying UAV kinematic constraints (such as maximum heading angle), and the constraint satisfaction rate decreases as terrain complexity increases; classic ant colony optimization suffers from problems such as blind search in the early stages, easy getting trapped in local optima, and insufficient path smoothness, and its search efficiency and path quality need to be improved in complex terrain.

[0004] These issues make it difficult for traditional algorithms to simultaneously consider path length, smoothness, algorithm efficiency, and constraint satisfaction rate in three-dimensional mountainous environments, thus failing to provide optimal flight trajectories for UAVs. Therefore, designing a path planning algorithm that can adapt to complex three-dimensional mountainous environments and has better overall performance has become a key requirement for the development of UAV path planning technology. Summary of the Invention

[0005] To overcome the problems of low search efficiency, poor path quality, and insufficient constraint satisfaction in UAV path planning in 3D mountainous environments, traditional path planning algorithms propose a UAV 3D path planning method based on an ant colony algorithm combined with particle swarm optimization. In the UAV 3D path planning scenario, this invention decomposes the 3D environment into multiple 2D planes. Each individual plane can be considered a 2D environment plane, called a tangent plane. A tangent plane refers to a 2D subspace formed by geometrically partitioning the 3D environment (e.g., extracting sections along a specific axis or at specific intervals), aiming to provide a simplified 2D representation for path planning in specific regions or at specific levels. Considering both the maximum pitch angle constraint and the maximum turn angle constraint of the UAV, a maximum heading angle constraint is set. This method first uses a particle swarm optimization algorithm for global search to obtain the initial path, and then uses an improved ant colony algorithm for local fine-tuning, combining the advantages of both to achieve efficient and high-quality path planning. The algorithm framework diagram of this invention is shown below. Figure 1 As shown.

[0006] The technical solution adopted in this invention includes the following steps:

[0007] Step 1: Read and load 3D mountain terrain data, build a 3D environment model based on the raster method, and set the start and end points containing (x,y,z) coordinates;

[0008] Step 2: Execute the particle swarm algorithm for global search, construct the search dimension, initialize the particle swarm position and velocity, use the sum of the Euclidean distances of the paths as the fitness function, iteratively update the particle position and velocity, and output a preliminary feasible initial path;

[0009] Step 3: Construct a pheromone map based on the initial path obtained by the particle swarm optimization algorithm, and enhance the pheromone concentration of the path and its surrounding areas to provide search guidance for the ant colony algorithm;

[0010] Step 4: Execute the ant colony algorithm. Ants move according to pheromone values ​​and heuristic functions, and update pheromones using a fitness function that balances path length and smoothness, combined with the maximum heading angle constraint.

[0011] Step 5: After reaching the iteration limit, output the final optimized 3D path of the UAV.

[0012] Compared with the prior art, the beneficial effects of the present invention are:

[0013] (1) Previous ant colony algorithms have problems such as blind search in the early stage and low search efficiency. This invention combines the global search advantage of particle swarm algorithm, obtains the initial path through particle swarm algorithm and enhances the pheromone in the corresponding area, provides effective guidance for ant colony algorithm, and improves the search efficiency and convergence speed of the algorithm.

[0014] (2) Previous path planning algorithms have focused on a single indicator in path quality evaluation. This invention introduces a comprehensive fitness function that takes into account both path length and smoothness in the ant colony algorithm. Through the pheromone update mechanism, the algorithm is more inclined to select high-quality paths with both short distance and small turning angle, thereby improving path quality.

[0015] (3) Previous algorithms have limitations in adapting to complex terrain. This invention adopts dynamic adjustment of search step size and combines maximum heading angle constraint to enable the algorithm to better adapt to complex three-dimensional mountain terrain, ensuring the safety and feasibility of the path. Attached Figure Description

[0016] Appendix Figure 1 : Path search pattern diagram for UAV 3D path planning.

[0017] Appendix Figure 2 Flowchart of the drone-hybrid ant colony algorithm. Detailed Implementation

[0018] The invention will now be further described with reference to the accompanying drawings.

[0019] Step 1: Read the 3D mountain terrain data, model it into a 3D raster map using the raster method, specify the horizontal and vertical coordinates of the start and end points according to the task requirements, obtain the corresponding ground height from the terrain data to supplement the z value, and determine the 3D coordinates of the start and end points.

[0020] Step 2: Execute the Particle Swarm Optimization (PSO) algorithm. Construct the search dimension according to the set number of intermediate nodes, randomly generate initial position and velocity for each particle, and initialize individual optimal and global optimal parameters. During the iteration process, calculate the fitness of each particle (the sum of the Euclidean distances along the paths) according to the formula... and Update the particle's velocity and position, continuously update the individual and global optimal solutions, and output the initial feasible path after reaching the iteration limit;

[0021] Step 3: Construct a pheromone map, formula The pheromone of the initial path obtained by the particle swarm algorithm and its surrounding area (within a distance of no more than a set radius r) is enhanced, while the pheromone of the remaining areas is set to the default initial pheromone value;

[0022] Step 4: Execute the improved ant colony algorithm. Ants on the pheromone map select their next moving node by considering distance, direction guidance, and angle factors. (Combined with the formula...) Determine if the heading angle meets the constraints (the maximum heading angle is set to 60°). If it does, the route can be reached directly. If it does not meet the constraints, calculate the optimal intermediate point. If a direct connection is not possible, dynamically adjust the step size based on factors such as terrain to move forward.

[0023] Step 5: After the ant completes the path, the path quality is measured by the comprehensive fitness function calculated by the formula C = w1·L + w2·S (which combines the path length L and the path smoothness S, where the path smoothness is calculated by the horizontal and vertical turns), and the pheromone is updated based on this.

[0024] Step 6: Repeat steps 4 and 5 until the set number of iterations is reached, and output the final 3D path of the UAV.

Claims

1. A method for UAV 3D path planning using a hybrid ant colony algorithm, characterized in that, Includes the following steps: Step 1: Read the 3D mountain terrain data, model it into a 3D raster map using the raster method, specify the horizontal and vertical coordinates of the start and end points according to the task requirements, obtain the corresponding ground height from the terrain data to supplement the z value, and determine the 3D coordinates of the start and end points. Step 2: Execute the Particle Swarm Optimization (PSO) algorithm. Construct the search dimension according to the set number of intermediate nodes, randomly generate initial position and velocity for each particle, and initialize individual optimal and global optimal parameters. During the iteration process, calculate the fitness of each particle (the sum of the Euclidean distances along the paths) according to the formula... and Update the particle's velocity and position, continuously update the individual and global optimal solutions, and output the initial feasible path after reaching the iteration limit; Step 3: Construct a pheromone map using the formula The pheromone of the initial path obtained by the particle swarm algorithm and its surrounding area (within a distance of no more than a set radius r) is enhanced, while the pheromone of the remaining areas is set to the default initial pheromone value; Step 4: Execute the improved ant colony algorithm. Ants on the pheromone map select their next moving node by considering distance, direction guidance, and angle factors. (Combined with the formula...) Determine if the heading angle meets the constraints (the maximum heading angle is set to 60°). If it does, the route can be reached directly. If it does not meet the constraints, calculate the optimal intermediate point. If a direct connection is not possible, dynamically adjust the step size based on factors such as terrain to move forward. Step 5: After the ant completes the path, the path quality is measured by the comprehensive fitness function calculated by the formula C = w1·L + w2·S (which combines the path length L and the path smoothness S, where the path smoothness is calculated by the horizontal and vertical turns), and the pheromone is updated based on this. Step 6: Repeat steps 4 and 5 until the set number of iterations is reached, and output the final 3D path of the UAV.

2. The method as described in claim 1, characterized in that, In step 2, the particle swarm optimization algorithm continuously updates the fitness and historical best of each particle based on position feasibility and path length during the iteration process, and adjusts the particle speed and position through inertia weight, cognitive term and social term.

3. The method as described in claim 1, characterized in that, In step 3, a pheromone map is constructed and preprocessed to provide search guidance for the ant colony algorithm in the next stage.

4. The method as described in claim 1, characterized in that, After the condition for stopping iteration is met in step 5, the fitness function value is evaluated, and the final optimized path obtained by the hybrid algorithm is output.

Images