A method, device, equipment and medium for three-dimensional path planning of a UAV

Abstract

The application discloses a UAV three-dimensional path planning method, device, equipment and medium, and relates to the field of UAV autonomous navigation and intelligent control. The method comprises the following steps: constructing an environment model of a UAV task area; modeling a path to be planned into a control point sequence based on the environment model, and initializing a particle population with intermediate control points in the sequence as position vectors of particles; constructing a multi-objective weighted fitness function based on path length, obstacle avoidance penalty, trajectory smoothness and energy consumption; performing multiple iteration optimizations on the initial population by using a particle swarm optimization algorithm with a multi-strategy fusion mechanism based on the multi-objective weighted fitness function to obtain an optimal control point sequence; the multi-strategy fusion mechanism comprises the following steps: introducing Levy disturbance and elite guide strategy in velocity updating, and implementing a constraint repair mechanism on infeasible solutions; and determining a feasible path of the UAV according to the optimal control point sequence. The application can find a safe, feasible and energy-optimized flight path in a complex three-dimensional obstacle space.

Description

Technical Field

[0001] This application relates to the field of autonomous navigation and intelligent control of unmanned aerial vehicles (UAVs), and in particular to a method, apparatus, device and medium for three-dimensional path planning of UAVs. Background Technology

[0002] With the widespread application of drone technology, its autonomous path planning capability in complex three-dimensional environments has become a key bottleneck. Currently, related path planning algorithms have significant shortcomings.

[0003] Graph search-based algorithms require gridded discretization of the continuous 3D space, leading to an exponential increase in computational complexity with increasing spatial dimension. Furthermore, they are prone to getting stuck in local optima in areas with dense obstacles or complex structures, making it difficult to find a globally feasible path. While standard particle swarm optimization (PSO) possesses the advantage of swarm intelligence, in high-dimensional continuous spaces, the particle swarm is prone to premature convergence, meaning it prematurely clusters near local optima, losing its global exploration capability and failing to effectively handle large-scale 3D search spaces. A common problem with related path planning algorithms is that they focus solely on minimizing path length, ignoring the physical constraints and energy consumption characteristics of the UAV. The planned paths often include non-smooth trajectories such as sharp turns and dramatic ascents and descents, increasing flight energy consumption and potentially exceeding the capabilities of the UAV's actuators, thus affecting flight safety.

[0004] Therefore, finding a safe, feasible, and energy-efficient flight path in a complex three-dimensional obstacle space has become an urgent problem to be solved. Summary of the Invention

[0005] The purpose of this application is to provide a method, apparatus, device and medium for three-dimensional path planning of unmanned aerial vehicles (UAVs), which can find a safe, feasible and energy-efficient flight path in a complex three-dimensional obstacle space.

[0006] To achieve the above objectives, this application provides the following solution.

[0007] In one aspect, this application provides a three-dimensional path planning method for unmanned aerial vehicles (UAVs), comprising the following steps.

[0008] Construct an environmental model of the UAV mission area; the environmental model includes: the UAV starting point, the UAV ending point, and a three-dimensional obstacle model with safety margin.

[0009] Based on the environmental model, with the starting point of the UAV as the first point and the ending point of the UAV as the last point, the path to be planned is modeled as a sequence of control points, and the intermediate control points in the sequence of control points are used as the position vectors of the particles to initialize the particle population, thus obtaining the initial population.

[0010] A multi-objective weighted fitness function is constructed based on path length, obstacle avoidance penalty, trajectory smoothness, and energy consumption.

[0011] Based on the multi-objective weighted fitness function, a particle swarm optimization algorithm with a multi-strategy fusion mechanism is used to iteratively optimize the initial population multiple times. When the iteration termination condition is reached, the optimal control point sequence is obtained. The multi-strategy fusion mechanism includes: introducing Levy perturbation in the velocity update, adopting an elite-guided strategy in the velocity update, and implementing a constraint repair mechanism for infeasible solutions obtained during the optimization process.

[0012] A smooth trajectory is generated based on the optimal control point sequence, and a feasible path for the UAV is determined based on the smooth trajectory.

[0013] Secondly, this application provides a three-dimensional path planning device for unmanned aerial vehicles (UAVs), comprising the following modules.

[0014] The environment model construction module is used to construct an environment model of the UAV mission area; the environment model includes: the UAV starting point, the UAV ending point, and a three-dimensional obstacle model with safety margin.

[0015] The population initialization module is used to model the path to be planned as a sequence of control points based on the environment model, with the starting point of the UAV as the first point and the ending point of the UAV as the last point, and to initialize the particle population by using the intermediate control points in the control point sequence as the position vectors of the particles, so as to obtain the initial population.

