Unmanned aerial vehicle path planning method and system based on ellipsoid sampling and adaptive expansion

Abstract

The application belongs to the technical field of unmanned aerial vehicle autonomous navigation and motion planning, and discloses an unmanned aerial vehicle path planning method and system based on ellipsoid sampling and adaptive expansion. A dynamic ellipsoid sampling region is constructed, sampling points are constrained to the region most likely to be connected between two random trees, and a lateral random offset is introduced. A multi-level target bias guide node expansion is performed, target straight line expansion and conical multi-ray obstacle avoidance expansion are sequentially executed, and a potential field correction mechanism is fused in the random expansion mode. The target bias probability is adjusted based on the expansion success rate in the window, and the expansion step, the conical amplification factor and the bias probability are jointly adjusted. The initial collision-free trajectory is subjected to greedy shortcut optimization, adaptive interpolation and B-spline multi-target trajectory optimization, and a smooth trajectory meeting the dynamic constraint is generated. The application is superior to the traditional RRT and related variant algorithms in dense obstacle and narrow channel scenes, and improves the path planning efficiency, path quality and robustness of the unmanned aerial vehicle in a complex urban environment.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and motion planning technology for unmanned aerial vehicles (UAVs), specifically to a UAV path planning method and system based on ellipsoidal sampling and adaptive extension. Background Technology

[0002] In recent years, the low-altitude economy, as an emerging industry, has seen its status continuously rise and its application scenarios expand, placing unprecedented demands on UAV trajectory planning technology. In complex urban low-altitude environments, UAVs need to quickly plan efficient and safe flight trajectories amidst dense building clusters. However, existing trajectory planning algorithms suffer from poor path quality, slow convergence speed, and low planning success rate when addressing these challenges, significantly impacting the mission execution and completion efficiency of UAVs.

[0003] Unmanned aerial vehicle (UAV) path planning algorithms are mainly divided into two categories: global planning algorithms and local planning algorithms. Global planning utilizes prior map information to plan a collision-free, safe trajectory for the UAV. Its quality depends on the degree of understanding of the global environment and mainly includes: graph search-based path planning algorithms, such as Dijkstra's algorithm and A... Algorithms include sampling-based path planning algorithms such as the Probabilistic Roadmaps Method (PRM) and Rapidly-exploring Random Tree (RRT); and biomimetic intelligence-based path planning algorithms such as the Ant Colony Algorithm (ACA) and Particle Swarm Optimization (PSO). Although domestic and international scholars have made numerous improvements to global path planning algorithms for different application scenarios and task requirements, these improvements often only optimize a single aspect and still face core problems such as slow convergence speed, poor path quality, parameter sensitivity, and low success rate in narrow passage planning, making it difficult to effectively balance planning efficiency and path quality.

[0004] Among existing local path planning algorithms, the Artificial Potential Field (APF) method is simple in structure and computationally efficient, but it suffers from problems such as target unreachability and local minima, and the trajectory is prone to oscillation in environments with dense obstacles. The Dynamic Window Approach (DWA), while offering strong real-time performance and directly considering dynamic constraints, lacks foresight; its fixed-weight evaluation function has poor adaptability in different environments and is prone to getting trapped in local optima, leading to planning failure. Therefore, practical applications often employ an architecture that integrates global and local planning. The global planning algorithm provides macroscopic path guidance, while the local planning algorithm, based on this, perceives potential sudden threats and performs real-time obstacle avoidance.

[0005] The RRT algorithm, a classic sampling-based path planning algorithm, does not require explicit modeling of the environment and can reduce computational complexity and shorten planning time in high-dimensional and complex spaces. However, due to its strong randomness in expansion, it cannot guarantee the quality of the solution. To address the problem of non-optimal paths in the RRT algorithm, RRT... The algorithm introduces parent node reselection and rewiring mechanisms to make the path asymptotically converge to the global optimum with increasing iterations. However, RRT The algorithm's asymptotic optimality requires a large number of iterations to converge, and its search efficiency drops sharply in environments with dense obstacles. The quality of the initial path is still affected by randomness, with a large number of redundant nodes and abrupt changes in direction, resulting in poor smoothness and difficulty in meeting the dynamic constraints of UAVs.

[0006] Therefore, for RRT To address the aforementioned shortcomings of the algorithm, scholars both domestically and internationally have proposed variant algorithms from multiple perspectives. RRT The Smart algorithm optimizes path extraction of beacon points based on the triangle inequality principle, guiding the random tree to perform dense sampling within a spherical region centered on the beacon points, generating a smoother path with fewer nodes; EP-RRT The algorithm uses the path itself as heuristic information, constructing a dynamically shrinking sampling region by expanding the path, significantly improving the planning efficiency in complex environments such as narrow passages and mazes; NN-RRT The algorithm utilizes the combined cost of neural network computing nodes to effectively handle multi-feature nonlinear interaction problems, resulting in stronger path adaptation capabilities. However, most improvements are still concentrated in single stages, lacking a complete solution that smoothly transitions from front-end search to back-end. How to systematically integrate various improvement strategies and achieve synergistic optimization across stages remains a critical issue that urgently needs to be addressed. Summary of the Invention

[0007] This invention aims to overcome the shortcomings of existing technologies and solve problems such as low search efficiency, poor parameter adaptability, unsatisfactory initial path quality, and non-smooth generated trajectories or failure to meet dynamic constraints in UAV path planning in complex urban environments. This invention forms a complete technical solution from front-end search to back-end smoothing, encompassing sampling constraints, node expansion, parameter adaptation, and trajectory optimization. The steps are tightly coupled and work collaboratively, significantly improving the overall performance of UAV global path planning in complex environments and providing key technical support for the safe and efficient flight of UAVs in dense urban low-altitude environments. In complex urban environments, an improved fast-expanding random tree algorithm is used for front-end path search, and a multi-stage path optimization strategy is used for back-end trajectory generation, ultimately resulting in an efficient and safe UAV path planning method and system for flight trajectories.

