A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles

Abstract

The present invention discloses a three-dimensional path multi-stage planning method for a low-altitude unmanned aerial vehicle, comprising the following steps: inputting a three-dimensional terrain model and a starting point for path planning; initializing the three-dimensional path based on a cubic B-spline curve and constructing an optimization model; performing active subspace identification and spatial mapping on the optimization variable space; executing an improved particle swarm algorithm in the active subspace and periodically rewarding the particles with the best fitness in the population; performing multi-stage refinement on the optimized three-dimensional path and executing a pattern search algorithm optimization on the densified path control points; and obtaining the optimized global optimal three-dimensional path.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) path planning and learning, and in particular to a three-dimensional path multi-stage planning method for low-altitude UAVs. Background Art

[0002] With the widespread application of low-altitude drones in logistics, emergency rescue, and geographic mapping, path planning technology in complex three-dimensional environments has become a key technical bottleneck restricting their intelligent development. Traditional path planning methods typically use the A* algorithm, rapid randomized tree (RRT), and their improved algorithms for three-dimensional path search. However, in complex low-altitude environments such as urban canyons and mountainous terrain, they suffer from poor path smoothness and slow convergence. In recent years, swarm intelligence optimization methods based on particle swarm optimization and genetic algorithms have been gradually applied to the field of drone path planning. However, these methods are prone to falling into local optimal solutions when optimizing in high-dimensional solution spaces and lack adaptability to three-dimensional terrain constraints.

[0003] Existing B-spline curve-based path modeling methods, while able to ensure path continuity, face dimensionality explosion when optimizing control points, significantly reducing computational efficiency. Furthermore, existing improved particle swarm algorithms often utilize linear inertia weight adjustment strategies to improve optimization performance. However, these algorithms lack dynamic adaptation mechanisms for planning in complex three-dimensional environments, making it difficult to balance global exploration with local exploitation. Consequently, key metrics such as fuel consumption and obstacle avoidance safety margins for the planned path remain suboptimal.

[0004] Current 3D path planning methods generally use a single-stage optimization framework, lacking a coordinated mechanism between path initialization and refined optimization. This results in the number of control points selected for the initial path directly affecting the final optimization results, while the subsequent optimization process struggles to correct structural defects in the initial path. Summary of the Invention

[0005] Purpose of the invention: The purpose of the present invention is to provide a three-dimensional path multi-stage planning method for low-altitude UAVs, which can realize dimensionality reduction, dynamic optimization strategy switching and a new type of path planning of multi-stage collaborative optimization, so as to improve the global optimality and planning efficiency of the three-dimensional path of low-altitude UAVs.

[0006] Technical solution: The present invention describes a three-dimensional path multi-stage planning method for low-altitude UAVs, comprising the following steps: inputting a three-dimensional terrain model and a starting point for path planning; initializing the three-dimensional path based on a cubic B-spline curve, and constructing an optimization model; performing active subspace identification and spatial mapping on the optimization variable space; executing an improved particle swarm algorithm within the active subspace, and periodically rewarding the particles with the best fitness in the population; performing multi-stage refinement on the optimized three-dimensional path, and performing pattern search algorithm optimization on the densified path control points; and obtaining the optimized global optimal three-dimensional path.

[0007] Furthermore, the three-dimensional terrain model uses the multi-Gaussian superposition technology, which is mathematically expressed as:

[0008] ;

[0009] in, Represents spatial coordinates ( x , y ), represents the center coordinate of the i-th peak, represents the height of the i-th peak, and Corresponding to the i-th peak x Axis and y The attenuation coefficient in the axial direction is used to control the terrain slope gradient. n represents the number of peaks.

[0010] Furthermore, the mathematical expression for initializing the three-dimensional path based on the cubic B-spline curve is:

[0011] ;

[0012] in, is the cubic B-spline basis function defined on the knot vector u, is the m control points of the three-dimensional space path curve, the parameterized interval .

[0013] Furthermore, the optimization variables of the optimization model are the coordinates of all control points, expressed as: ;

[0014] The fitness function is:

[0015] ;

[0016] in, is the main optimization target, which represents the length of the three-dimensional path, represents the collision penalty term, represents the curvature penalty term, represents the knot penalty term.

[0017] Furthermore, the collision penalty calculation formula is:

[0018] ;

[0019] in, Indicates the elevation value of the i-th collision detection point on the three-dimensional path, is the indicator function, when 1 when , otherwise 0;

[0020] The curvature penalty term is calculated as:

[0021] ;

[0022] in, represents the curvature of the i-th curvature detection point on the three-dimensional path, represents the maximum allowable curvature value, where the curvature is calculated by the following formula:

[0023] ;

[0024] Where, and Represents the three-dimensional space path curve The first and second derivatives with respect to the parameter u, To prevent extremely small positive numbers whose denominator is zero and thus cause calculation failure;

[0025] The calculation formula for the knot penalty term is:

[0026] .

[0027] Furthermore, the active subspace is identified by eigendecomposition of the gradient covariance matrix. The specific steps include:

[0028] Calculate the gradient covariance matrix C:

[0029] ;

[0030] in, is the mathematical expectation operator, is the fitness function with respect to the variable The gradient vector of , M is the total number of sampling samples, is the i-th sampling point.

[0031] Furthermore, the speed and position update formula of the improved particle swarm algorithm is:

[0032] ;

[0033] in, and represents the velocity vector of the i-th particle in the population at the k-th iteration and the k+1-th iteration, represents the individual historical optimal position of the i-th particle under the k-th iteration, represents the optimal position of the group at the kth iteration, and Indicates the k The position of the i-th particle in the next iteration, 、 The weight coefficient representing individual cognition and social group experience, represents the inertia weight coefficient of the kth iteration, which is updated by the following formula:

[0034] ;

[0035] Among them, K is the total number of iterations of the improved particle swarm optimization algorithm.

[0036] Furthermore, the adaptive boundary-aware strategy adjusts the mutation intensity according to the proximity of the particle to the boundary:

[0037] ;

[0038] in, represents the normalized distance from the jth dimension to the nearest boundary, Represents the variable value of the j-th dimension of the optimization variable, and Indicates the maximum and minimum allowed values ​​of the j-th dimension.

[0039] Furthermore, the mutation operation includes: adding Gaussian perturbation to the selected mutation dimension; if it exceeds the boundary, correcting it by using the mirror reflection method.

[0040] Furthermore, the periodic reward includes executing a pattern search algorithm on the particle with the best fitness every T iterations, and the steps include:

[0041] a Set the initial search grid and positive definite expansion set , where d represents the direction vector of the control point arrangement;

[0042] b. Calculate the fitness value of the current control point arrangement;

[0043] c. Search for a better solution along a random direction. If no solution is found, shrink the grid and repeat until the termination condition is met.

[0044] Multi-stage refinement includes:

[0045] e inserts a new node into the node vector, and the new node is the average value of the adjacent nodes;

[0046] f adjusts the control point vector and optimizes the densified path through pattern search;

[0047] gIf the fitness decrease value is less than the threshold, terminate the refinement, otherwise repeat step ef.

[0048] Beneficial effects: Compared with the existing technology, the present invention has the following significant advantages: three penalty terms are introduced in the design of the fitness function, so that the planned path has excellent geometric properties and practical applicability; on the basis of the traditional particle swarm algorithm, active subspace technology, adaptive boundary perception and mutation operation are combined, so that the algorithm still has excellent global optimality in the three-dimensional path planning scenario facing dimensionality explosion, and is not easy to fall into local optimal values; by periodically applying rewards to the best particles in the population, the population leading role of the best particles is enhanced, which is conducive to improving the diversity and vitality of the population, and thus improving the global optimality of the planned path; through a multi-stage refinement strategy, optimization starts from sparse control points and gradually densifies the control points, which not only enhances the expressive ability of the planned three-dimensional path and improves the global optimality, but also greatly improves the path planning efficiency of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 is a flow chart of the present invention;

[0050] Figure 2 It is a schematic diagram of the performance of different algorithms of the present invention under three-dimensional terrain;

[0051] Figure 3 Schematic diagram of the performance of the active subspace particle swarm algorithm of the present invention without adding adaptive boundary perception and mutation operation under three-dimensional terrain;

[0052] Figure 4 Schematic diagram of the performance of the active subspace particle swarm algorithm with added adaptive boundary perception and mutation operation under three-dimensional terrain;

[0053] Figure 5 It is the convergence curve of the fitness function of different algorithms of the present invention. DETAILED DESCRIPTION

[0054] The technical solution of the present invention will be further described below with reference to the accompanying drawings.

[0055] like Figure 1 As shown, an embodiment of the present invention provides a three-dimensional path multi-stage planning method for low-altitude UAVs, comprising the following steps:

[0056] (1) Input the 3D terrain model and the starting point of the path planning; the 3D terrain model uses the multi-Gaussian superposition technique, and the mathematical expression is:

[0057] ;

[0058] in, Represents spatial coordinates ( x , y ), represents the center coordinate of the i-th peak, represents the height of the i-th peak, and Corresponding to the i-th peak x Axis and y The attenuation coefficient in the axial direction is used to control the terrain slope gradient. n represents the number of peaks.