[0016] The fitness function construction module is used to construct multi-objective weighted fitness functions based on path length, obstacle avoidance penalty, trajectory smoothness, and energy consumption.

[0017] The control point optimization module is used to perform multiple iterations of particle swarm optimization algorithm with a multi-strategy fusion mechanism on the initial population based on the multi-objective weighted fitness function. When the iteration termination condition is reached, the optimal control point sequence is obtained. The multi-strategy fusion mechanism includes: introducing Levy perturbation in velocity update, adopting an elite-guided strategy in velocity update, and implementing a constraint repair mechanism for infeasible solutions obtained during the optimization process.

[0018] A feasible path determination module is used to generate a smooth trajectory based on the optimal control point sequence, and determine the feasible path of the UAV based on the smooth trajectory.

[0019] Thirdly, this application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the processor executes the computer program to implement the UAV three-dimensional path planning method described in any one of the above.

[0020] Fourthly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the UAV three-dimensional path planning method described above.

[0021] According to the specific embodiments provided in this application, this application has the following technical effects: This application provides a method, apparatus, device, and medium for UAV three-dimensional path planning. A multi-objective weighted fitness function is constructed based on path length, obstacle avoidance penalty, trajectory smoothness, and energy consumption, taking into account the physical motion constraints and energy consumption characteristics of the UAV itself. Based on the multi-objective weighted fitness function, a particle swarm optimization algorithm with a multi-strategy fusion mechanism iteratively optimizes the initial population multiple times to obtain the optimal control point sequence. The multi-strategy fusion mechanism includes: introducing Levy perturbation and elite guidance strategies in velocity updates, and implementing a constraint repair mechanism for infeasible solutions, avoiding the "premature convergence" phenomenon of particle swarm optimization, and effectively coping with a large-scale three-dimensional search space. This application can find a safe, feasible, and energy-optimal flight path in a complex three-dimensional obstacle space. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in the embodiments of this application or related technologies, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0023] Figure 1 This is a flowchart illustrating a UAV three-dimensional path planning method provided in an embodiment of this application.

[0024] Figure 2 This is a schematic diagram illustrating a specific implementation process of the UAV three-dimensional path planning method provided in this application embodiment.

[0025] Figure 3 This is a schematic diagram of a three-dimensional flight mission environment model provided in an embodiment of this application.

[0026] Figure 4 A stereoscopic image of a three-dimensional flight trajectory obtained using multiple algorithms, provided as an embodiment of this application.

[0027] Figure 5 A top view of a three-dimensional flight trajectory obtained using multiple algorithms, provided as an embodiment of this application.

[0028] Figure 6 This is a schematic diagram of the functional modules of a UAV three-dimensional path planning device provided in an embodiment of this application.

[0029] Figure 7This is a schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0030] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0031] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0032] Currently, there are UAV path planning methods based on improved hybrid particle swarm optimization (PSO) algorithms. These methods introduce crossover and mutation operations from genetic algorithms into PSO to enhance diversity. However, their improvement strategies are relatively simplistic and do not fully consider the motion constraints and energy consumption models unique to 3D paths. Furthermore, their premature convergence problem has not been fundamentally solved, and their global optimization capability in complex obstacle environments remains limited.

[0033] The UAV 3D path planning method, apparatus, equipment and medium provided in this application are based on the Levy flight improved particle swarm optimization algorithm to realize UAV 3D path planning. This application takes into account global optimization capability, multi-objective optimization, strict motion constraint satisfaction and real-time performance, and finds a safe, feasible and energy-efficient flight path in a complex 3D obstacle space.

[0034] In one exemplary embodiment, such as Figure 1 As shown, a three-dimensional path planning method for unmanned aerial vehicles (UAVs) is provided, including the following steps.

[0035] Step 101: Construct an environmental model of the UAV mission area; the environmental model includes: the UAV starting point, the UAV ending point, and a three-dimensional obstacle model with safety margin.

[0036] Step 102: Based on the environment model, with the starting point of the UAV as the first point and the ending point of the UAV as the last point, the path to be planned is modeled as a sequence of control points, and the intermediate control points in the sequence of control points are used as the position vectors of the particles to initialize the particle population, thus obtaining the initial population.

[0037] Step 103: Construct a multi-objective weighted fitness function based on path length, obstacle avoidance penalty, trajectory smoothness, and energy consumption.

[0038] Step 104: Based on the multi-objective weighted fitness function, the initial population is iteratively optimized multiple times using a particle swarm optimization algorithm with a multi-strategy fusion mechanism. When the iteration termination condition is reached, the optimal control point sequence is obtained.

[0039] The multi-strategy fusion mechanism includes: introducing Levy perturbation in velocity updates, adopting an elite-guided strategy in velocity updates, and implementing a constraint repair mechanism for infeasible solutions obtained during the optimization process.