[0008] The technical solution of this invention is as follows: A UAV path planning method based on ellipsoidal sampling and adaptive expansion, comprising the following steps: Step 1: Construct the dynamic ellipsoidal sampling region; The starting point and the target point are used as the root nodes of the random tree to construct the starting point tree and the target tree respectively; based on the nearest node pair of the starting point tree and the target tree, the center of the dynamic ellipsoid sampling region is determined, and the radius of the ellipsoid is dynamically adjusted according to the nearest distance between the starting point tree and the target tree to guide the random tree to grow in the region most likely to be connected. Step 2: Node expansion strategy guided by multi-level target bias and potential field; Random point sampling is performed in the dynamic ellipsoid sampling area. When the random number is less than or equal to the target bias probability, the target-oriented expansion mode is triggered, and the target linear expansion and conical multi-ray obstacle-around expansion are performed in sequence. If the conical multi-ray obstacle-around expansion fails or the random number is greater than the target bias probability, the random expansion mode is triggered, and the artificial potential field method is introduced to make the new node expand along the resultant force direction, while maintaining the completeness of the probability and generating nodes that are far away from the obstacle and tend towards the target. Step 3: Adjust the adaptive bias probability and extended parameters in conjunction; Based on the success rate of the target-oriented expansion mode within the window, the target bias probability is adaptively adjusted. The expansion step size and conical magnification factor are linearly linked and adjusted with the current bias probability until the node is expanded to the target point range. The node is then traced back to obtain the initial collision-free trajectory. Step 4: Multi-stage trajectory optimization strategy based on B-spline curves; The initial collision-free trajectory is sequentially subjected to greedy shortcut optimization, adaptive interpolation, and B-spline-based multi-objective optimization. By constructing a multi-objective cost function containing smoothness, safety, and dynamic constraints, the optimal control point sequence is solved, and finally a safe and smooth trajectory that satisfies the dynamic constraints of the UAV is generated.

[0009] Step 1 specifically includes: Step 1-1: Determine the center point of the dynamic ellipsoid sampling region: Construct a starting point tree with the starting point and the target point as the root nodes. and target tree Calculate and compare the Euclidean distances between all nodes in two random trees to determine the nearest node pair: (1) in, The shortest distance between two random trees; and They are the starting point trees. Nodes and target tree The nodes on; For nodes With nodes The Euclidean distance between them; The center point of the dynamic ellipsoidal sampling region From the midpoint of the nearest node pair Determined, defined as: (2) in, Starting point tree Up distance to target tree The nearest node; For target tree Up distance from the starting point tree The nearest node; At the midpoint The superposition point is perpendicular to the line connecting the starting point and the target point. Random unit direction vector in orthogonal plane , represented as: (3) in, The random azimuth angle determines the offset direction; They are respectively with Orthogonal unit orthogonal basis; express It follows a uniform probability distribution, and its value ranges from 0 to... The center point after superimposing the lateral random offset is marked with a symbol. The formula is shown below: (4) in, This is the offset distance; This is the offset scaling factor; To extend the step size; Step 1-2: Construct the dynamic ellipsoidal sampling region: Based on the obtained center point after superimposed lateral random offset Define the dynamic ellipsoid sampling region: (5) in, This is a three-dimensional sampling area; Sampling points 3D coordinates; The center point 3D coordinates; The sampling radius; , is the high compression ratio coefficient; According to the starting point tree and target tree closest distance Dynamically adjust sampling radius Sampling radius Defined as: (6) in, These are the preset maximum ellipsoid radius and the preset minimum ellipsoid radius, respectively; This represents the distance between the starting point and the target point.

[0010] Step 2 specifically includes: Step 2-1: Target line expansion: Generate random numbers ,like Less than or equal to the target bias probability Then, within the dynamic ellipsoid sampling region, the target-guided expansion mode is triggered. First, the first-level target straight-line expansion is performed, extending the target point... As sampling points, new nodes are expanded along the target direction. : (7) in, It is the nearest node in the random tree; If the new node is expanded and If the line connecting the target collides with an obstacle, the first-stage straight-line expansion fails, and the second-stage conical multi-ray obstacle-around expansion is initiated; if the new node of the expansion... and If the connection between the line and the obstacle passes the collision detection, it is added to the corresponding starting tree. or target tree ; Step 2-2: Conical multi-ray obstacle-around expansion: in the target direction Define the conical region as the axis of the cone: (8) in, Represents a set of conical regions; Indicates a half angle of a cone; This represents the direction vector of the ray inside the cone; Represents a unit sphere in three-dimensional space; express and The included angle; the first within the conical region The rectangular coordinates of the standard unit vector of the ray are: (9) in, Represents a unit vector. Indicates the polar angle; Indicates azimuth; The first term can be expressed in the form of vector addition. The direction vector of the ray : (10) in, Let be the axial basis vector of the cone. They are respectively with Orthogonal unit vectors; Along direction vector Generate new nodes : (11) in, Indicates the conical magnification factor; The new nodes corresponding to each ray are checked sequentially, and the first new node that passes the collision detection is added to the random tree and the expansion is terminated. When all rays in the conical multi-ray barrier expansion fail to expand or a random number of rays are generated... When the time comes, switch to random expansion mode and add new nodes to the random tree; Steps 2-3: Random Extension Mode: The random extension mode integrates the artificial potential field method to construct a hybrid gravitational field and a repulsive field, thus creating a hybrid gravitational field. Gravity at the target point and gravity of random sampling points Weighted combination: (12) (13) (14) in, and These are the gravitational gain coefficients at the target point and the random point, respectively. and These are the corresponding weighting coefficients; The distance between the sampling point and the random point; The distance between the sampling point and the target point; For those within the range of repulsive force The first An obstacle, and the repulsive force it generates. It consists of two components: First repulsive component The direction is from the obstacle to the current node: (15) Second repulsive component The direction is from the current node to the target point: (16) in, This is the repulsive force gain coefficient; Distance adjustment factor; For sampling points and the environment The distance between the obstacles; The distance between the sampling point and the target point; If the current node's repulsive force has a range of effect The total number of obstacles inside is Then the total resultant force on the node is: (17) The new node extends along the direction of the total resultant force, and its location is: (18).