[0059] (2) Initialize the three-dimensional path based on the cubic B-spline curve and build an optimization model; the mathematical expression of the three-dimensional path initialized based on the cubic B-spline curve is:

[0060] ;

[0061] in, is the cubic B-spline basis function defined on the knot vector u, is the m control points of the three-dimensional space path curve, the parameterized interval .

[0062] Constructing an optimization model includes constructing optimization variables and fitness functions, where the optimization variables are set to all control points. x , y , z Coordinates, expressed as ;

[0063] In addition, the fitness function is an enhanced expression based on the penalty term, which comprehensively expresses the main optimization objective and the penalty term as follows:

[0064] ;

[0065] in, is the main optimization target, which represents the length of the three-dimensional path, represents the collision penalty term, represents the curvature penalty term, represents the knot penalty term.

[0066] The collision penalty term is used to prevent the path from intersecting the terrain surface and is expressed as:

[0067] ;

[0068] in, Indicates the elevation value of the i-th collision detection point on the three-dimensional path, is the indicator function, when 1 when , otherwise 0;

[0069] The curvature penalty term is used to suppress high-frequency geometric oscillations and ensure that the planned path meets the execution capability of the aircraft. It is expressed as:

[0070] ;

[0071] in, represents the curvature of the i-th curvature detection point on the three-dimensional path, represents the maximum allowable curvature value, where the curvature is calculated by the following formula:

[0072] ;

[0073] Where, and Represents the three-dimensional space path curve The first and second derivatives with respect to the parameter u, To prevent extremely small positive numbers whose denominator is zero and thus cause calculation failure;

[0074] The knot penalty term forces the path to maintain spatial monotonicity by penalizing reverse movement in all dimensions, thereby maintaining the logical progressive characteristics and alleviating optimization stagnation. and , if all coordinate components decrease simultaneously, the penalty is activated,

[0075] The calculation formula for the knot penalty term is:

[0076] .

[0077] (3) Active subspace identification and spatial mapping are performed on the optimization variable space; the active subspace is identified by eigendecomposition of the gradient covariance matrix, and the dimension with significant eigenvalues ​​in the eigenvalue matrix is ​​taken as the active subspace.

[0078] Gradient covariance matrix C The statistical expectation of the gradient outer product is quantified, and its diagonal elements represent the average square partial derivatives in each parameter direction. The present invention estimates it through the Latin hypercube sampling method, which is mathematically expressed as:

[0079] ;

[0080] in, is the mathematical expectation operator, is the fitness function with respect to the variable The gradient vector of , M is the total number of sampling samples, is the i-th sampling point.

[0081] Due to the symmetric semi-positive definiteness of C, its eigenvalue decomposition can be expressed as:

[0082] ;

[0083] Where W is the orthogonal eigenvector matrix, is the eigenvalue matrix in descending order, is the i-th eigenvalue, which quantifies the change intensity of the gradient covariance in the direction of the corresponding eigenvector, and p is the dimension of the total parameter space. The eigenvalue matrix is ​​truncated according to the significance of the eigenvalue: ,in, is a diagonal matrix consisting of the first r significant eigenvalues, and r is the dimension of the active subspace.

[0084] (4) Execute the improved particle swarm algorithm in the active subspace and periodically reward the particles with the best fitness in the population; the improved particle swarm algorithm refers to the particle swarm algorithm with adaptive boundary perception and mutation operations executed in the active subspace.

[0085] Specifically, in the active subspace, the speed and position update formulas of the particle swarm optimization algorithm are modified as follows:

[0086] ;

[0087] in, and represents the velocity vector of the i-th particle in the population at the k-th iteration and the k+1-th iteration, represents the individual historical optimal position of the i-th particle under the k-th iteration, represents the optimal position of the group at the kth iteration, and Indicates the k The position of the i-th particle in the next iteration, 、 The weight coefficient representing individual cognition and social group experience, represents the inertia weight coefficient of the kth iteration, which is updated by the following formula:

[0088] ;

[0089] Among them, K is the total number of iterations of the improved particle swarm optimization algorithm.

[0090] Adaptive boundary perception dynamically adjusts the exploration strategy based on the proximity of the particle to the constraint boundary. The proximity is expressed as follows:

[0091] ;

[0092] in, represents the normalized distance from the jth dimension to the nearest boundary, Represents the variable value of the j-th dimension of the optimization variable, and Indicates the maximum and minimum allowed values ​​of the j-th dimension.

[0093] The mutation operation includes mutation dimension selection, adding Gaussian perturbation, and mirror reflection correction.

