Three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicle

Abstract

The invention discloses a three-dimensional path multi-stage planning method for a low-altitude unmanned aerial vehicle. The method comprises the following steps: inputting a three-dimensional terrain model and a starting point of path planning; initializing a three-dimensional path based on a cubic B-spline curve, and constructing an optimization model; performing active subspace identification and space mapping on the optimization variable space; executing an improved particle swarm algorithm in the active subspace, and periodically rewarding optimal fitness particles in the population; performing multi-stage refinement on the optimized three-dimensional path, and executing mode search algorithm optimization on the encrypted path control points; and obtaining an 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 particularly to a multi-stage three-dimensional path planning method for low-altitude UAVs. Background Art

[0002] With the wide application of low-altitude UAVs in fields such as logistics transportation, emergency rescue, and geographical mapping, the path planning technology in complex three-dimensional environments has become a key technical bottleneck restricting their intelligent development. Traditional path planning methods usually use algorithms such as A*, Rapidly-Exploring Random Trees (RRT) and their improved algorithms for three-dimensional path search. However, when facing complex low-altitude environments such as urban canyons and mountainous undulations, there are problems such as poor path smoothness and slow convergence speed. In recent years, swarm intelligence optimization methods based on particle swarm optimization algorithms, genetic algorithms, etc. have gradually been applied to the field of UAV path planning. However, they are prone to falling into local optimal solutions during the optimization process in high-dimensional solution spaces and have insufficient adaptability to three-dimensional terrain constraints.

[0003] In the prior art, although the path modeling method based on B-spline curves can ensure path continuity, it faces the problem of dimensional explosion during the optimization of control points, resulting in a significant decrease in computational efficiency. In addition, most of the existing improved particle swarm algorithms improve the optimization performance through the linear adjustment strategy of inertia weight, but lack a dynamic adaptation mechanism in complex three-dimensional environment planning, making it difficult to balance the relationship between global exploration and local exploitation, resulting in key indicators such as fuel consumption and obstacle avoidance safety margin of the planned path not being optimal.

[0004] Currently, the general three-dimensional path planning methods adopt a single-stage optimization framework and lack a coordination mechanism in the path initialization and fine optimization links. This leads to the selection of the number of control points of the initial path directly affecting the final optimization effect, and it is difficult to correct the structural defects of the previous path during the later optimization process. Summary of the Invention

[0005] Object of the Invention: The object of the present invention is to provide a multi-stage three-dimensional path planning method for low-altitude UAVs, which can realize a new type of path planning with dimension reduction, dynamic optimization strategy switching, and 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: A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to the present invention includes the following steps: input a three-dimensional terrain model and a starting point for path planning; initialize a three-dimensional path based on a cubic B-spline curve and construct an optimization model; perform active subspace identification and space mapping on the optimization variable space; execute an improved particle swarm algorithm in the active subspace and periodically reward the optimal fitness particles in the population; perform multi-stage refinement on the optimized three-dimensional path and execute a pattern search algorithm to optimize the path control points after densification; obtain the optimized global optimal three-dimensional path.

[0007] Further, the three-dimensional terrain model adopts a multi-Gaussian superposition technique, and the mathematical expression is: ; Wherein, represents the elevation value at the spatial coordinate ( x , y ), represents the central coordinate of the i-th peak, represents the height of the i-th mountain peak, and correspond to the attenuation coefficients in the x axis and y axis directions of the i-th mountain peak respectively, and are used to regulate the terrain slope gradient, and n represents the number of mountain peaks.

[0008] Further, the mathematical expression for initializing the three-dimensional path based on the cubic B-spline curve is: ; Wherein, 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, and the parameterization interval is .

[0009] Further, the optimization variables of the optimization model are the coordinates of all control points, which are expressed as: ; The fitness function is: ; Wherein, is the main optimization objective, representing the length of the three-dimensional path, represents the collision penalty term, represents the curvature penalty term, represents the knot penalty term.