[0040] Step 105: Generate a smooth trajectory based on the optimal control point sequence, and determine a feasible path for the UAV based on the smooth trajectory.

[0041] In another exemplary embodiment of this application, step 101 specifically includes: determining a bounded three-dimensional Euclidean space in the world coordinate system; modeling all obstacles within the UAV mission area in the bounded three-dimensional Euclidean space to obtain an obstacle set; performing Minkowski operation and dilation operation on each obstacle in the obstacle set according to the minimum safe distance to generate a three-dimensional obstacle model with a safety margin; the minimum safe distance is determined based on the maximum outer radius of the UAV body, the UAV positioning system error, and the flight control response margin.

[0042] Step 101 constructs a bounded flight space environment model that includes a starting point, an ending point, and a three-dimensional obstacle model with safety margins. In practical applications, the more specific implementation process of step 101 is as follows.

[0043] This step aims to provide a precise and secure physical constraint basis for path planning. First, a bounded three-dimensional Euclidean space is defined in the world coordinate system. ,in and Represents horizontal plane coordinates. Indicates altitude, and These are the minimum and maximum permissible flight altitudes set based on low-altitude airspace management regulations, terrain data, and the flight performance of the UAVs. and These represent the minimum and maximum x-coordinate values ​​of the task region in the X direction, i.e., the range of the region boundary on the X-axis. Similarly, and This refers to the minimum and maximum x-coordinate values ​​of the task area in the Y direction, i.e., the range of the area boundary on the Y-axis. Secondly, all static obstacles within the task area (including but not limited to buildings, communication towers, high-voltage lines, mountains, or trees) are digitally modeled: regular obstacles are represented using axis-aligned bounding boxes or cylinders, while complex terrain or irregular structures are constructed using point cloud data to create convex hull or polygonal mesh models, forming an obstacle set. Furthermore, based on the maximum outer radius of the UAV body... Typical errors of GNSS / visual fusion positioning systems and flight control response margin Calculate the minimum safe distance For each obstacle, Minkowski and dilation operations are performed to generate a 3D obstacle model with a safety margin (i.e., a set of infeasible regions). Finally, clearly specify the starting point of the task. With the target point Both were verified to be located through X-ray inspection or point containment testing. Within this framework, ensure that a feasible solution exists for the path planning problem. This represents the area remaining after removing infeasible regions from the entire flight space, i.e., the area where the drone can fly safely (feasible domain). This environmental model can be derived from digital twin city platforms, high-precision maps, or real-time reconstruction using airborne sensors, providing reliable spatial constraints for subsequent optimization.

[0044] In another exemplary embodiment of this application, step 102 specifically includes: assigning the intermediate control points in the control point sequence to the control vertices of multiple cubic Bézier curves, with the first derivatives at the connection points of adjacent cubic Bézier curves being continuous; randomly generating the coordinates of the intermediate control points in the environment model based on the assigned intermediate control points; determining whether the path formed by the intermediate control points at these coordinates interferes with the three-dimensional obstacle model according to a collision detection algorithm; if there is no interference and the distance between the intermediate control points is within a set distance range, then determining the coordinates of the intermediate control points as a particle swarm that satisfies the obstacle avoidance conditions to obtain an initial population; otherwise, regenerating the coordinates of the intermediate control points.

[0045] Step 102 employs a path encoding method with fixed start and end points and optimizable intermediate control points (optional polyline or Bézier control points), and initializes a particle swarm that meets obstacle avoidance conditions to obtain the initial population. In practical applications, step 102 is implemented in more detail as follows.

[0046] This step completes the parameterized representation of the path and the initial population construction. Specifically, the path to be planned is modeled as a sequence of paths... An ordered sequence of three-dimensional control points The first point Fixed as , the end point Fixed as ,middle Points As optimizable variables, they together constitute the particle's position vector. The control point sequence directly corresponds to the control vertices of the piecewise cubic Bézier curve: every four consecutive points... Define a Bézier subcurve such that the first derivative of adjacent subcurves is continuous at the junction. (continuous), thus naturally possessing trajectory smoothness. Initialize the particle swarm size to N, for each particle: in Randomly generate intermediate control point coordinates within the range; use a fast collision detection algorithm to determine whether the polyline path formed by the control points is collided with the line. Interference occurs; if a collision occurs, the particle is resampled until the basic obstacle avoidance conditions are met; simultaneously, a coarse kinematic pre-screening is applied: limiting the horizontal distance between adjacent control points to no more than [a certain value]. The vertical height difference does not exceed This is to avoid generating extremely steep or erratic paths. Through the above strategies, we ensure that the initial particle swarm not only lies within the feasible region but also possesses a good geometric distribution and reasonable initial motion, laying the foundation for subsequent efficient optimization.