[0011] Step 3 specifically includes: Step 3-1: Adaptive target bias probability: Define the sliding window size as... Record the latest Results of secondary goal-oriented expansion 1 indicates that the new node was successfully inserted. Indicates failure; calculate the success rate of expanding within the window: (19) Based on the success rate of in-window expansion Calculate the expected target bias probability : (20) in, and These represent the minimum and maximum target bias probabilities, respectively; the current target bias probability is recursively updated using first-order exponential smoothing. (twenty one) in, It is a smoothing factor; Indicates the first The target bias probability of the wheel. Indicates the first The target bias probability of the wheel; Step 3-2: Linkage adjustment of step size and conical magnification factor: bias probability of the current target Normalization is performed to obtain the weighting factors. = Expand step size and conical magnification factor With weighting factors Perform linear linkage adjustment: (twenty two) (twenty three) in, and These are the maximum and minimum preset expansion step sizes, respectively. and These are the maximum and minimum preset conic magnification factors, respectively; repeat steps 1 to 3 until the range starting point tree is reached. Up and target tree Successful connection, backtracking nodes yields the initial collision-free trajectory. .

[0012] Step 4 specifically includes: Step 4-1: Greedy Shortcut Optimization: Initial Collision-Free Trajectory ,in Starting position The target point location, Define the connection point between two random trees; define the window range of the connection point. : (twenty four) in, Near the connection point One node; From the left node of the window Begin by traversing the right-hand nodes of the window in reverse order. Detect node distance Does it satisfy the distance constraint? No collision constraints and safety margin Constraints, among which To preset the maximum shortcut distance, the farthest node pair that satisfies all constraints will be selected. By performing straight-line connections and replacing the original local path segments, we obtain the locally optimized path at the connection points. ; The path based on the locally optimized connection point From the starting point Begin by traversing subsequent nodes. The system checks whether the distance between nodes simultaneously satisfies the distance constraint, the no-collision constraint, and the safety margin constraint. It then selects the farthest node that satisfies the constraints and directly connects it to the current node, deleting all redundant intermediate nodes until the target point is reached. After optimization, the number of path nodes is reduced to [number missing]. , represented as Its length satisfies ; Step 4-2: Adaptive interpolation: Traverse the optimized path The length of the straight line segment is used to determine whether interpolation is needed, when adjacent nodes... European distance When this occurs, no interpolation is performed on the line segment; The interpolation distance threshold; when When calculating, the number of points that need to be inserted uniformly is calculated. : (25) No. The positions of the interpolation points are: ,in All interpolation points lie on the original line segment, ultimately resulting in a dense path with evenly distributed nodes. ; This represents the total number of nodes in the dense path. Step 4-3: Constructing a multi-objective optimization problem based on B-splines: using dense paths Using ordered nodes as initial control points, cubic uniform B-spline curves are used to map dense paths. Parametric representation: The cubic uniform B-spline curve is derived from the sequence of control points. Definition: The trajectory optimization problem is transformed into a multi-objective nonlinear optimization problem. A multi-objective cost function is constructed, the optimal control point sequence is found, and the multi-objective cost function is minimized. (26) in, These are the weighting coefficients for each cost item; Path length cost Record the length of the planned path and select the shortest feasible path; Smoothing Cost Using an elastic band model, calculations were performed at three consecutive control points. , and The square norm of the composite deformation vector measures the smoothness of the curve; the cost formula is defined as: (27) Security Costs A piecewise penalty function is used for each control point. Calculate the signed distance to the nearest obstacle. ,when A second penalty is applied at the same time; the cost formula is defined as: (28) in, These are the weighting coefficients; Minimum safe distance; Cost of dynamic constraints : Utilizing the convex hull property of a cubic uniform B-spline curve, by controlling the velocity of the control points acceleration and accelerometer Constraints are applied, and each penalty function independently penalizes each directional component in three-dimensional space, taking a value of zero within the constraint boundary and exhibiting quadratic or cubic growth outside the boundary; the cost formula is defined as: (29) in, These are the weighting coefficients for each item; , , This is a penalty function used to measure the velocity of the control points. acceleration and accelerometer Does it exceed the dynamic limits of the drone? (Acceleration) For acceleration The derivative; The gradient descent method is used to iteratively solve the multi-objective cost function to obtain the optimal control point sequence. The sequence is then substituted into the expression of a cubic uniform B-spline curve to generate the final optimized trajectory that satisfies smoothness, safety, and dynamic feasibility.

[0013] A UAV path planning system based on ellipsoidal sampling and adaptive extension includes a processor, a memory, and a computer program stored in the memory. When the processor executes the computer program, it implements a UAV path planning method based on ellipsoidal sampling and adaptive extension.