[0010] Further, the calculation formula for the collision penalty term is: ; Wherein, represents the elevation value of the i-th collision detection point on the three-dimensional path, is an indicator function that takes 1 when and 0 otherwise; The calculation formula for the curvature penalty term is: ; where, represents the curvature of the i-th curvature detection point on the three-dimensional path, represents the maximum allowable value of the curvature, where the curvature is calculated by the following formula: ; In the formula, and respectively represent the first-order derivative and the second-order derivative of the three-dimensional space path curve with respect to the parameter u, is a very small positive number to prevent the denominator from being zero and causing calculation failure; The calculation formula for the knot penalty term is: .

[0011] Furthermore, the active subspace is identified through the eigenvalue decomposition of the gradient covariance matrix. The specific steps include: Calculate the gradient covariance matrix C: ; where, is the mathematical expectation operator, is the gradient vector of the fitness function with respect to the variable , M is the total number of sampling samples, is the i-th sampling point.

[0012] Furthermore, the velocity and position update formulas of the improved particle swarm algorithm are: ; where, and represent the velocity vectors 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 at the k-th iteration, represents the global optimal position at the k-th iteration, and represent the position of the i-th particle at the k -th iteration, , represent the weight coefficients of individual cognition and social group experience, represents the inertia weight coefficient at the k-th iteration, which is updated by the following formula: ; Among them, K is the total number of iterations of the improved particle swarm optimization algorithm.

[0013] Furthermore, the adaptive boundary awareness strategy adjusts the mutation intensity according to the proximity of the particle to the boundary: ; Among them, represents the normalized distance from the j-th dimension to the nearest boundary, represents the variable value of the j-th dimension of the optimization variable, and represent the maximum allowable value and the minimum allowable value of the j-th dimension.

[0014] Furthermore, the mutation operation includes: adding Gaussian perturbation to the selected mutation dimension; if it goes out of bounds, it is corrected using the mirror reflection method.

[0015] Furthermore, the periodic reward includes performing a pattern search algorithm on the optimal fitness particle every T iterations, and the steps include: a Set the initial search grid and the positive definite expansion set , where d represents the direction vector of the control point arrangement; b Calculate the fitness value of the current control point arrangement; c Search for a better solution in a random direction. If not found, shrink the grid and repeat until the termination condition is met; The multi-stage refinement includes: e Insert a new node into the node vector, and the new node is the average of adjacent nodes; f Adjust the control point vector and optimize the densified path through pattern search; g If the fitness decrease value is less than the threshold, terminate the refinement, otherwise repeat steps e - f.

[0016] Beneficial effects: Compared with the prior art, the present invention has the following remarkable advantages: Three penalty terms are introduced in the design of the fitness function, making the planned path have excellent geometric properties and practical applicability; The active subspace technology, adaptive boundary awareness, and mutation operation are combined on the basis of the traditional particle swarm optimization algorithm, making the algorithm still have excellent global optimality in the three-dimensional path planning scenario facing the curse of dimensionality and not easily falling into local optimal values; By periodically rewarding the optimal particles in the population, the leading role of the optimal particle population is enhanced, which is beneficial to improving the diversity and vitality of the population, and further improving the global optimality of the planned path; Through a multi-stage refinement strategy, starting from sparse control points for optimization and gradually densifying the control points, not only enhances the expression ability of the planned three-dimensional path and improves the global optimality, but also greatly improves the path planning efficiency of the algorithm. Description of the Drawings

[0017] Figure 1 is the flow chart of the present invention; Figure 2 is the schematic diagram of the performance of different algorithms of the present invention under three-dimensional terrain; Figure 3 is the schematic diagram of the performance of the active subspace particle swarm optimization algorithm without adding adaptive boundary awareness and mutation operation of the present invention under three-dimensional terrain; Figure 4 is the schematic diagram of the performance of the active subspace particle swarm optimization algorithm with adding adaptive boundary awareness and mutation operation of the present invention under three-dimensional terrain; Figure 5 is the convergence curve of the fitness function of different algorithms of the present invention. Specific embodiments

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

[0019] As Figure 1 shown, an embodiment of the present invention provides a three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles, including the following steps: (1) Input the three-dimensional terrain model and the starting point of path planning; the three-dimensional terrain model adopts the multi-Gaussian superposition technology, and the mathematical expression is: ; Among them, represents the elevation value at the spatial coordinate ( x , y ), represents the central coordinate of the i-th peak, represents the height of the i-th mountain peak, and respectively correspond to the attenuation coefficients in the x axis and y axis directions of the i-th mountain peak, which are used to regulate the terrain slope gradient, and n represents the number of mountain peaks.