[0047] In another exemplary embodiment of this application, step 103 constructs a comprehensive evaluation index for quantifying path quality, namely, the expression of the multi-objective weighted fitness function is as follows.

[0048] .

[0049] .

[0050] .

[0051] .

[0052] .

[0053] in, This represents a multi-objective weighted fitness function; This indicates the path length, reflecting the efficiency of task execution. This represents the obstacle avoidance penalty, which applies if any line segment of the path intersects with... If they intersect, take the maximum positive number. Otherwise, the value is 0, forcing the algorithm to avoid dangerous areas; This represents the trajectory smoothness term; Indicates energy consumption; Represents the path encoding vector (decision variable vector); This indicates the weight corresponding to the path length item; This indicates the weight corresponding to the obstacle avoidance penalty item; This indicates the weight corresponding to the trajectory smoothness term; This indicates the weight corresponding to the energy consumption item; Indicates the sequence number of the control point; Indicates the number of control points; Indicates the first The coordinates of the control points; Indicates the first The coordinates of the control points; This indicates the number of intermediate control points in the control point sequence. Indicates the first Local heading angles at each control point; Indicates the first Local heading angles at each control point; Indicates the first Local heading angles at each control point; Indicates the first Z-axis height coordinates of each control point; Indicates the first Z-axis height coordinates of each control point; Indicates the first Z-axis height coordinates of each control point; Indicates the first The vertical velocity of the path segment, i.e., the instantaneous velocity in the Z-axis direction; Indicates the set cruise speed; For the first Turning radius of the segment path; Indicates the first coefficient; Indicates the second coefficient; Indicates the third coefficient; Indicates the first The horizontal coordinates of each control point; Indicates the first The horizontal ordinate of each control point; Indicates the first The horizontal coordinates of each control point; Indicates the first The horizontal ordinate of each control point.

[0054] The second-order difference form effectively suppresses path jitter and high-frequency oscillations. The vertical velocity expression is: ,coefficient , and Based on the motor efficiency curve and aerodynamic model calibration, the energy consumption is accurately reflected. The weighting coefficients satisfy... Furthermore, it can be dynamically adjusted according to the task type. This function is the first to explicitly incorporate smoothness into the optimization objective, avoiding significant corrections in the post-processing stage and significantly improving the overall solution quality.

[0055] In another exemplary embodiment of this application, step 104 performs Levy-enhanced particle swarm optimization, introduces Levy perturbations in the velocity update to improve global search capability, and implements a constraint repair mechanism for infeasible solutions.

[0056] This step implements the core optimization process. Based on the standard PSO velocity update formula, three enhancement strategies are integrated. First, selective Levy flight perturbation: only for the bottom 30% of "inferior" particles in the current generation's fitness ranking, a Levy random vector is superimposed on their velocity update, as shown in the following velocity update formula.

[0057] .

[0058] in, Indicates the first Individual particles Update speed during the next iteration; Indicates the first Individual particles The current velocity at the next iteration; express The inertia weight at each moment is used to balance the algorithm's global search capability and local search capability; This represents the individual learning factor, used to adjust the step size of a particle's flight towards its historical best position; This represents the first random number in the interval [0,1]. This represents the global learning factor, used to adjust the step size of a particle's flight towards the group's historical best position; This represents the second random number within the interval [0,1]. This indicates an adaptive step size (decreasing with iteration). This mechanism preserves the stable convergence of high-quality particles while giving low-quality particles a strong ability to escape. Indicates the first The individual historical best position found by each particle during the search process; Indicates the first Individual particles The current position at the next iteration; This represents the globally optimal position found by the entire particle swarm during the search process; The characteristic exponent parameter represents the Lévy distribution and is used to control the step size distribution characteristics of Lévy flight. ; This represents a Levy random vector, generated using the Mantegna algorithm.

[0059] Secondly, an elite-guided strategy is adopted, except for global optimization. In addition, maintain a capacity of The Elite Archive stores historical non-dominated solutions and introduces the best individual from the archive as a social learning target with a certain probability during velocity updates, enhancing population diversity. Finally, a constraint repair mechanism is implemented. For particles that cross boundaries or collide, instead of directly punishing or discarding them, repair operations are performed, such as fine-tuning the nearest control point along the opposite direction of the obstacle gradient, or inheriting partial gene fragments from neighboring feasible particles to generate new feasible individuals.

[0060] The iteration process continues until the iteration termination condition is met (such as the number of iterations reaching the set maximum number of iterations). Continuous setting number of times If the multi-objective weighted fitness function value remains unchanged (no improvement) or the obtained feasible solution satisfies the set threshold, the final output is the optimal control point sequence. This multi-strategy fusion mechanism significantly improves the algorithm's global search capability and robustness in complex 3D non-convex spaces.