[0014] The beneficial effects of this invention are: 1) Improve path search efficiency: By constructing a dynamic ellipsoidal sampling region, the sampling points are concentrated in the most likely connected areas, reducing invalid expansion and accelerating the convergence speed of the algorithm; at the same time, the introduction of lateral random offset enhances the search capability for narrow channels and effectively improves the adaptability of the algorithm in dense obstacle environments. 2) Achieved an adaptive balance between target orientation and local obstacle avoidance: By using a multi-level target bias strategy, a conical multi-ray obstacle avoidance mechanism is activated when the target's straight-line expansion is blocked, which avoids blind expansion and enhances the local obstacle avoidance capability; a potential field correction mechanism is added during the random expansion process, so that the node can move away from the obstacle when expanding, and the target point is reached through the distance adjustment factor. 3) Adaptability and robustness: The sliding window expansion success rate feedback mechanism is used to adaptively adjust the target bias probability; at the same time, the expansion step size and conical magnification factor are linked to the bias probability for adjustment, so that the algorithm can converge quickly in coarse search in open environment and ensure safety in fine search in obstacle-dense environment, thus improving the robustness and adaptability of the algorithm. 4) Higher quality generated trajectories: Through a multi-stage trajectory optimization strategy that combines greedy shortcut optimization, adaptive interpolation, and B-spline-based multi-objective optimization, the optimal control point sequence is solved to generate executable UAV trajectories that simultaneously satisfy dynamic constraints, high-order smoothness, and strict collision-free behavior. 5) The system's comprehensive performance advantages were verified: In four complex urban environments with different obstacle densities and characteristics, the present invention outperformed traditional RRT in terms of planning efficiency, path quality, narrow passage capacity, and trajectory smoothness. Its related variant algorithms are particularly suitable for UAV autonomous trajectory planning tasks in dense urban low-altitude environments, and have significant innovative and practical engineering value. Attached Figure Description

[0015] Figure 1 This invention relates to an improved RRT (Recursive Terrain-Based Path Planning) method for unmanned aerial vehicles (UAVs) based on ellipsoidal sampling and adaptive extension. Algorithm overall flowchart.

[0016] Figure 2 To improve RRT for this invention Algorithm and Traditional RRT Comparison charts of the performance of the algorithm and its variants in obstacle-dense and narrow passage scenarios, where (a) is a comparison chart of average path length, (b) is a comparison chart of average planning time, (c) is a comparison chart of average number of nodes, and (d) is a comparison chart of average number of iterations.

[0017] Figure 3 This is a schematic diagram of the multi-ray barrier-circling mechanism of a cone.

[0018] Figure 4 A schematic diagram illustrating the stochastic extension of the artificial potential field method.

[0019] Figure 5 The image shows a comparison of the path planning results of various algorithms in obstacle-dense scenarios. (a) shows the RRT results. (b) is BI-RRT (c) is BI-APF-RRT (d) is the algorithm of this invention.

[0020] Figure 6 The image shows a comparison of the path planning results of various algorithms in a narrow passage scenario. (a) shows the RRT results. (b) is BI-RRT (c) is BI-APF-RRT (d) is the algorithm of this invention.

[0021] Figure 7 The images show a comparison of the path planning optimization effects before and after the algorithm of this invention. (a) is a scene with dense obstacles, and (b) is a scene with a narrow passage. Detailed Implementation

[0022] To enable those skilled in the art to fully understand and implement the present invention, the technical solution of the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0023] Example 1: A UAV path planning method based on ellipsoidal sampling and adaptive expansion, the steps of which are as follows: Step 1: Construct a dynamic ellipsoidal sampling region; by constructing a dynamically shrinking ellipsoidal sampling region, the sampling points are concentrated in the region between two random trees that are most likely to be connected, avoiding blind search in the entire state space.

[0024] Step 1.1: Determine the center point of the dynamic ellipsoid sampling region; The simulated urban environment with dense obstacles is set to a range of 200m. 200m 100m, starting from the starting point Build a starting tree with rice as the root node , with target point Construct the target tree with meter as the root node Calculate the Euclidean distance between all nodes in the two trees, determine the closest node pair, record the closest distance between the two trees, and calculate the midpoint of the closest node pair. In the direction perpendicular to the line connecting the starting point and the target point Generate random unit direction vectors in the orthogonal plane Superimposed on the midpoint To improve the lateral search capability of the algorithm, the center point of the ellipsoidal region is now... Set the offset ratio coefficient. Expand step size The subsequent steps will provide adaptive adjustments.

[0025] Step 1.2: Construct the dynamic ellipsoidal sampling region; With the center point A dynamic ellipsoidal sampling region is constructed based on this; the ellipse height compression ratio is set to [value missing]. Ellipse radius The shortest distance between two trees Dynamically adjusted; the straight-line distance from the starting point to the target point is Then the maximum and minimum radii are set as follows: .

[0026] Step 2. Node expansion strategy guided by multi-level target bias and potential field; Within the dynamic ellipsoidal constraint sampling area constructed in step 1, the target-oriented expansion mode is triggered by the adaptive bias probability. The target linear expansion and the conical multi-ray obstacle-around expansion are executed sequentially. In the random expansion mode, a potential field correction mechanism is introduced to make the new node expand along the direction of the total resultant force, generating a node that is far away from the obstacle and tends towards the target.

[0027] Multi-level target offset propagation, including target linear propagation and conical multi-ray obstacle propagation; Generate random numbers ,like Trigger the target-guided expansion mode within the current ellipsoidal constrained sampling region; if Execute random expansion mode.

[0028] First, perform the first-level target linear expansion: directly extend the target point... As sampling points, along the target direction Generate new nodes ,like The node in the current random tree that is closest to the target direction If the connection does not collide with any obstacle, the new node is added to the random tree.

[0029] When the target's straight-line expansion is obstructed, instead of directly abandoning that direction, a conical region is formed around the original direction to attempt obstacle avoidance, effectively enhancing local obstacle avoidance capability while maintaining target orientation. The second-stage conical multi-ray obstacle avoidance mechanism is activated: [The mechanism operates along the target direction]. Set the cone half angle as the axis of the cone. In the conical region Multiple rays are generated internally.