[0020] (2) Initialize the three-dimensional path based on the cubic B-spline curve and construct an optimization model; the mathematical expression for initializing the three-dimensional path based on the cubic B-spline curve is: ; Among them, is the cubic B-spline basis function defined on the knot vector u, are the m control points of the three-dimensional space path curve, and the parameterization interval is .

[0021] Constructing the optimization model includes constructing optimization variables and a fitness function, where the optimization variables are set as all the x , y , zCoordinates, expressed as ; In addition, the fitness function is an enhanced expression based on penalty terms, and the main optimization objective and penalty terms are comprehensively expressed as: ; Among them, is the main optimization objective, representing the length of the three-dimensional path, represents the collision penalty term, represents the curvature penalty term, represents the knot penalty term.

[0022] The collision penalty term is used to prevent the path from intersecting the terrain surface and is expressed as: ; Among them, represents the elevation value of the i-th collision detection point on the three-dimensional path, is the indicator function, which takes 1 when and 0 otherwise; The curvature penalty term is used to suppress high-frequency geometric oscillations and ensure that the planned path conforms to the execution ability of the aircraft and is expressed as: ; Among them, represents the curvature of the i-th curvature detection point on the three-dimensional path, represents the maximum allowable value of curvature, where the curvature is calculated by the following formula: ; In the formula, and respectively represent the first derivative and the second derivative of the three-dimensional space path curve with respect to the parameter u, is a very small positive number to prevent the denominator from being zero and causing calculation failure; The knot penalty term forces the path to maintain spatial monotonicity by penalizing full-dimensional reverse movement, thereby maintaining the logical progression characteristic and alleviating optimization stagnation. For consecutive control points and , if all coordinate components decrease simultaneously, the penalty is activated, The calculation formula of the knot penalty term is: .

[0023] (3) Perform active subspace identification and space mapping on the optimization variable space; among them, the active subspace is identified through the eigenvalue decomposition of the gradient covariance matrix, and the dimensions with significant eigenvalues in the eigenvalue matrix are used as the active subspace.

[0024] Gradient covariance matrix CQuantifies the statistical expectation of the outer product of gradients, whose diagonal elements represent the average squared partial derivatives in the directions of each parameter. The present invention estimates it through the Latin hypercube sampling method, and the mathematical expression is: ; Among them, is the mathematical expectation operator, is the gradient vector of the fitness function with respect to the variable , M is the total number of sampling samples, is the i-th sampling point.

[0025] Due to the symmetric positive semi-definiteness of C, its eigenvalue decomposition can be expressed as: ; Among them, W is the orthogonal eigenvector matrix, is the eigenvalue matrix arranged in descending order, is the i-th eigenvalue, quantifying 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 degree of the eigenvalues: , among which, is the diagonal matrix composed of the first r significant eigenvalues, and r is the dimension of the active subspace.

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

[0027] Specifically, in the active subspace, the velocity and position update formulas of the particle swarm algorithm are modified as: ; Among them, and represent the velocity vectors 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 at the k-th iteration, represents the global optimal position at the k-th iteration, and represent the position of the i-th particle at the k -th iteration, , represent the weight coefficients of individual cognition and social group experience, represents the inertia weight coefficient at the k-th iteration, which is updated through the following formula: ; Among them, K is the total number of iterations of the improved particle swarm algorithm.

[0028] Adaptive boundary perception dynamically adjusts the exploration strategy according to the proximity of particles to the constraint boundary, and the proximity is represented by the following formula: ; where, represents the normalized distance from the j-th dimension to the nearest boundary, represents the variable value of the j-th dimension of the optimization variable, and represent the maximum allowable value and the minimum allowable value of the j-th dimension.

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