[0061] In another exemplary embodiment of this application, step 105 specifically includes: generating a smooth trajectory based on the optimal control point sequence using B-spline or Bézier interpolation methods; verifying the smooth trajectory; if the smooth trajectory satisfies the UAV kinematic constraints, then determining that the smooth trajectory passes the verification and identifying the smooth trajectory as a feasible path for the UAV; the UAV kinematic constraints include: turning radius constraints, vertical speed constraints, and control command constraints.

[0062] Step 105 generates a smooth trajectory from the optimized control point sequence using B-spline or Bézier interpolation methods, verifies whether it satisfies the UAV kinematic constraints, and outputs the final feasible path. In practical applications, the more specific implementation process of step 105 is as follows.

[0063] This step completes the transformation and verification from discrete optimization results to a continuous executable trajectory. The optimal control point sequence output from step 104 is then used. Input a piecewise cubic Bézier curve generator: every four consecutive points Define a Bézier subcurve. The expression is as follows.

[0064] .

[0065] This represents the local parameters of the Bézier subcurve. It is a locally normalized variable, meaning it is neither time nor spatial distance, but rather a mathematical interpolation parameter. For example, when... At that time, the curve was located at the starting point. ;when At that time, the curve was located at the starting point. ,when As the point gradually changes from 0 to 1, This will depict the entire curved trajectory from the starting point to the end point.

[0066] Adjacent sub-curves automatically satisfy the condition at the connection point. The system generates a continuous, globally smooth, and angle-free parametric trajectory. Subsequently, a rigorous kinematic feasibility test is performed on this trajectory: first, the curvature is calculated using the following formula.

[0067] .

[0068] in, Indicates the trajectory in the path parameters The curvature at a point. The greater the curvature, the more curved the path. This represents the normalized path parameter, and its typical value range is... ; The function representing the generated smooth trajectory curve is also known as the position vector; express right The first derivative, also known as the tangent vector, reflects the direction of the tangent to the curve. express right The second derivative, also known as the curvature vector, reflects the rate of change of the curve's bending. This is used to verify the turning radius, as shown in the following formula.

[0069] .

[0070] in, Indicates the trajectory in The turning radius at that point; This indicates the drone's cruising speed, i.e., its current flight speed; This represents the acceleration due to gravity, approximately 9.8. ; This represents the drone's maximum tilt angle, which is the reciprocal of the curvature. Then, the vertical velocity is calculated. ,verify It can also perform six-degree-of-freedom dynamics simulations to verify that the control commands are within the capabilities of the actuator.

[0071] If all constraints are satisfied, the trajectory is sampled uniformly over time to generate a standard waypoint file containing position, velocity, and attitude commands, such as a MAVLINK file. If there is a local violation, additional control points are inserted in the violation section, and a lightweight local LevyPSO re-optimization is initiated with the original optimal solution as the initial value until full compliance is achieved. The final output trajectory can be directly uploaded to mainstream flight control systems such as PX4 and ArduPilot for execution, achieving a seamless transition from intelligent planning to physical flight.

[0072] A specific implementation process of the UAV three-dimensional path planning method in this embodiment is as follows: Figure 2 As shown; the three-dimensional flight mission environment model is as follows Figure 3 As shown; a stereoscopic image of the three-dimensional flight trajectory obtained using multiple algorithms is shown below. Figure 4 As shown, the yellow trajectory is the flight trajectory obtained by the UAV 3D path planning method in this embodiment; the top view of the 3D flight trajectory obtained by using multiple algorithms is shown below. Figure 5 As shown. Figure 4 and Figure 5 In the diagram, the red trajectory represents the flight trajectory obtained by the UAV 3D path planning method in this embodiment, the black trajectory represents the flight trajectory obtained by the standard particle swarm optimization algorithm, and the blue trajectory represents A. The flight trajectory obtained by the algorithm, the yellow trajectory is RRT. The flight trajectory obtained by the algorithm; the green trajectory is the flight trajectory obtained by the genetic algorithm.

[0073] The UAV 3D path planning method in this embodiment is based on the Levy improved particle swarm optimization algorithm, which solves the problems of weak global search capability, easy getting trapped in local optima, and insufficient consideration of UAV energy consumption and motion constraints in the prior art. Thus, it can efficiently plan a flight path that simultaneously satisfies safety, smoothness and energy consumption in complex 3D environments.

[0074] Because this embodiment can generate safe, smooth, and energy-efficient flight paths for drones in complex three-dimensional obstacle environments, it is suitable for application scenarios with high requirements for path quality and real-time performance, such as aerial photography, logistics delivery, power line inspection, and emergency rescue.

