An unmanned aerial vehicle three-dimensional path planning method based on improved snake optimization algorithm
By improving the snake optimization algorithm, constructing urban terrain and cost functions, and using non-monotonic factors and golden sine strategies to optimize UAV 3D trajectory planning, the problems of the algorithm being prone to getting trapped in local optima, high path redundancy, and slow convergence speed are solved, and the shortest path is quickly planned and energy consumption is optimized.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- SICHUAN UNIVERSITY OF SCIENCE AND ENGINEERING
- Filing Date
- 2026-03-25
- Publication Date
- 2026-06-09
Smart Images
Figure CN121898437B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of UAV three-dimensional trajectory planning, and in particular to a UAV three-dimensional trajectory planning method based on an improved snake optimization algorithm. Background Technology
[0002] In recent years, research on drone technology has received widespread attention worldwide. Path planning is a prerequisite for autonomous drone operation. Its core purpose is to plan a feasible path from the starting point to the target point for the drone, which requires consideration of constraints such as terrain environment, path length, elevation difference, and smoothing angle.
[0003] Mainstream algorithms for UAV 3D trajectory planning include heuristic algorithms, machine learning, metaheuristic algorithms, and hybrid algorithms. Heuristic algorithms (such as A* and Rapid Expanding Random Tree (RRT)) have simple structures but lack adaptability to complex environments. Machine learning relies on neural networks to generate and optimize paths, but suffers from high training costs. Metaheuristic algorithms (such as Genetic Algorithm (GA), Particle Swarm Optimization (PSO), and Snake Optimization (SO)) each have their own drawbacks; for example, GA has low optimization accuracy, PSO is prone to premature convergence, and SO is prone to getting trapped in local optima. Hybrid algorithms often use metaheuristic algorithms as a framework, integrating other algorithms or strategies to compensate for the limitations of a single algorithm, but they are unlikely to change the underlying mechanism of the algorithm itself.
[0004] While current UAV 3D trajectory planning methods have achieved considerable success, they still suffer from the following limitations: 1) The algorithms are prone to getting trapped in local optima, making it difficult to find the shortest path that satisfies the constraints; 2) The generated paths have high redundancy and inadequate handling of local details; 3) In terrain with multiple obstacles, the algorithms converge slowly and tend to bypass dense obstacle clusters, leading to increased planned path length and ultimately increased UAV energy consumption. Therefore, how to quickly plan the shortest and most optimal trajectory in complex terrain is a major problem that urgently needs to be solved. Summary of the Invention
[0005] To address the aforementioned shortcomings in existing technologies, this invention provides a UAV 3D trajectory planning method based on an improved snake optimization algorithm. This method solves the following problems existing in UAV 3D trajectory planning algorithms in practical engineering: 1) The algorithm is prone to getting trapped in local optima, making it difficult to find the shortest path that satisfies the constraints; 2) The generated paths have high redundancy and inadequate handling of local details; 3) In multi-obstacle terrain, the algorithm converges slowly and the generated paths tend to bypass dense obstacle groups, which leads to an increase in the length of the planned path and ultimately increases the energy consumption of the UAV.
[0006] To achieve the above objectives, the technical solution adopted by this invention is: a UAV three-dimensional trajectory planning method based on an improved snake optimization algorithm, comprising the following steps:
[0007] S1. Construct the city terrain;
[0008] S2. Construct a cost function that includes path length, elevation difference, smoothing angle, no-fly zone, and building collision, and form a UAV flight model with urban terrain as the background.
[0009] S3. Initialize the snake optimization algorithm parameters and divide the population into male and female categories after initialization.
[0010] S4. Enter the iteration process, based on the food index. The size of the value determines whether to enter the exploration or development stage, and in this process, non-monotonic factors are used. Food Index Make dynamic adjustments;
[0011] S5, when the food index When the value is less than the first preset threshold, the exploration phase begins, and the process proceeds to S9.
[0012] S6, when the food index When the temperature exceeds a first preset threshold, the development phase begins. During the development phase, the temperature index is used to determine the appropriate parameters. The size of the [something] determines whether to initiate foraging or interactive behavior, and utilizes non-monotonic factors. Temperature index Make dynamic adjustments;
[0013] S7, When the food index The temperature index is greater than the first preset threshold and When the value exceeds the second preset threshold, the snake swarm continues its foraging behavior and enters S9;
[0014] S8, When the food index The temperature index is greater than the first preset threshold and When the value is less than the second preset threshold, the snake swarm enters interactive behavior and enters S9, which involves combat mode and the golden sine strategy.
[0015] S9. Update individual information and determine the iteration count. Has the maximum number of iterations been reached? If yes, then exit the loop and output the optimal individual position; otherwise, return to S4.
[0016] S10. Based on the optimal individual position, the UAV flight model is used to calculate the UAV's three-dimensional trajectory plan.
[0017] Furthermore, the construction of urban terrain includes the following steps:
[0018] Initialize a 20×20 zero matrix;
[0019] Assign random values within the range [10, 25] to the preset elements in the zero matrix, where a value of 0 indicates no building and a non-zero value indicates the building height at the corresponding location;
[0020] Based on the result of random value assignment, differentiated values are assigned to different sub-regions of the zero matrix;
[0021] Based on the differentiated assignment results, the zero matrix is expanded in dimension to generate a 1000×1000 matrix;
[0022] For each non-zero element in the generated 1000×1000 matrix, a building rectangle region is constructed. Starting from the coordinates of the non-zero element, an integer in the range [20,30] is randomly selected to the right as the building width, and an integer in the range [20,30] is randomly selected downwards as the building length. The non-zero element values are assigned to all elements within the building rectangle region to present the spatial distribution of the buildings and complete the construction of the urban terrain.
[0023] Furthermore, the expression for the cost function is as follows:
[0024]
[0025]
[0026]
[0027]
[0028]
[0029]
[0030]
[0031]
[0032]
[0033] in, Represents the cost function, , and All represent weight parameters. Represents the distance parameter. Indicates the elevation difference parameter. Indicates the smooth angle parameter. This indicates the cost of collisions in no-fly zones. Indicates the cost of building collisions, ( , , ) indicates the drone's first The three-dimensional coordinates of each track point, , , ) indicates the drone's first The three-dimensional coordinates of each track point This indicates the total number of waypoints. This represents the sum of the altitude differences between all waypoints. Indicates the drone's first The altitude of each waypoint Indicates the drone's first The first waypoint, the first The first waypoint and the first The angle between two vectors formed by the coordinates of the track points Indicates the drone's first The first waypoint, the first The first waypoint and the first The angle between two vectors formed by the coordinates of the track points 、 and This indicates the coordinates of three consecutive waypoints. ( , , ) indicates the drone's first The three-dimensional coordinates of each track point Indicates the drone's first The cost of collisions at each waypoint Indicates the drone's first The distance from each trackpoint to the center of the no-fly zone Representing an interval random numbers, Representing an interval random numbers, Indicates the cost of the collision zone. Indicates the cost of the warning zone. Indicates the drone's first The cost of collisions at each waypoint Indicates the area occupied by the building x coordinate range Indicates the area occupied by the building y coordinate range Indicates the area occupied by the building z Coordinate range.
[0034] Furthermore, the initialization of snake optimization algorithm parameters, and the subsequent division of the population into male and female categories, includes the following steps:
[0035] Initialize the snake optimization algorithm parameters, including setting the population size, individual dimension, maximum number of iterations, and upper and lower bounds;
[0036] Initialize the population and perform grouping using the following formula to obtain two groups: male and female.
[0037]
[0038] in, Indicates the population size of male snakes. Indicates population size, This indicates the female population size.
[0039] Furthermore, the food index The expression is as follows:
[0040]
[0041]
[0042] in, Represents a constant. Indicates the current iteration number. Indicates the maximum number of iterations. This represents a random number within the interval [0,1].
[0043] Furthermore, during the exploration phase, the formula for updating the location of male snakes when searching for food is as follows:
[0044]
[0045]
[0046] in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male population in the 1st month. A random individual in the next iteration. Represents a constant. It indicates the male's ability to seek food. and The constants representing the upper and lower bounds, respectively. Represents a random number within the interval [0,1]. express Adaptability, Indicates the male's first The individual in the first Fitness after the next iteration;
[0047] The formula for updating the location of a female snake during the exploration phase when searching for food is as follows:
[0048]
[0049]
[0050] in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female sex population in the 1st month. A random individual in the next iteration. Represents a constant. It indicates the female's ability to seek food. express Adaptability, Indicates the female's first The individual in the first Fitness after the next iteration.
[0051] Furthermore, using the following formula, through the non-monotonic factor... Temperature index Make dynamic adjustments:
[0052]
[0053] in, Indicates the current iteration number. This indicates the maximum number of iterations.
[0054] Furthermore, the expression for the movement trajectory of the snake swarm maintaining its foraging behavior is as follows:
[0055]
[0056] in, Indicates male or female The individual in the first Position after the next iteration This represents the position of the globally optimal individual. Represents a constant. Represents a random number within the interval [0,1]. Indicates male or female The individual in the first The position after the next iteration.
[0057] Furthermore, the expression for updating the position of a male when entering combat mode is as follows:
[0058]
[0059]
[0060] in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male's first The individual in the first Position after the next iteration Represents a constant. Indicates male fighting ability. Represents a random number within the interval [0,1]. This indicates the optimal position for a female individual. This represents the fitness of the optimal female individual. express The fitness of;
[0061] The expression for updating the position of females when entering combat mode is as follows:
[0062]
[0063]
[0064] in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female's first The individual in the first Position after the next iteration Indicates the female's combat strength. Represents a random number within the interval [0,1]. Indicates the optimal position for a male individual. This represents the fitness of the optimal male individual. express The fitness of;
[0065] The expression for the male's entry into the golden sine strategy is as follows:
[0066]
[0067]
[0068]
[0069]
[0070] in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male's first The individual in the first Position after the next iteration Representing an interval random numbers, Representing an interval random numbers, and Both represent constants. Indicates the golden ratio;
[0071] The expression for the female entering the golden sinusoidal strategy is as follows:
[0072]
[0073] in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female's first The individual in the first The position after the next iteration.
[0074] Furthermore, the step of calculating the UAV's three-dimensional trajectory plan based on the optimal individual position using the UAV flight model includes the following steps:
[0075] The 15-dimensional vector of the optimal individual is mapped to x, y, and z components according to the first 5, middle 5, and last 5 dimensions, respectively, to obtain 5 track points. These are then expanded into multiple track points through interpolation and connected to generate the UAV's 3D planned track.
[0076] The beneficial effects of this invention are:
[0077] To address the problems existing in the aforementioned UAV 3D trajectory planning algorithms in practical engineering, this invention constructs a flight model that includes urban terrain and a cost function, utilizing a non-monotonic factor. Dynamically adjust temperature index This study optimizes the algorithm's performance in balancing global search and local exploitation, enhancing its ability to escape local optima and enabling the solution of the shortest path under constraints in UAV 3D trajectory planning. It also introduces a golden sine strategy to replace the mating pattern in the standard snake optimization algorithm, leveraging the advantages of the golden sine strategy. and Set a threshold and utilize Dynamically adjust the search step size and use random variables. and Ensuring population diversity enhances the algorithm's ability to optimize details and reduces path redundancy in the UAV's 3D trajectory. A non-monotonic factor is introduced into the standard snake optimization algorithm. By dynamically adjusting the food index This invention allows the algorithm to flexibly switch between the exploration and development phases, improves the early convergence speed, enhances its search capabilities in complex environments, and strengthens its ability to traverse dense obstacle clusters. The invention offers at least the following advantages:
[0078] 1. The improved snake optimization algorithm can quickly find the shortest path that meets the constraints; the standard snake optimization algorithm uses a monotonically increasing temperature exponent. This prevents the algorithm from dynamically adjusting for both global search and local development. The improved snake optimization algorithm introduces a non-monotonic factor. Dynamically adjust temperature index This allows the algorithm to flexibly switch between global search and local development, enhancing its ability to escape local optima and enabling the solution of the shortest path under constraints in UAV 3D trajectory planning.
[0079] 2. The improved snake optimization algorithm effectively reduces the path redundancy of the UAV's 3D trajectory; the standard snake optimization algorithm uses the male and female snakes as the first... The ratio of individual fitness is the proximity speed, with the positional difference between the two individuals as the step size. However, the ratio of male to female fitness is the first step size. The potential for high similarity or significant differences among individual snakes can lead to insufficient detail processing capabilities in later stages of the algorithm. The improved snake optimization algorithm employs a golden sine strategy, utilizing the golden sine... and Setting thresholds for utilization Dynamically adjust the step size and use random variables and Ensuring population diversity enhances the algorithm's ability to optimize details and reduces path redundancy in UAV 3D flight paths.
[0080] 3. The improved snake optimization algorithm enhances its ability to traverse dense obstacles; the standard snake optimization uses a monotonically increasing food index. This prevents the algorithm from dynamically adjusting between the exploration and development phases. The improved snake optimization algorithm introduces a non-monotonic factor. Dynamically adjust food index This allows the algorithm to switch flexibly between the exploration and development phases, improving the convergence speed in the early stages, enhancing its search capabilities in complex environments, and strengthening its ability to traverse dense obstacles. Attached Figure Description
[0081] Figure 1 This is a flowchart of the method of the present invention.
[0082] Figure 2 This is a topographic map of the city.
[0083] Figure 3A three-dimensional perspective on snake optimization algorithm (SO) and improved snake optimization algorithm (ESO) in urban terrain trajectory planning.
[0084] Figure 4 A top-down perspective of the Snake Optimization Algorithm (SO) and the Improved Snake Optimization Algorithm (ESO) in urban terrain trajectory planning.
[0085] Figure 5 The iterative curves of the Snake Optimization Algorithm (SO) and the Improved Snake Optimization Algorithm (ESO) in urban terrain trajectory planning are shown. Detailed Implementation
[0086] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.
[0087] Example 1
[0088] like Figure 1 As shown, this invention provides a UAV 3D trajectory planning method based on an improved snake optimization algorithm, the implementation method of which is as follows:
[0089] S1. Construct the city terrain, which is implemented as follows:
[0090] Initialize a 20×20 zero matrix;
[0091] Assign random values within the range [10, 25] to the preset elements in the zero matrix, where a value of 0 indicates no building and a non-zero value indicates the building height at the corresponding location;
[0092] Based on the result of random value assignment, differentiated values are assigned to different sub-regions of the zero matrix;
[0093] Based on the differentiated assignment results, the zero matrix is expanded in dimension to generate a 1000×1000 matrix;
[0094] For each non-zero element in the generated 1000×1000 matrix, a building rectangle region is constructed. Starting from the coordinates of the non-zero element, an integer in the range [20,30] is randomly selected to the right as the building width, and an integer in the range [20,30] is randomly selected downwards as the building length. The non-zero element values are assigned to all elements within the building rectangle region to present the spatial distribution of the buildings and complete the construction of the urban terrain.
[0095] In this embodiment, a 20×20 zero matrix is first initialized, and approximately 20% of its elements are assigned random values within the range [10, 25] (an element value of 0 indicates no building, and a non-zero value represents the building height at the corresponding location). Then, different sub-regions of the matrix are assigned differentiated values: 30% of the elements in the lower left sub-region are assigned random values within the range [25, 125], 40% of the elements in the upper left sub-region are assigned random values within the range [75, 125], and 10% of the elements in the lower right sub-region are assigned random values within the range [50, 100]. Next, the matrix is expanded by inserting 49 blank rows below each row and 49 blank columns to the right of each column, generating a 1000×1000 matrix. Finally, a rectangular building region is constructed for each non-zero element in the matrix. Starting from the coordinates of this element, an integer within the range [20, 30] is randomly selected to the right as the building width, and an integer within the range [20, 30] is randomly selected downwards as the building length. This non-zero element value is then assigned to all elements within the rectangular region, thus representing the spatial distribution of the buildings.
[0096] like Figure 2 As shown, after constructing the urban terrain, set the starting point coordinates (290, 830); set the ending point coordinates (700, 240); set the center point coordinates of the four semi-circular no-fly zones to (160, 530), (400, 420), (520, 760), and (720, 440) respectively; each no-fly zone is divided into a warning zone and a collision zone, with the following corresponding parameters: No-fly zone 1: warning zone radius 70, collision zone radius 35; No-fly zone 2: warning zone radius 50, collision zone radius 25; No-fly zone 3: warning zone radius 60, collision zone radius 30; No-fly zone 4: warning zone radius 50, collision zone radius 25.
[0097] S2. Construct a cost function that includes path length, elevation difference, smoothing angle, no-fly zone, and building collision, and form a UAV flight model with urban terrain as the background.
[0098] In this embodiment, coordinate mapping and waypoint expansion are performed on the 15 dimensions of the algorithm population: the first 5 dimensions of the 15 dimensions are set as x Axis coordinate components, the middle 5 dimensions are set as y Axis coordinate components, the last 5 dimensions are set to z The axis coordinate components were used to obtain the coordinates of 5 track points; then, interpolation was used to expand the coordinates of the 5 initial track points to 80 track points, while simultaneously modifying the coordinates of each track point... x axis, y axis, z Boundary value constraints are applied to the axes to ensure that the coordinates of the waypoints are within the flight range of the UAV.
[0099] The expression for the cost function is as follows:
[0100]
[0101] in, , and All represent weight parameters, with values of 0.7, 0.2, and 0.1 respectively; The distance parameter is expressed as follows:
[0102]
[0103] in,( , , ) indicates the drone's first The three-dimensional coordinates of each track point, , , ) indicates the drone's first The three-dimensional coordinates of each track point This indicates the total number of waypoints.
[0104] This is the elevation difference parameter, and its expression is as follows:
[0105]
[0106] in, This represents the sum of the altitude differences between all waypoints. Indicates the first The altitude of each trackpoint.
[0107] This is the smooth angle parameter, and its expression is as follows:
[0108]
[0109] in, 、 and This indicates the coordinates of three consecutive waypoints. Indicates the drone's first The first waypoint, the first The first waypoint and the first The angle between two vectors formed by the coordinates of the track points Indicates the drone's first The first waypoint, the first The first waypoint and the first The angle between two vectors formed by the coordinates of the track points.
[0110] 、 、 The expression representing the coordinates of three consecutive waypoints is as follows:
[0111]
[0112] in,( , , ) indicates the drone's first The three-dimensional coordinates of each track point.
[0113] This is the collision cost in the no-fly zone, expressed as follows:
[0114]
[0115] in, Indicates the drone's first Collision cost of each waypoint:
[0116]
[0117] in, Indicates the drone's first The cost of collisions at each waypoint Indicates the first The distance from each trackpoint to the center of the no-fly zone Representing an interval Random numbers (collision zone). Representing an interval Random numbers (warning area) This represents the collision zone cost, with a size of 200. This indicates the cost of the warning zone, which is 100 in size.
[0118] The cost of a building collision is expressed as follows:
[0119]
[0120] in, Indicates the area occupied by the building x coordinate range Indicates the area occupied by the building y coordinate range Indicates the area occupied by the building z Coordinate range.
[0121] S3. Initialize the snake optimization algorithm parameters. After initializing the population, divide the population into male and female categories. The implementation method is as follows:
[0122] Initialize the snake optimization algorithm parameters, including setting the population size, individual dimension, maximum number of iterations, and upper and lower bounds; specifically, set the population size N=30; individual dimension Dim=15; maximum number of iterations T=200; upper bound ub=1; lower bound lb=0.
[0123] Initialize the population and perform grouping using the following formula to obtain two groups: male and female.
[0124]
[0125] in, Indicates the population size of male snakes. Indicates population size, This indicates the female population size.
[0126] S4. Enter the iteration process, based on the food index. The size of the value determines whether to enter the exploration or development stage, and in this process, non-monotonic factors are used. Food Index Make dynamic adjustments;
[0127] In this embodiment, the non-monotonic factor Food Index Dynamic adjustment is performed, and its expression is as follows:
[0128]
[0129]
[0130] in, This represents a constant with a value of 0.5. Indicates the current iteration number. Indicates the maximum number of iterations. This represents a random number within the interval [0,1].
[0131] S5, when the food index When the amount is less than the first preset threshold, it is determined that there is insufficient food, and the exploration phase is initiated, proceeding to S9;
[0132] In this embodiment, when the food index When food is deemed insufficient, the algorithm enters the exploration phase, during which the snake searches for food in random directions. The formula for updating the position of a male snake during the exploration phase is as follows:
[0133]
[0134] in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male population in the 1st month. A random individual in the next iteration. This represents a constant with a value of 0.05. The formula representing a male's ability to seek food is as follows:
[0135]
[0136] in, express Adaptability, Indicates the male's first The individual in the first Fitness after each iteration. The formula for updating the location of female snakes during the exploration phase when searching for food is as follows:
[0137]
[0138] in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female sex population in the 1st month. A random individual in the next iteration. This represents a constant with a value of 0.05. The formula representing a female's ability to seek food is as follows:
[0139]
[0140] in, express Adaptability, Indicates the female's first The individual in the first Fitness after the next iteration.
[0141] S6, when the food index When the temperature exceeds a first preset threshold, it is determined that there is insufficient food, and the development phase begins. During the development phase, the temperature index is used as a reference. The size of the [something] determines whether to initiate foraging or interactive behavior, and utilizes non-monotonic factors. Temperature index Make dynamic adjustments:
[0142]
[0143] in, Indicates the current iteration number. This indicates the maximum number of iterations.
[0144] S7, When the food index When the temperature index is greater than the first preset threshold, When the value exceeds the second preset threshold, the snake swarm continues its foraging behavior and enters S9;
[0145] In this embodiment, when the food index When the temperature index is greater than the first preset threshold, When the value exceeds the second preset threshold, the snake swarm continues its foraging behavior, and its movement trajectory is described by the following foraging formula:
[0146]
[0147] in, Indicates male or female The individual in the first Position after the next iteration This represents the position of the globally optimal individual. This represents a constant of size 2. Represents a random number within the interval [0,1]. Indicates male or female The individual in the first The position after the next iteration.
[0148] S8, When the food index When the temperature index is greater than the first preset threshold, When the value is less than the second preset threshold, the snake swarm enters interactive behavior and enters S9, which involves combat mode and the golden sine strategy.
[0149] In this embodiment, the snake swarm has a 60% probability of entering combat mode and a 40% probability of entering the Golden Sine Strategy.
[0150] The expression for updating the position of a male when entering combat mode is as follows:
[0151]
[0152]
[0153] in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male's first The individual in the first Position after the next iteration This represents a constant of size 2. Indicates male fighting ability. Represents a random number within the interval [0,1]. This indicates the optimal position for a female individual. This represents the fitness of the optimal female individual. express The degree of adaptability.
[0154] The expression for updating the position of females when entering combat mode is as follows:
[0155]
[0156]
[0157] in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female's first The individual in the first Position after the next iteration This represents a constant of size 2. Indicates the female's combat strength. Represents a random number within the interval [0,1]. Indicates the optimal position for a male individual. This represents the fitness of the optimal male individual. express The degree of adaptability.
[0158] The expression for the male's entry into the golden sine strategy is as follows:
[0159]
[0160]
[0161]
[0162]
[0163] in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male's first The individual in the first Position after the next iteration Representing an interval random numbers, Representing an interval random numbers, and Both represent constants. Indicates the golden ratio;
[0164] The expression for the female entering the golden sinusoidal strategy is as follows:
[0165]
[0166] in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female's first The individual in the first The position after the next iteration.
[0167] S9. Update individual information and determine the iteration count. Has the maximum number of iterations been reached? If yes, then exit the loop and output the optimal individual position; otherwise, return to S4.
[0168] S10. Based on the optimal individual position, the UAV's three-dimensional trajectory planning is calculated using the UAV flight model. The implementation method is as follows:
[0169] The 15-dimensional vector of the optimal individual is mapped to x, y, and z components according to the first 5, middle 5, and last 5 dimensions, respectively, to obtain 5 track points. These are then expanded into multiple track points through interpolation and connected to generate a 3D planned track for the UAV. The flight model includes urban terrain and cost function.
[0170] Example 2
[0171] To verify the effectiveness of the method of the present invention, a comparative experiment was conducted on a simulation platform.
[0172] Experimental environment setup:
[0173] After constructing the city terrain, set the starting point coordinates (290, 830); set the ending point coordinates (700, 240).
[0174] Four semi-circular no-fly zones are set with center point coordinates of (160, 530), (400, 420), (520, 760), and (720, 440), respectively. Each no-fly zone is divided into a warning zone and a collision zone, with the following parameters: No-fly zone 1: warning zone radius 70, collision zone radius 35; No-fly zone 2: warning zone radius 50, collision zone radius 25; No-fly zone 3: warning zone radius 60, collision zone radius 30; No-fly zone 4: warning zone radius 50, collision zone radius 25.
[0175] Analysis of experimental results:
[0176] from Figure 3 As can be seen, the improved snake optimization algorithm (ESO) chooses to avoid buildings and fly at low altitudes, while the snake optimization algorithm (SO) chooses to bypass building clusters and fly at high altitudes, which is more costly. This shows that the improved snake optimization algorithm is more adaptable to complex urban terrain environments.
[0177] from Figure 4As can be seen, the improved snake optimization algorithm perfectly avoids buildings and finds the shortest path during low-altitude flight, thus demonstrating that the improved snake optimization algorithm can find the shortest path under constraints.
[0178] from Figure 5 The results show that the improved snake optimization algorithm converges faster in the early stages of its iteration curve compared to the snake optimization algorithm. In terms of convergence accuracy, the fitness of the improved snake optimization algorithm converges to 509, while the fitness of the snake optimization algorithm converges to 674. The improved snake optimization algorithm improves the fitness performance of UAV 3D trajectory planning by approximately 24.48% compared to the snake optimization algorithm.
[0179] Conclusion: The improved snake optimization algorithm (ESO) proposed in this invention can meet the needs of UAV 3D trajectory planning. It has the advantages of fast convergence in the early stage, strong ability to escape local optima and high convergence accuracy. It can effectively adapt to complex urban terrain environment, solve the shortcomings of existing snake optimization algorithms in trajectory planning, and provide a reliable trajectory planning solution for UAV autonomous operation.
Claims
1. A UAV 3D trajectory planning method based on an improved snake optimization algorithm, characterized in that, Includes the following steps: S1. Construct the city terrain; S2. Construct a cost function that includes path length, elevation difference, smoothing angle, no-fly zone, and building collision, and form a UAV flight model with urban terrain as the background. S3. Initialize the snake optimization algorithm parameters and divide the population into male and female categories after initialization. S4. Enter the iteration process, based on the food index. The size of the value determines whether to enter the exploration or development stage, and in this process, non-monotonic factors are used. Food Index Make dynamic adjustments; S5, when the food index When the value is less than the first preset threshold, the exploration phase begins, and the process proceeds to S9. S6, when the food index When the temperature exceeds a first preset threshold, the development phase begins. During the development phase, the temperature index is used to determine the appropriate parameters. The size of the [something] determines whether to initiate foraging or interactive behavior, and utilizes non-monotonic factors. Temperature index Make dynamic adjustments; S7, When the food index The temperature index is greater than the first preset threshold and When the value exceeds the second preset threshold, the snake swarm continues its foraging behavior and enters S9; S8, When the food index The temperature index is greater than the first preset threshold and When the value is less than the second preset threshold, the snake swarm enters interactive behavior and enters S9, which involves combat mode and the golden sine strategy. S9. Update individual information and determine the iteration count. Has the maximum number of iterations been reached? If yes, then exit the loop and output the optimal individual position; otherwise, return to S4. S10. Based on the optimal individual position, the UAV flight model is used to calculate the UAV's three-dimensional trajectory plan.
2. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, The construction of urban terrain includes the following steps: Initialize a 20×20 zero matrix; Assign random values within the range [10, 25] to the preset elements in the zero matrix, where an element value of 0 indicates no building and a non-zero value indicates the building height at the corresponding location; Based on the result of random value assignment, differentiated values are assigned to different sub-regions of the zero matrix; Based on the differentiated assignment results, the zero matrix is expanded in dimension to generate a 1000×1000 matrix; For each non-zero element in the generated 1000×1000 matrix, a building rectangle region is constructed. Starting from the coordinates of the non-zero element, an integer in the range [20,30] is randomly selected to the right as the building width, and an integer in the range [20,30] is randomly selected downwards as the building length. The non-zero element values are assigned to all elements within the building rectangle region to present the spatial distribution of the buildings and complete the construction of the urban terrain.
3. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, The expression for the cost function is as follows: in, Represents the cost function, , and All represent weight parameters. Represents the distance parameter. Indicates the elevation difference parameter. Indicates the smooth angle parameter. This indicates the cost of collisions in no-fly zones. Indicates the cost of building collisions, ( , , ) indicates the drone's first The three-dimensional coordinates of each track point, , , ) indicates the drone's first The three-dimensional coordinates of each track point This indicates the total number of waypoints. This represents the sum of the altitude differences between all waypoints. Indicates the drone's first The altitude of each waypoint Indicates the drone's first The first waypoint, the first The first waypoint and the first The angle between two vectors formed by the coordinates of the track points Indicates the drone's first The first waypoint, the first The first waypoint and the first The angle between two vectors formed by the coordinates of the track points 、 and This indicates the coordinates of three consecutive waypoints. ( , , ) indicates the drone's first The three-dimensional coordinates of each track point Indicates the drone's first The cost of collisions at each waypoint Indicates the drone's first The distance from each trackpoint to the center of the no-fly zone Representing an interval random numbers, Representing an interval random numbers, Indicates the cost of the collision zone. Indicates the cost of the warning zone. Indicates the drone's first The cost of collisions at each waypoint Indicates the area occupied by the building x coordinate range Indicates the area occupied by the building y coordinate range Indicates the area occupied by the building z Coordinate range.
4. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, The initialization of snake optimization algorithm parameters, and the subsequent division of the population into male and female categories, includes the following steps: Initialize the snake optimization algorithm parameters, including setting the population size, individual dimension, maximum number of iterations, and upper and lower bounds; Initialize the population and perform grouping using the following formula to obtain two groups: male and female. in, Indicates the population size of male snakes. Indicates population size, This indicates the female population size.
5. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, The food index The expression is as follows: in, Represents a constant. Indicates the current iteration number. Indicates the maximum number of iterations. This represents a random number within the interval [0,1].
6. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, During the exploration phase, the formula for updating the location of male snakes when searching for food is as follows: in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male population in the 1st month. A random individual in the next iteration. Represents a constant. It indicates the male's ability to seek food. and The constants representing the upper and lower bounds, respectively. Represents a random number within the interval [0,1]. express Adaptability, Indicates the male's first The individual in the first Fitness after the next iteration; The formula for updating the location of a female snake during the exploration phase when searching for food is as follows: in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female sex population in the 1st month. A random individual in the next iteration. Represents a constant. It indicates the female's ability to seek food. express Adaptability, Indicates the female's first The individual in the first Fitness after the next iteration.
7. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, Using the following formula, through the non-monotonic factor Temperature index Make dynamic adjustments: in, Indicates the current iteration number. This indicates the maximum number of iterations.
8. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, The expression for the movement trajectory of the snake swarm to maintain its foraging behavior is as follows: in, Indicates male or female The individual in the first Position after the next iteration This represents the position of the globally optimal individual. Represents a constant. Represents a random number within the interval [0,1]. Indicates male or female The individual in the first The position after the next iteration.
9. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, The expression for updating the position of a male when entering combat mode is as follows: in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male's first The individual in the first Position after the next iteration Represents a constant. Indicates male fighting ability. Represents a random number within the interval [0,1]. This indicates the optimal position for a female individual. This represents the fitness of the optimal female individual. express The fitness of; The expression for updating the position of females upon entering combat mode is as follows: in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female's first The individual in the first Position after the next iteration Indicates the female's combat strength. Represents a random number within the interval [0,1]. Indicates the optimal position for a male individual. This represents the fitness of the optimal male individual. express The fitness of; The expression for the male's entry into the golden sine strategy is as follows: in, Indicates the male's first The individual in the first Position after the next iteration Indicates the male's first The individual in the first Position after the next iteration Representing an interval random numbers, Representing an interval random numbers, and Both represent constants. Indicates the golden ratio; The expression for the female entering the golden sinusoidal strategy is as follows: in, Indicates the female's first The individual in the first Position after the next iteration Indicates the female's first The individual in the first The position after the next iteration.
10. The UAV 3D trajectory planning method based on the improved snake optimization algorithm according to claim 1, characterized in that, The process of calculating the UAV's three-dimensional trajectory plan based on the optimal individual position using the UAV flight model includes the following steps: The 15-dimensional vector of the optimal individual is mapped to x, y, and z components according to the first 5, middle 5, and last 5 dimensions, respectively, to obtain 5 track points. These are then expanded into multiple track points through interpolation and connected to generate the UAV's 3D planned track.