[0094] Specifically, the selection of mutation dimension adopts random number ,like , then set it as the mutation dimension. For the mutation dimension, by generating Gaussian perturbations Update variables: If the position after mutation is out of bounds, use the mirror reflection method to correct it:

[0095] ;

[0096] Periodically rewarding the best fitness particle in the population means that every T iterations, the best fitness particle in the current population is additionally optimized by the pattern search algorithm. The pattern search algorithm steps include:

[0097] a. Obtain a preset initial search grid size and a positive definite expansion set, wherein the expression of the positive definite expansion set is: where d represents the direction vector of the control point arrangement;

[0098] b. Taking the control point arrangement of the optimized three-dimensional path as the current control point arrangement, and calculating the fitness value of the current control point arrangement;

[0099] c. Search the current control point arrangement in the direction of a random part of the positive definite expansion set, and calculate the fitness value of the control point arrangement after the search;

[0100] If the fitness value of the changed control point arrangement is greater than the fitness value of the current control point arrangement, then traverse all remaining directions in the positive definite expansion set. If a control point arrangement with a smaller fitness value is still not found, shrink the grid size and repeat c until the grid threshold or the maximum number of iterations is reached. The current control point arrangement is used as the secondary optimization three-dimensional path.

[0101] If the fitness value of the changed control point arrangement is less than the fitness value of the current control point arrangement, the searched control point arrangement is used as the current control point arrangement, and step c is repeated to obtain a secondary optimized three-dimensional path.

[0102] Node insertion refers to inserting a new node at the middle node sequence interval of the node vector U. The new node is the average value of the two adjacent nodes, and the new node vector is obtained. Specifically, when a node is inserted for the first time, the initial node vector of the three-dimensional path is , insert new nodes in sequence to make it , when inserting the node for the second time, it becomes:

[0103] ;

[0104] in: ;

[0105] The subsequent insertion steps are carried out according to the above rules;

[0106] Node vector based Adjust the control point vector to obtain a new control point vector;

[0107] Specifically, after inserting a new node, in order to keep the shape of the three-dimensional path represented by the B-spline curve unchanged, the control points in the control point vector need to be adjusted; for example, for a node vector and control point vectors The B-spline three-dimensional path is constructed, and a new node is added Inserted into the node vector U of the B-spline three-dimensional path, the new node vector and control point vectors for:

[0108] ;

[0109] Each term in the new control point vector is adjusted as follows:

[0110] ;

[0111] in, is the mth control point after the inserted control point, The first m control points, is any natural number, The values ​​are as follows:

[0112] ;

[0113] (5) The optimized three-dimensional path is refined in multiple stages, and the pattern search algorithm is optimized on the control points of the densified path to obtain the optimized global optimal three-dimensional path.

[0114] The path is gradually refined by combining progressive node insertion with local optimization, which includes the following steps:

[0115] e. Obtain a three-dimensional path consisting of sparse control point vectors and node vectors;

[0116] f. Insert nodes into the three-dimensional path;

[0117] g. Adjust the control point vector based on the densified node vector to obtain a new control point vector, and perform pattern search algorithm optimization;

[0118] h. Check the decrease in the optimal fitness value after densification compared to the value before densification. If it is less than the set threshold, the global optimal three-dimensional path is obtained. Otherwise, return to step f.

[0119] Table 1 Parameter settings of different optimization methods

[0120] ;

[0121] from Figure 2 It can be seen that the method provided by the present invention obtains a better three-dimensional path in complex terrain scenarios than other algorithms. The fitness function value is only 169.4, and no penalty constraints are violated. The parameter settings of different algorithms are shown in Table 1. Figure 5 The changes in the fitness function values ​​of all the tested methods for path planning in the same three-dimensional scene are shown. It can be clearly seen that the method provided by the present invention can more quickly find a three-dimensional path that does not violate the penalty constraints because it uses a sparse control point vector for optimization in the initial stage. Other algorithms are very likely to fall into local optimal values ​​when faced with complex three-dimensional scene planning, and may even fail to meet the set penalty constraints. Figure 3 and Figure 4 The performance of the active subspace particle swarm algorithm on three-dimensional terrain is demonstrated, respectively, without adaptive boundary awareness and mutation operations, and with adaptive boundary awareness and mutation operations. It can be clearly seen that when adaptive boundary awareness and mutation operations are not added, a large number of particles in the swarm are already at the boundaries of the three-dimensional space and cannot escape after 10 iterations. This causes the swarm vitality to decline rapidly and fails to obtain the globally optimal three-dimensional path. In contrast, with adaptive boundary awareness and mutation operations, the swarm maintains a certain level of population diversity in the early iterations and gradually converges to the globally optimal region in the later iterations, making the planned three-dimensional path highly globally optimal.