[0075] The following section provides a detailed explanation of the implementation process of the UAV 3D path planning method described in this application, using a typical urban low-altitude logistics delivery application scenario. In this embodiment, the UAV needs to fly from the urban delivery station to the designated receiving point, passing through areas with high-rise buildings and communication facilities along the way.

[0076] Step 1: Construct a bounded flight space environment that includes a start point, an end point, and a three-dimensional obstacle model with safety margins, as detailed below.

[0077] The flight mission area is defined as a cubic space of 100m × 100m × 50m, that is: The lowest flight altitude (Avoiding pedestrians and vehicles on the ground), maximum flight altitude (Complies with the Civil Aviation Administration's "Rules for the Safety Management of Civil Unmanned Aerial Vehicle Operations" regarding urban low-altitude restrictions). Six static obstacles are placed in this area: four high-rise office buildings, modeled as cylinders with a radius of... , , , The location was obtained from a digital city map; the two communication towers are modeled as cylinders with a radius of... , The six obstacles have varying radii and heights. The maximum outer diameter of the multi-rotor UAV used is... Typical error of positioning system and control margin Calculate the minimum safe flight distance for the drone: To facilitate subsequent calculations, rounding is used. Minkowski expansion is performed on all obstacles to generate safe but infeasible regions. The task starts on the roof of the delivery station. The final destination is the balcony of the delivery point. X-ray testing confirmed that both were located in .

[0078] Step 2: Use a path coding method with fixed start and end points and optimizable intermediate control points (optional polyline or Bézier control points) and initialize a particle swarm that meets the basic obstacle avoidance conditions, as follows.

[0079] The path is parameterized using piecewise cubic Bezier curves. The path is defined as consisting of 7 control points (n=6), i.e.: ,in , Fixed, 5 points in the middle to To optimize the variables, each particle has a dimension of 3 x 5 = 15. The particle swarm size is initialized to N = 50. For each particle, in... Uniformly randomized within to Coordinates; construct a polyline path A fast ray collision detection algorithm based on AABB hierarchical bounding volumes is used to determine whether the polyline is collided with... If the particles intersect, the particle is regenerated until all 50 collision-free initial individuals are obtained. Simultaneously, the horizontal distance between adjacent control points is constrained to no more than 30m, and the vertical height difference to no more than 8m, to avoid generating extreme paths. After initialization, each particle corresponds to a potentially feasible path.

[0080] Step 3: Design a multi-objective weighted fitness function that integrates path length, obstacle avoidance penalty, trajectory smoothness, and energy consumption assessment, as detailed below.

[0081] The fitness function weights are set as follows: path length Obstacle avoidance safety , smoothness Energy consumption ,satisfy Penalty items The energy consumption coefficient is calibrated based on the measured motor efficiency. , , Cruise speed set to The path of a certain particle was calculated as follows: , , , Therefore, the fitness function can be expressed as: This evaluation mechanism can effectively distinguish path quality and guide the algorithm to evolve towards overall optimization.

[0082] Step four: Perform Levy-enhanced particle swarm optimization, introduce Levy perturbations in the velocity update to improve global search capability, and implement constraint repair mechanisms for infeasible solutions, as detailed below.

[0083] The algorithm parameters are set as follows: maximum number of iterations. Inertial weight The learning factor decreased linearly from 0.9 to 0.4. Levy parameters Step length Elite file capacity The threshold for judging inferior particles is the bottom 30% in terms of fitness ranking.

[0084] The optimization process is as follows: From generation 1 to 40, particles explore extensively, and some are repaired due to collisions; from generation 50 to 120, the population converges towards a collision-free region, and Levy perturbations help escape local minima; after generation 150, the fitness stabilizes, and the global optimum is reached. The final output is the optimal control point sequence. .

[0085] Step 5: Generate a smooth trajectory from the optimized control point sequence using B-spline or Bézier interpolation, and verify whether it satisfies the UAV kinematic constraints. Output the final feasible path as follows.

[0086] Will The 7 control points are divided into two cubic Bezier curves. and Generate global Continuous trajectory Perform motion calibration on the trajectory and calculate the minimum turning radius. Actual trajectory curvature analysis shows The vertical velocity calculation result is All constraints were satisfied, eliminating the need for local re-optimization. The trajectory was finally discretized into 101 waypoints at a 10Hz sampling rate, generating a standard MAVLink waypoint file, which was then uploaded to the PX4 flight control system for execution.

[0087] Based on the same inventive concept, this application also provides a UAV three-dimensional path planning device for implementing the UAV three-dimensional path planning method described above. The solution provided by this device is similar to the solution described in the above method; therefore, the specific limitations in one or more UAV three-dimensional path planning device embodiments provided below can be found in the limitations of the UAV three-dimensional path planning method described above, and will not be repeated here.

[0088] In one exemplary embodiment, such as Figure 6 As shown, a UAV three-dimensional path planning device is provided, comprising the following modules.

[0089] The environment model construction module 601 is used to construct an environment model of the UAV mission area; the environment model includes: the UAV starting point, the UAV ending point, and a three-dimensional obstacle model with safety margin.

[0090] The population initialization module 602 is used to model the path to be planned as a sequence of control points based on the environment model, with the starting point of the UAV as the first point and the ending point of the UAV as the last point, and initialize the particle population by using the intermediate control points in the control point sequence as the position vectors of the particles, so as to obtain the initial population.

[0091] The fitness function construction module 603 is used to construct a multi-objective weighted fitness function based on path length, obstacle avoidance penalty, trajectory smoothness and energy consumption.

[0092] The control point optimization module 604 is used to perform multiple iterations of particle swarm optimization algorithm with a multi-strategy fusion mechanism on the initial population based on the multi-objective weighted fitness function. When the iteration termination condition is reached, the optimal control point sequence is obtained. The multi-strategy fusion mechanism includes: introducing Levy perturbation in velocity update, adopting an elite-guided strategy in velocity update, and implementing a constraint repair mechanism for infeasible solutions obtained during the optimization process.

[0093] The feasible path determination module 605 is used to generate a smooth trajectory based on the optimal control point sequence and determine the feasible path of the UAV based on the smooth trajectory.

[0094] In one exemplary embodiment, a computer device is provided, which may be a server or a terminal, and its internal structure diagram may be as follows. Figure 7As shown, this computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and a database. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The database stores video tag processing data. The I / O interfaces are used for exchanging information between the processor and external devices. The communication interface is used for communication with external terminals via a network connection. When executed by the processor, the computer program implements a three-dimensional path planning method for unmanned aerial vehicles (UAVs).

[0095] Those skilled in the art will understand that Figure 7 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer equipment to which the present application is applied. Specific computer equipment may include, for example, [the following is a list of possible additional structures]. Figure 7 The embodiments show more or fewer components, combinations of certain components, or different component arrangements. In one exemplary embodiment, a computer device is provided, including a memory and a processor, the memory storing a computer program, which the processor executes to implement the steps in the above-described method embodiments.

[0096] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0097] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0098] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0099] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A three-dimensional path planning method for unmanned aerial vehicles (UAVs), characterized in that, The UAV three-dimensional path planning method includes: Construct an environmental model of the UAV mission area; the environmental model includes: the UAV starting point, the UAV ending point, and a three-dimensional obstacle model with safety margin; Based on the environmental model, with the starting point of the UAV as the first point and the ending point of the UAV as the last point, the path to be planned is modeled as a sequence of control points, and the intermediate control points in the sequence of control points are used as the position vectors of the particles to initialize the particle population, thus obtaining the initial population. A multi-objective weighted fitness function is constructed based on path length, obstacle avoidance penalty, trajectory smoothness, and energy consumption. Based on the multi-objective weighted fitness function, a particle swarm optimization algorithm with a multi-strategy fusion mechanism is used to iterate and optimize the initial population multiple times. When the iteration termination condition is reached, the optimal control point sequence is obtained. The multi-strategy fusion mechanism includes: introducing Levy perturbation in velocity update, adopting an elite-guided strategy in velocity update, and implementing a constraint repair mechanism for infeasible solutions obtained during the optimization process. A smooth trajectory is generated based on the optimal control point sequence, and a feasible path for the UAV is determined based on the smooth trajectory. The expression for the multi-objective weighted fitness function is: ； ； ； ； in, This represents a multi-objective weighted fitness function; Indicates the path length item; This indicates the penalty for obstacle avoidance; This represents the trajectory smoothness term; Indicates energy consumption; Represents the path encoding vector; This indicates the weight corresponding to the path length item; This indicates the weight corresponding to the obstacle avoidance penalty item; Indicates the weight corresponding to the trajectory smoothness term; This indicates the weight corresponding to the energy consumption item; Indicates the sequence number of the control point; Indicates the number of control points; Indicates the first The coordinates of the control points; Indicates the first The coordinates of the control points; This indicates the number of intermediate control points in the control point sequence. Indicates the first Local heading angles at each control point; Indicates the first Local heading angles at each control point; Indicates the first Local heading angles at each control point; Indicates the first Z-axis height coordinates of each control point; Indicates the first Z-axis height coordinates of each control point; Indicates the first Z-axis height coordinates of each control point; Indicates the first Vertical velocity of the path segment; Indicates the set cruise speed; For the first Turning radius of the segment path; Indicates the first coefficient; Indicates the second coefficient; Indicates the third coefficient; The speed update formula is: ； in, Indicates the first Individual particles Update speed during the next iteration; Indicates the first Individual particles The current velocity at the next iteration; express Inertia weight at any given moment; Indicates an individual's learning factor; This represents the second random number within the interval [0,1]. Represents the global learning factor; This represents the second random number within the interval [0,1]. Indicates adaptive step size; Indicates the first The individual historical best position found by each particle during the search process; Indicates the first Individual particles The current position at the next iteration; This represents the globally optimal position found by the entire particle swarm during the search process; The characteristic index parameter represents the Lévy distribution; This represents a Levy random vector.