[0030] , in , To and Orthogonal unit vectors; Indicates the polar angle; This indicates the azimuth angle. New nodes are generated along this direction. ,in This is the current cone magnification factor, which will be adaptively adjusted in subsequent steps. New nodes corresponding to each ray are detected sequentially, and the first new node to pass the collision detection is added to the random tree. If all ray expansions fail, the system switches to random expansion mode.

[0031] In the stochastic expansion mode, a hybrid gravitational field is constructed to apply gravity to the target point. and gravity of random sampling points By performing a weighted combination, a mixed gravity is obtained. Set the gravitational gain coefficient at the target point. Gravitational gain coefficient at random sampling points The weighting coefficients are respectively and .

[0032] Construct an improved repulsive field function for the first [value] within the repulsive field range. An obstacle generates a repulsive force consisting of two components. and Composition; Parameter settings: repulsion gain coefficient Range of repulsive force Distance adjustment factor .

[0033] The combined force of the mixed attraction and the improved repulsion guides the expansion of the node, allowing the expanded node to move away from obstacles while approaching the target, thus significantly improving the safety of the path.

[0034] Step 3. Adjust the adaptive bias probability in conjunction with the extended parameters; This step adaptively adjusts the target bias probability based on the success rate of the target-oriented expansion mode within the window, while linearly adjusting the expansion step size and conical magnification factor in conjunction with the current bias probability to enhance the algorithm's adaptability and robustness in complex environments.

[0035] Step 3.1, Adaptive target bias probability; Target bias probability From the nearest window The success rate of secondary goal-oriented expansion is determined by setting parameters. ; Expand success rate ,in 1 indicates successful expansion; insert the new node. This indicates failure.

[0036] Calculate the expected target bias probability based on the expansion success rate. Maximum target bias probability minimum value Then, the current target bias probability is recursively updated using first-order exponential smoothing. Smoothing factor Set to 0.3. This mechanism does not require precise tuning of the target bias probability, but rather dynamically adjusts the probability based on environmental information through a closed-loop feedback mechanism, exhibiting strong adaptability and robustness.

[0037] Step 3.2: Adjust the step size and conical magnification factor in conjunction with the circuit. The weighting factor is obtained by normalizing the current target bias probability. The step size and conical magnification factor are linearly linked and adjusted with the weighting factor to expand the step size range. Meters, conical magnification factor range When the environment is open and the success rate of expansion is high, the weighting factor... To accelerate convergence, increase the step size and conical magnification factor to approach 1; when obstacles are dense and expansion is frequently hindered, the weighting factor... Approaching 0, reduce the step size and conical magnification factor for a finer search.

[0038] Step 4. Multi-stage trajectory optimization strategy based on B-spline curves; The goal of this step is to sequentially perform greedy shortcut optimization, adaptive interpolation, and cubic uniform B-spline curve fitting on the initial collision-free trajectories generated in steps 1 to 3. By constructing a multi-objective cost function that includes smoothing, safety, and dynamic constraints, the optimal control point sequence is solved, and finally a safe and smooth trajectory that satisfies the dynamic constraints of the UAV is generated.

[0039] Step 4.1: Optimize the greedy shortcut; Local shortcut optimization at connection points: Set nodes near connection points Determine the window range near the connection point. From the left node of the window Start traversing the right-hand nodes of the window in reverse order. Check whether the connection between the nodes satisfies the distance constraint. Collision-free constraints and safety margin constraints By connecting the farthest node pairs that satisfy all constraints with straight lines and replacing the original local path segments, the problem of abrupt changes in direction at the connection points of the two trees is effectively solved.

[0040] Global Greedy Shortcut Optimization: Paths Based on Local Optimization of Connector Points Starting from the origin, traverse subsequent nodes, select the farthest node that satisfies the above constraints and connect it directly to the current node, delete all redundant nodes in between, and repeat the above process until the target point is reached, thus obtaining the optimized path. .

[0041] Step 4.2, Adaptive Interpolation; Set interpolation distance threshold Traverse the greedily optimized path Calculate the Euclidean distance between all adjacent nodes in the array. .like No interpolation is performed on this line segment; if Insert evenly on the line segment A new point. All interpolation points lie strictly on the original line segment and do not change the topology of the path.

[0042] Step 4.3: Multi-objective optimization problem based on B-splines; Constructing a multi-objective cost function: ; Path length cost Record whether the planned path satisfies the shortest length condition, and assign weights. ; Smoothing Cost Using the elastic band model, weights The trajectory is inherently smooth by minimizing the degree of non-collinearity of three consecutive control points.

[0043] Security Costs A piecewise penalty function is used, with weights For each control point Calculate the signed distance to the nearest obstacle. , Apply secondary punishment Set weight coefficients Minimum safe distance .

[0044] The cost of the dynamic constraint term utilizes the convex hull property of the B-spline curve, with weights... By controlling the speed of the control points acceleration and accelerometer Apply constraints, and set the weights as follows: This ensures that the trajectory's speed and acceleration strictly meet the dynamic constraints of the UAV.

[0045] Experimental verification and effect analysis; To verify the effectiveness and superiority of the method of the present invention, a three-dimensional flight path planning simulation platform for UAVs was built on a computer equipped with an Intel Core i7 processor and 16GB of memory, based on MATLAB R2021b. Dense obstacle environment and narrow passage environment were constructed to simulate the complex urban environment encountered by UAVs during actual flight.