Claims

1. A three-dimensional path multi-stage planning method for low-altitude UAVs, characterized by: The following steps are involved: Input the 3D terrain model and the starting point of the path planning; Initialize the three-dimensional path based on the cubic B-spline curve and build an optimization model; Active subspace identification and spatial mapping are performed on the optimization variable space; the improved particle swarm algorithm is executed in the active subspace, and the particles with the best fitness in the population are periodically rewarded; the optimized three-dimensional path is refined in multiple stages, and the pattern search algorithm is optimized on the control points of the densified path; the optimized global optimal three-dimensional path is obtained. The optimization variables of the optimization model are the coordinates of all control points, which can be expressed as: ; The fitness function is: ； in, is the main optimization target, which represents the length of the three-dimensional path, represents the collision penalty term, represents the curvature penalty term, represents the knot penalty term; The collision penalty calculation formula is: ； in, Indicates the elevation value of the i-th collision detection point on the three-dimensional path, is the indicator function, when 1 when , otherwise 0; The curvature penalty term is calculated as: ； in, represents the curvature of the i-th curvature detection point on the three-dimensional path, represents the maximum allowable curvature value, where the curvature is calculated by the following formula: ； Where, and Represents the three-dimensional space path curve The first and second derivatives with respect to the parameter u, To prevent extremely small positive numbers whose denominator is zero and thus cause calculation failure; The calculation formula for the knot penalty term is: ； The active subspace is identified by eigendecomposition of the gradient covariance matrix. The specific steps include: Calculate the gradient covariance matrix C: ； in, is the mathematical expectation operator, is the fitness function with respect to the variable The gradient vector of , M is the total number of sampling samples, is the i-th sampling point; The speed and position update formula of the improved particle swarm algorithm is: ； in, and represents the velocity vector of the i-th particle in the population at the k-th iteration and the k+1-th iteration, represents the individual historical optimal position of the i-th particle under the k-th iteration, represents the optimal position of the group at the kth iteration, and Indicates the k The position of the i-th particle in the next iteration, 、 The weight coefficient representing individual cognition and social group experience, represents the inertia weight coefficient of the kth iteration, which is updated by the following formula: ； Among them, K is the total number of iterations of the improved particle swarm optimization algorithm.

2. A three-dimensional path multi-stage planning method for low-altitude UAVs according to claim 1, characterized in that: The three-dimensional terrain model uses multi-Gaussian superposition technology, which is mathematically expressed as: ； in, Represents spatial coordinates ( x , y ), represents the center coordinate of the i-th peak, represents the height of the i-th peak, and Corresponding to the i-th peak x Axis and y The attenuation coefficient in the axial direction is used to control the terrain slope gradient. n represents the number of peaks.

3. The multi-stage three-dimensional path planning method for low-altitude UAVs according to claim 1 is characterized in that: The mathematical expression for initializing a three-dimensional path based on a cubic B-spline curve is: ； in, is the cubic B-spline basis function defined on the knot vector u, is the m control points of the three-dimensional space path curve, the parameterized interval .

4. The method for multi-stage three-dimensional path planning for low-altitude UAVs according to claim 1, characterized in that: The adaptive boundary-aware strategy adjusts the mutation intensity based on the proximity of the particle to the boundary: ； in, represents the normalized distance from the jth dimension to the nearest boundary, Represents the variable value of the j-th dimension of the optimization variable, and Indicates the maximum and minimum allowed values ​​of the j-th dimension.

5. The method for multi-stage three-dimensional path planning for low-altitude UAVs according to claim 1, characterized in that: The mutation operation includes: adding Gaussian perturbation to the selected mutation dimension; if it exceeds the boundary, it is corrected by using the mirror reflection method.

6. The method for multi-stage three-dimensional path planning for low-altitude UAVs according to claim 1, characterized in that: The periodic reward involves executing a pattern search algorithm on the particle with the best fitness every T iterations. The steps include: a. Setting the initial search grid and the positive definite expansion set , where d represents the direction vector of the control point arrangement; b calculates the fitness value of the current control point arrangement; c searches for a better solution along a random direction. If no better solution is found, the grid is contracted, and the process is repeated until the termination condition is met; the multi-stage refinement includes: e inserting a new node into the node vector, where the new node is the average value of the adjacent nodes; f adjusting the control point vector and optimizing the densified path through pattern search; g if the fitness drop value is less than the threshold, the refinement is terminated, otherwise steps ef are repeated.

Images