2. The UAV three-dimensional path planning method according to claim 1, characterized in that, Constructing an environmental model of the UAV mission area, specifically including: Define a bounded three-dimensional Euclidean space in the world coordinate system; Within the bounded three-dimensional Euclidean space, all obstacles within the UAV mission area are modeled to obtain an obstacle set; Minkowski and expansion operations are performed on each obstacle in the obstacle set according to the minimum safe distance to generate a three-dimensional obstacle model with a safety margin; the minimum safe distance is determined based on the maximum outer radius of the UAV body, the UAV positioning system error, and the flight control response margin.

3. The UAV three-dimensional path planning method according to claim 1, characterized in that, The particle population is initialized by using the intermediate control points in the control point sequence as the particle position vectors, resulting in an initial population, specifically including: The intermediate control points in the control point sequence are assigned to the control vertices of multiple cubic Bézier curves, and the first derivatives at the connection points of adjacent cubic Bézier curves are continuous. Based on the corresponding intermediate control points, the coordinates of the intermediate control points are randomly generated in the environmental model. The collision detection algorithm determines whether the path formed by the intermediate control points at this coordinate interferes with the three-dimensional obstacle model. If there is no interference and the distance between intermediate control points is within the set distance range, then the coordinates of the intermediate control points are determined as the particle swarm that meets the obstacle avoidance conditions, and the initial population is obtained; otherwise, the coordinates of the intermediate control points are regenerated.