[0046] Simulation scene setup; This invention selects environments with dense obstacles and narrow passages as the core test scenarios, mainly based on the following considerations: Dense obstacle environments are the most common operating scenarios for low-altitude flights in cities. When drones fly in a three-dimensional space with buildings and unevenly distributed obstacles, the algorithm needs to quickly identify passable paths among a large number of obstacles. This places extremely high demands on the target orientation and computational efficiency of the sampling strategy. In this scenario, the algorithm's search efficiency and obstacle avoidance capabilities in complex spaces are verified. Narrow passage environments represent extreme constraints, characterized by a severely compressed free space for passage, posing a significant challenge to the connectivity of path planning algorithms. Traditional RRT... The algorithm and its variants have an extremely low hit rate for sampling points in narrow channels, resulting in a significant drop in planning success rate and difficulty in guaranteeing the quality of the planned path. Therefore, this scenario allows us to verify the algorithm's connectivity and robustness under strong constraints.

[0047] Comparison methods and evaluation indicators selection; Select RRT BI-RRT and BI-APF-RRT The algorithm serves as a benchmark for comparison. The performance of the improved algorithm is comprehensively and objectively evaluated through assessment of four quantitative indicators: average path length, average planning time, average number of nodes, and average number of iterations. All algorithms operate at the same starting and target point locations in the simulation environment.

[0048] Experimental results; To verify the superiority of this invention, we first statistically analyzed various indicators of the algorithm in obstacle-dense scenarios. The algorithm proposed in this invention generates the shortest path length, has a more concentrated node distribution, and a shorter planning time. Experimental data are shown in Table 1.

[0049] Table 1 Performance data of various algorithms in scenes with dense obstacles To further verify the search capability and success rate of the algorithm in a strongly constrained environment, two vertical obstacle walls were set up, each with a narrow passage that allows the drone to pass through. The planning success rates of each algorithm were calculated, as shown in Table 2.

[0050] Table 2. Planning success rate of various algorithms in narrow passage scenarios Table 3 shows the successful planning data for each algorithm in the narrow passage scenario.

[0051] Table 3 Performance data of various algorithms in narrow passage scenarios Results analysis: The experimental data in Tables 1 to 3 verify the comprehensive performance advantages of the algorithm proposed in this invention from multiple dimensions. The following analysis focuses on two aspects: dense obstacle scenarios and narrow passage scenarios.

[0052] Analysis of dense obstacle scenes; In environments with dense obstacles, this invention achieved optimal performance across all four evaluation metrics. Regarding path length, the average length of this invention was 310.02m, compared to RRT. 347.73m, BI-RRT 351.98m and BI-APF-RRT The distance is reduced by 10.84%, 11.92%, and 7.98% respectively from 336.89m. This result shows that the present invention focuses the search on the most valuable region through ellipsoidal constrained sampling, and combined with the guidance of the potential field correction mechanism, effectively avoids unnecessary detours, making the planned path more compact and efficient.

[0053] In terms of planning efficiency, the average planning time of this invention is only 0.38s, which is comparable to RRT. 5.97s, BI-RRT 0.89s and BI-APF-RRT Compared to 0.52s, the time reductions were 93.63%, 57.30%, and 26.92%, respectively. This significant improvement is attributed to two factors: first, the ellipsoidal sampling region dynamically shrinks as the two trees approach each other, concentrating sampling points in high-value areas and greatly reducing invalid collision detections; second, the adaptive parameter adjustment mechanism enables the algorithm to automatically adopt smaller step sizes in dense environments, reducing the expansion failure rate caused by crossing obstacles.

[0054] In terms of node utilization and convergence speed, the average number of nodes in this invention is only 97, which is lower than that of RRT. The number of iterations was reduced by 91.03% from 1081; the average number of iterations was only 137, compared to RRT. The 1847 occurrences were reduced by 92.58%. Even the BI-APF-RRT, which performs best among comparison algorithms, achieved this. The number of nodes (160) and the number of iterations (481) of the previous method are still significantly higher than those of the present invention. This fully demonstrates that the present invention has significant advantages in sampling efficiency and convergence speed, and can quickly approximate the optimal path under conditions of extremely low resource consumption.

[0055] 5.3.2 Analysis of narrow passage scenarios; Narrow passage environments pose a severe challenge to the connectivity of planning algorithms. Table 2 shows that the standard RRT... The algorithm's planning success rate in narrow channels is only 4%. This is because uniform random sampling has an extremely low hit rate in narrow regions, making it difficult to extend the random tree into the channel within a limited number of iterations. BI-RRT and BI-APF-RRT Success rates of 81% and 67% were achieved, respectively. The former benefited from the bidirectional expansion mechanism, which increased the probability of both ends entering the channel simultaneously. The latter, although it introduced an artificial potential field for guidance, was prone to local oscillations at the channel entrance, which interfered with the stable expansion of the two trees, resulting in a lower success rate than BI-RRT. This invention achieved a 96% planning success rate in narrow passage scenarios, significantly outperforming all compared algorithms.

[0056] Based on the data from successful planning in Table 3, the average path length generated by this invention in narrow passage scenarios is 480.46m, which is significantly shorter than that of RRT. (483.25m) and BI-APF-RRT The path length (503.52m) is shortened, and the path standard deviation is smaller, indicating that the algorithm's path quality is more stable across multiple runs. In terms of planning time, this invention requires only 0.63s, compared to RRT. Shortened by 78.64%, compared to BI-APF-RRT Shortened by 4.55%; average number of nodes 226, compared to RRT Reduced by 82.32%, compared to BI-RRT Reduced by 38.59%; average number of iterations was 397, compared to BI-APF-RRT. The 1126 times were reduced by 64.74%. Even BI-APF-RRT, which performed best among all comparison algorithms, was not affected. The number of iterations (1126) and the number of nodes (364) of the previous invention still far exceed those of the present invention, indicating that the present invention has a significant leading advantage in search efficiency and stability in strongly constrained scenarios such as narrow channels.