[0030] Specifically, the selection of the mutation dimension uses a random number , if , then it is set as the mutation dimension. For the mutation dimension, the variable is updated by generating Gaussian perturbation : . If the position after mutation exceeds the boundary, the mirror reflection method is used for correction: ; Periodic reward The optimal fitness particle in the population refers to performing the pattern search algorithm optimization on the optimal fitness particle in the current population every T iteration times. The steps of the pattern search algorithm include: a. Obtain the preset initial search grid size and positive definite expansion set. The expression of the positive definite expansion set is, where d represents the direction vector of the control point arrangement; b. Take the control point arrangement of optimizing the three-dimensional path once as the current control point arrangement, and calculate the fitness value of the current control point arrangement; c. Search in the random part of the directions in the positive definite expansion set for the current control point arrangement, and calculate the fitness value of the control point arrangement after the search; 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 the remaining directions in the positive definite expansion set. If no control point arrangement with a smaller fitness value is found, then shrink the grid size, and repeat c until the grid threshold or the maximum number of iterations is reached, and take the current control point arrangement as the secondary optimized three-dimensional path; If the fitness value of the changed control point arrangement is less than the fitness value of the current control point arrangement, then take the control point arrangement after the search as the current control point arrangement, and repeat c to obtain the secondary optimized three-dimensional path.

[0031] Node insertion means inserting new nodes at intervals in the middle node sequence of the node vector U. The new nodes are the average values of two adjacent nodes, and the new node vector is obtained Specifically, when inserting nodes for the first time, the initial node vector of the three-dimensional path is , insert new nodes in sequence to make it become , when inserting nodes for the second time, make it become: ; Where: ; Subsequent insertion steps are carried out according to the above rules; Based on the node vector Adjust the control point vector to obtain a new control point vector; Specifically, after inserting new nodes, in order to keep the shape of the three-dimensional path represented by the B-spline curve unchanged, it is necessary to adjust the control points in the control point vector; for example, for a node vector and a control point vector constituting a B-spline three-dimensional path, insert a new node into the node vector U of the B-spline three-dimensional path, then the obtained new node vector and the control point vector are: ; Each item in the new control point vector is adjusted according to the following formula: ; Where, is the m-th control point after inserting the control point, is the m -th control point before inserting the control point, is any natural number, takes values as follows: ; (5) Refine the optimized three-dimensional path in multiple stages, and perform pattern search algorithm optimization on the control points of the densified path; obtain the optimized globally optimal three-dimensional path.

[0032] Realize the gradual refinement of the path through the combination of progressive node insertion and local optimization, which specifically includes the following steps: e. Obtain a three-dimensional path composed of a sparse control point vector and a node vector; f. Insert nodes into the three-dimensional path; g. Based on the densified node vector, adjust the control point vector to obtain a new control point vector, and perform pattern search algorithm optimization; h. Check the decrease value of the optimal fitness value after densification compared with that before densification. If it is less than the set threshold, then obtain the globally optimal three-dimensional path, otherwise return to step f; Table 1 Parameter settings of different optimization methods ; It can be seen from Figure 2 that the method provided by the present invention obtains a better three-dimensional path than other algorithms in complex terrain scenarios. The fitness function value is only 169.4, and no penalty constraints set are violated. The parameter settings of different algorithms are shown in Table 1. Figure 5 shows the change of the fitness function values of all test methods for path planning in the same three-dimensional scenario. It can be clearly seen that since the method provided by the present invention uses a sparse control point vector for optimization in the initial stage, it can find a three-dimensional path that does not violate the penalty constraints more quickly. Other algorithms are prone to falling into local optima or even unable to meet the set penalty constraints when facing complex three-dimensional scenario planning. Figure 3 and Figure 4 show the performance of the active subspace particle swarm algorithm without adding adaptive boundary perception and mutation operations and the active subspace particle swarm algorithm with adding adaptive boundary perception and mutation operations in three-dimensional terrain respectively. It can be clearly seen that when not adding adaptive boundary perception and mutation operations, a large number of particles in the population are already at the boundary of the three-dimensional space and cannot escape when the number of iterations is 10, which causes the vitality of the population to decline rapidly and cannot obtain the globally optimal three-dimensional path. On the contrary, after adding adaptive boundary perception and mutation operations, the population maintains a certain population diversity in the early stage of iteration and gradually gathers in the globally optimal region in the later stage of iteration, making the planned three-dimensional path highly globally optimal.

Claims

1. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles, characterized in that It includes the following steps: Input the three-dimensional terrain model and the starting point of path planning; Initialize the three-dimensional path based on the cubic B-spline curve and construct an optimization model; Perform active subspace identification and space mapping on the optimization variable space; execute the improved particle swarm optimization algorithm within the active subspace and periodically reward the optimal fitness particles in the population; Refine the optimized three-dimensional path in multiple stages and optimize the path control points after densification by using the pattern search algorithm; obtain the optimized global optimal three-dimensional path.

2. The three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 1, wherein The three-dimensional terrain model adopts the multi-Gaussian superposition technology, and the mathematical representation is: ； Among them, represents the elevation value at the spatial coordinate ( x , y ), represents the central coordinate of the i-th peak top, represents the height of the i-th mountain peak, and respectively correspond to the attenuation coefficients in the x axis direction and the y axis direction of the i-th mountain peak, and are used to regulate the terrain slope gradient. n represents the number of mountain peaks.

3. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 1, characterized in that, The mathematical expression for initializing the three-dimensional path based on the cubic B-spline curve is: ； Among them, is the cubic B-spline basis function defined on the knot vector u, are the m control points of the three-dimensional space path curve, and the parameterization interval is .

4. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 1, characterized in that The optimization variables for the optimized model are the coordinates of all control points, expressed as: ; The fitness function is: ； Among them, is the main optimization objective, representing the length of the three-dimensional path, represents the collision penalty term, represents the curvature penalty term, represents the knot penalty term.

5. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 4, characterized in that The calculation formula for the collision penalty term is: ； Among them, represents the elevation value of the i-th collision detection point on the three-dimensional path, is an indicator function, which takes 1 when and 0 otherwise; The calculation formula for the curvature penalty term is: ； Among them, represents the curvature of the i-th curvature detection point on the three-dimensional path, represents the maximum allowable value of the curvature, where the curvature is calculated by the following formula: ； In the formula, and respectively represent the first derivative and the second derivative of the three-dimensional space path curve with respect to the parameter u, is a very small positive number to prevent the denominator from being zero and causing the calculation to fail; The calculation formula for the knot penalty term is: 。 6. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 1, characterized in that, The active subspace is identified through the eigenvalue decomposition of the gradient covariance matrix, and the specific steps include: Calculate the gradient covariance matrix C: ； Among them, is the mathematical expectation operator, is the gradient vector of the fitness function with respect to the variable , M is the total number of sampling samples, is the i-th sampling point.

7. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 1, characterized in that The update formulas for the velocity and position of the improved particle swarm optimization algorithm are: ； Among them, and represent the velocity vectors of the $i$-th particle in the $k$-th iteration and the $(k + 1)$-th iteration in the population, represents the individual historical best position of the $i$-th particle in the $k$-th iteration, represents the global best position in the $k$-th iteration, and represent the k position of the $i$-th particle in the $k$-th iteration, and represent the weight coefficients of individual cognition and social group experience, represents the inertia weight coefficient in the $k$-th iteration, which is updated by the following formula: ； Among them, K is the total number of iterations of the improved particle swarm optimization algorithm.

8. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 7, characterized in that, The adaptive boundary perception strategy adjusts the mutation intensity according to the proximity of the particle to the boundary: ； wherein, represents the normalized distance from the j-th dimension to the nearest boundary, represents the variable value of the j-th dimension of the optimization variable, and represent the maximum allowable value and the minimum allowable value of the j-th dimension.

9. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 1, characterized in that The mutation operation includes: adding Gaussian perturbation to the selected mutation dimension; if it goes out of bounds, the mirror reflection method is used for correction.

10. A three-dimensional path multi-stage planning method for low-altitude unmanned aerial vehicles according to claim 1, characterized in that The periodic reward includes performing a pattern search algorithm on the optimal fitness particle every T iterations. The steps include: a. Setting an initial search grid and a positive definite expansion set , where d represents the direction vector of the control point arrangement; b. Calculating the fitness value of the current control point arrangement; c. Searching for a better solution in a random direction. If no better solution is found, the grid is shrunk 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 of adjacent nodes; f. Adjusting the control point vector and optimizing the densified path through pattern search; g. If the fitness decrease value is less than the threshold, the refinement is terminated; otherwise, steps e-f are repeated.

Images