4. The UAV three-dimensional path planning method according to claim 1, characterized in that, The iteration termination conditions include: the number of iterations reaches the set maximum number of iterations, the value of the multi-objective weighted fitness function remains unchanged for a set number of consecutive iterations, or the obtained feasible solution satisfies the set threshold.

5. The UAV three-dimensional path planning method according to claim 1, characterized in that, Generating a smooth trajectory based on the optimal control point sequence, and determining a feasible path for the UAV based on the smooth trajectory, specifically includes: Based on the optimal control point sequence, a smooth trajectory is generated using B-spline or Bézier interpolation methods. The smooth trajectory is verified. If the smooth trajectory satisfies the UAV kinematic constraints, the smooth trajectory is determined to pass the verification and is identified as a feasible path for the UAV. The UAV kinematic constraints include: turning radius constraints, vertical speed constraints, and control command constraints.

6. A three-dimensional path planning device for unmanned aerial vehicles (UAVs), characterized in that, The UAV three-dimensional path planning device employs the UAV three-dimensional path planning method according to any one of claims 1-5, and the UAV three-dimensional path planning device comprises: An environment model building module is used to construct an environment model of the UAV mission area; the environment model includes: the UAV starting point, the UAV ending point, and a three-dimensional obstacle model with safety margin; The population initialization module is used to model the path to be planned as a sequence of control points based on the environment model, with the starting point of the UAV as the first point and the ending point of the UAV as the last point, and to initialize the particle population by using the intermediate control points in the control point sequence as the position vectors of the particles, so as to obtain the initial population. The fitness function construction module is used to construct multi-objective weighted fitness functions based on path length, obstacle avoidance penalty, trajectory smoothness, and energy consumption. The control point optimization module is used to perform multiple iterations of particle swarm optimization algorithm with a multi-strategy fusion mechanism on the initial population based on the multi-objective weighted fitness function. When the iteration termination condition is reached, the optimal control point sequence is obtained. The multi-strategy fusion mechanism includes: introducing Levy perturbation in velocity update, adopting an elite-guided strategy in velocity update, and implementing a constraint repair mechanism for infeasible solutions obtained during the optimization process. A feasible path determination module is used to generate a smooth trajectory based on the optimal control point sequence, and determine the feasible path of the UAV based on the smooth trajectory.

7. A computer device, comprising: A memory, a processor, and a computer program stored in the memory and capable of running on the processor, characterized in that the processor executes the computer program to implement the UAV three-dimensional path planning method according to any one of claims 1-5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by a processor, the computer program implements the UAV three-dimensional path planning method as described in any one of claims 1-5.

Images