[0057] In summary, the present invention achieves superior performance compared to existing methods in both dense obstacle scenarios and narrow passage scenarios, fully demonstrating the effectiveness and engineering applicability of the proposed technical solution in terms of planning efficiency, path quality, and robustness.

Claims

1. A UAV path planning method based on ellipsoidal sampling and adaptive expansion, characterized in that, The steps are as follows: Step 1: Construct the dynamic ellipsoidal sampling region; The starting point and the target point are used as the root nodes of the random tree to construct the starting point tree and the target tree respectively; based on the nearest node pair of the starting point tree and the target tree, the center of the dynamic ellipsoid sampling region is determined, and the radius of the ellipsoid is dynamically adjusted according to the nearest distance between the starting point tree and the target tree to guide the random tree to grow in the region most likely to be connected. Step 2: Node expansion strategy guided by multi-level target bias and potential field; Random point sampling is performed in the dynamic ellipsoid sampling area. When the random number is less than or equal to the target bias probability, the target-oriented expansion mode is triggered, and the target linear expansion and conical multi-ray obstacle-around expansion are performed in sequence. If the conical multi-ray obstacle-around expansion fails or the random number is greater than the target bias probability, the random expansion mode is triggered, and the artificial potential field method is introduced to make the new node expand along the resultant force direction, while maintaining the completeness of the probability and generating nodes that are far away from the obstacle and tend towards the target. Step 3: Adjust the adaptive bias probability and extended parameters in conjunction; Based on the success rate of the target-oriented expansion mode within the window, the target bias probability is adaptively adjusted. The expansion step size and conical magnification factor are linearly linked and adjusted with the current bias probability until the node is expanded to the target point range. The node is then traced back to obtain the initial collision-free trajectory. Step 4: Multi-stage trajectory optimization strategy based on B-spline curves; The initial collision-free trajectory is sequentially subjected to greedy shortcut optimization, adaptive interpolation, and B-spline-based multi-objective optimization. By constructing a multi-objective cost function containing smoothness, safety, and dynamic constraints, the optimal control point sequence is solved, and finally a safe and smooth trajectory that satisfies the dynamic constraints of the UAV is generated.

2. The UAV path planning method based on ellipsoidal sampling and adaptive expansion according to claim 1, characterized in that, Step 1 specifically includes: Step 1-1: Determine the center point of the dynamic ellipsoid sampling region: Construct a starting point tree with the starting point and the target point as the root nodes. and target tree Calculate and compare the Euclidean distances between all nodes in two random trees to determine the nearest node pair: (1) in, The shortest distance between two random trees; and They are the starting point trees respectively Nodes and target tree The nodes on; For nodes With nodes The Euclidean distance between them; The center point of the dynamic ellipsoidal sampling region From the midpoint of the nearest node pair Determined, defined as: (2) in, Starting point tree Up distance to target tree The nearest node; For target tree Up distance from the starting point tree The nearest node; At the midpoint The superposition point is perpendicular to the line connecting the starting point and the target point. Random unit direction vector in orthogonal plane , represented as: (3) in, The random azimuth angle determines the offset direction; They are respectively with Orthogonal unit orthogonal basis; express It follows a uniform probability distribution, and its value ranges from 0 to... The center point after superimposing the lateral random offset is marked with a symbol. The formula is shown below: (4) in, This is the offset distance; This is the offset scaling factor; To extend the step size; Step 1-2: Construct the dynamic ellipsoidal sampling region: Based on the obtained center point after superimposed lateral random offset Define the dynamic ellipsoid sampling region: (5) in, This is a three-dimensional sampling area; Sampling points 3D coordinates; The center point 3D coordinates; The sampling radius; , is the high compression ratio coefficient; According to the starting point tree and target tree closest distance Dynamically adjust sampling radius Sampling radius Defined as: (6) in, These are the preset maximum ellipsoid radius and the preset minimum ellipsoid radius, respectively; This represents the distance between the starting point and the target point.

3. The UAV path planning method based on ellipsoidal sampling and adaptive expansion according to claim 2, characterized in that, Step 2 specifically includes: Step 2-1: Target line expansion: Generate random numbers ,like Less than or equal to the target bias probability Then, within the dynamic ellipsoid sampling region, the target-guided expansion mode is triggered. First, the first-level target straight-line expansion is performed, extending the target point... As sampling points, new nodes are expanded along the target direction. : (7) in, It is the nearest node in the random tree; If the new node is expanded and If the line connecting the target collides with an obstacle, the first-stage straight-line expansion fails, and the second-stage conical multi-ray obstacle-around expansion is initiated; if the new node of the expansion... and If the connection between the line and the obstacle passes the collision detection, it is added to the corresponding starting tree. or target tree ; Step 2-2: Conical multi-ray obstacle-around expansion: in the target direction Define the conical region as the axis of the cone: (8) in, Represents a set of conical regions; Indicates a half angle of a cone; This represents the direction vector of the ray inside the cone; Represents a unit sphere in three-dimensional space; express and The included angle; the first within the conical region The rectangular coordinates of the standard unit vector of the ray are: (9) in, Represents a unit vector. Indicates the polar angle; Indicates azimuth; The first term can be expressed in the form of vector addition. The direction vector of the ray : (10) in, Let be the axial basis vector of the cone. They are respectively with Orthogonal unit vectors; Along direction vector Generate new nodes : (11) in, Indicates the conical magnification factor; The new nodes corresponding to each ray are checked sequentially, and the first new node that passes the collision detection is added to the random tree and the expansion is terminated. When all rays in the conical multi-ray barrier expansion fail to expand or a random number of rays are generated... When the time comes, switch to random expansion mode and add new nodes to the random tree; Steps 2-3: Random Extension Mode: The random extension mode integrates the artificial potential field method to construct a hybrid gravitational field and a repulsive field, thus creating a hybrid gravitational field. Gravity at the target point and gravity of random sampling points Weighted combination: (12) (13) (14) in, and These are the gravitational gain coefficients at the target point and the random point, respectively. and These are the corresponding weighting coefficients; The distance between the sampling point and the random point; The distance between the sampling point and the target point; For those within the range of repulsive force The first An obstacle, and the repulsive force it generates. It consists of two components: First repulsive component The direction is from the obstacle to the current node: (15) Second repulsive component The direction is from the current node to the target point: (16) in, This is the repulsive force gain coefficient; Distance adjustment factor; For sampling points and the environment The distance between the obstacles; The distance between the sampling point and the target point; If the current node's repulsive force has a range of effect The total number of obstacles inside is Then the total resultant force on the node is: (17) The new node extends along the direction of the total resultant force, and its location is: (18)。 4. The UAV path planning method based on ellipsoidal sampling and adaptive expansion according to claim 3, characterized in that, Step 3 specifically includes: Step 3-1: Adaptive target bias probability: Define the sliding window size as... Record the latest Results of secondary goal-oriented expansion 1 indicates that the new node was successfully inserted. Indicates failure; calculate the success rate of expanding within the window: (19) Based on the success rate of in-window expansion Calculate the expected target bias probability : (20) in, and These represent the minimum and maximum target bias probabilities, respectively; the current target bias probability is recursively updated using first-order exponential smoothing. (21) in, It is a smoothing factor; Indicates the first The target bias probability of the wheel. Indicates the first The target bias probability of the wheel; Step 3-2: Linkage adjustment of step size and conical magnification factor: bias probability of the current target Normalization is performed to obtain the weighting factors. = Expand step size and conical magnification factor With weighting factors Perform linear linkage adjustment: (22) (23) in, and These are the maximum and minimum preset expansion step sizes, respectively. and These are the maximum and minimum preset conic magnification factors, respectively; repeat steps 1 to 3 until the range starting point tree is reached. Up and target tree Successful connection, backtracking nodes yields the initial collision-free trajectory. .

5. The UAV path planning method based on ellipsoidal sampling and adaptive expansion according to claim 4, characterized in that, Step 4 specifically includes: Step 4-1: Greedy Shortcut Optimization: Initial Collision-Free Trajectory ,in Starting position The target point location, Define the connection point between two random trees; define the window range of the connection point. : (24) in, Near the connection point One node; From the left node of the window Begin by traversing the right-hand nodes of the window in reverse order. Detect node distance Does it satisfy the distance constraint? No collision constraints and safety margin Constraints, among which To preset the maximum shortcut distance, the farthest node pair that satisfies all constraints will be selected. By performing straight-line connections and replacing the original local path segments, we obtain the locally optimized path at the connection points. ; The path based on the locally optimized connection point From the starting point Begin by traversing subsequent nodes. The system checks whether the distance between nodes simultaneously satisfies the distance constraint, the no-collision constraint, and the safety margin constraint. It then selects the farthest node that satisfies the constraints and directly connects it to the current node, deleting all redundant intermediate nodes until the target point is reached. After optimization, the number of path nodes is reduced to [number missing]. , represented as Its length satisfies ; Step 4-2: Adaptive interpolation: Traverse the optimized path The length of the straight line segment is used to determine whether interpolation is needed, when adjacent nodes... European distance When this occurs, no interpolation is performed on the line segment; The interpolation distance threshold; when When calculating, the number of points that need to be inserted uniformly is calculated. : (25) No. The positions of the interpolation points are: ,in All interpolation points lie on the original line segment, ultimately resulting in a dense path with evenly distributed nodes. ; This represents the total number of nodes in the dense path. Step 4-3: Constructing a multi-objective optimization problem based on B-splines: using dense paths Using ordered nodes as initial control points, cubic uniform B-spline curves are used to map dense paths. Parametric representation: The cubic uniform B-spline curve is derived from the sequence of control points. Definition: The trajectory optimization problem is transformed into a multi-objective nonlinear optimization problem. A multi-objective cost function is constructed, the optimal control point sequence is found, and the multi-objective cost function is minimized. (26) in, These are the weighting coefficients for each cost item; Path length cost Record the length of the planned path and select the shortest feasible path; Smoothing Cost Using an elastic band model, calculations were performed at three consecutive control points. , and The square norm of the composite deformation vector measures the smoothness of the curve; the cost formula is defined as: (27) Security Cost A piecewise penalty function is used for each control point. Calculate the signed distance to the nearest obstacle. ,when Apply a second penalty The cost formula is defined as: (28) in, These are the weighting coefficients; Minimum safe distance; Cost of dynamic constraints : Utilizing the convex hull property of a cubic uniform B-spline curve, by controlling the velocity of the control points acceleration and accelerometer Constraints are applied, and each penalty function independently penalizes each directional component in three-dimensional space, taking a value of zero within the constraint boundary and exhibiting quadratic or cubic growth outside the boundary; the cost formula is defined as: (29) in, These are the weighting coefficients for each item; , , This is a penalty function used to measure the velocity of the control points. acceleration and accelerometer Does it exceed the dynamic limits of the drone? (Acceleration) For acceleration The derivative; The gradient descent method is used to iteratively solve the multi-objective cost function to obtain the optimal control point sequence. The sequence is then substituted into the expression of a cubic uniform B-spline curve to generate the final optimized trajectory that satisfies smoothness, safety, and dynamic feasibility.

6. A UAV path planning system based on ellipsoidal sampling and adaptive extension, characterized in that, The device includes a processor, a memory, and a computer program stored in the memory. When the processor executes the computer program, it implements the UAV path planning method based on ellipsoidal sampling and adaptive extension as described in any one of claims 1-5.

Images