Unmanned aerial vehicle path planning method and system

Abstract

The embodiment of the invention discloses an unmanned aerial vehicle path planning method and system. The method comprises the following steps: S1, constructing an environment model of unmanned aerial vehicle operation; s2, setting an objective function and constraint conditions of unmanned aerial vehicle path planning; s3, based on the environment model, the objective function and the constraint condition, determining information of an approximate optimal path by adopting an ant colony algorithm; and S4, applying the information of the approximate optimal path to a sequential quadratic programming algorithm to determine the optimal path. According to the unmanned aerial vehicle path planning method provided by the invention, on the basis of the ant colony algorithm, the SQP algorithm is fused for secondary optimization, so that the problems that the traditional ant colony algorithm is easy to fall into local optimum and slow in convergence speed are solved, the path length of unmanned aerial vehicle path planning is successfully shortened, the number of iterations is reduced, and the path planning efficiency is improved. According to the method, more efficient and more accurate path optimization is realized, and the optimized algorithm has relatively high adaptability and robustness.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicles, and particularly relates to a method and system for unmanned aerial vehicle path planning. BACKGROUND

[0002] Unmanned aerial vehicles are considered as the core driving force for promoting the development of low-altitude economy due to their advantages of convenience, efficiency, fast response and freedom from ground transportation. However, in a dense urban environment, high-rise buildings, scattered vegetation, restricted flight areas such as airport clearance areas and large event venues, as well as dynamic obstacles such as birds and other aircraft increase the difficulty of unmanned aerial vehicle path planning. Efficient and reasonable unmanned aerial vehicle path planning needs to effectively avoid these obstacles, reduce collision risks, ensure the safety and stability of unmanned aerial vehicle flight, reduce potential harm to urban infrastructure and personnel, and build a strong safety line for the sustainable development of low-altitude economy. In the low-altitude economy mode, efficient unmanned aerial vehicle path planning needs to make full use of urban low-altitude space, improve efficiency, reduce operating costs and improve economic benefits.

[0003] Among common unmanned aerial vehicle path planning algorithms, the A* algorithm has high search efficiency but is not suitable for dynamic environments; the particle swarm optimization algorithm has an advantage in convergence speed but cannot handle complex scenarios and multi-objective planning well; the genetic algorithm can handle complex and large-scale optimization problems but has a slow search process and convergence speed. Compared with other path planning algorithms, the ant colony algorithm is favored for its efficient global search capability, positive feedback mechanism, excellent robustness and simple implementation. However, in practical applications, the ant colony algorithm also faces some challenges, such as being easily trapped in local optima and having a slow convergence speed, which can lead to poor flight path quality of unmanned aerial vehicles and prolonged planning periods. SUMMARY

[0004] Therefore, it is necessary to provide a method and system for unmanned aerial vehicle path planning that can solve the problems of traditional ant colony algorithms being easily trapped in local optima and having a slow convergence speed.

[0005] A method for unmanned aerial vehicle path planning, comprising the following steps:

[0006] S1. Constructing an environment model for unmanned aerial vehicle operation;

[0007] S2. Setting a target function and constraint conditions for unmanned aerial vehicle path planning;

[0008] S3. Based on the environment model, the target function and the constraint conditions, using an ant colony algorithm to determine information of an approximately optimal path;

[0009] S4. Applying the information of the approximately optimal path to a sequential quadratic programming algorithm to determine an optimal path.

[0010] In one embodiment, the environment model comprises a static obstacle model, a no-fly zone environment model and a sudden obstacle model;

[0011] The static obstacle model involves the following formula:

[0012]

[0013] wherein (x, y) represents the planar position of any point in the static obstacle model at this moment; (x i ,y i ) represents the center coordinates of the static obstacle model; h i represents the height of any point in the actual static obstacle; z(x, y) represents the elevation value of the corresponding point in the static obstacle model; R represents the footprint radius of the actual static obstacle; and r represents the distance of any point in the static obstacle model from the center of the static obstacle model at this moment;

[0014] The no-fly zone environment model involves the following formula:

[0015]

[0016] wherein (x, y) represents the planar position of any point in the no-fly zone environment model at this moment; (x i ,y i ) represents the center coordinates of the ith no-fly zone; z(x, y) represents the vertical height corresponding to the point (x, y); R represents the simulation radius of the no-fly zone; and r represents the distance of the point (x, y) from the center of the no-fly zone model at this moment;

[0017] The sudden obstacle model involves the following formula:

[0018] (x-x0) 2 +(y-y0) 2 +(z-z0) 2 =r 2

[0019] wherein (x, y, z) represents the three-dimensional coordinates of any point in the sudden obstacle model at this moment; (x0, y0, z0) represents the center coordinates of the sudden obstacle model; and r represents the simulation radius of the sudden obstacle.

[0020] In one embodiment, the objective function comprises a minimum length index function, and the constraint condition comprises a minimum path length constraint and a UAV collision constraint;

[0021] The minimum length index function involves the following formula:

[0022]

[0023]

[0024] wherein F(x) represents a minimum length index function; n represents a total number of path points in a planned path of the UAV; x i represents an i-th point on the path; (a i ,b i ,z i ) represents coordinate values of x i ; d(x i ,x i+1 ) represents a Euclidean distance from x i to x i+1 ;

[0025] The minimum path length constraint involves a formula as follows:

[0026]

[0027] wherein f i represents each path distance; f min represents a minimum path length;

[0028] The UAV collision constraint involves a formula as follows:

[0029]

[0030] wherein a space obstacle model O i is a sphere with a radius of R ; a center coordinate of the space obstacle model is (O xi ,O yi ,O zi ); (x, y, z) represents a mass center coordinate of the UAV in a three-dimensional space; L represents a distance between the UAV and the space obstacle; and R represents a safety radius of the UAV.

[0031] In one of the embodiments, the step S3 comprises:

[0032] S31. initializing ant colony parameters and pheromone concentrations carried by all ants;

[0033] S32. assigning all ants to initial nodes;

[0034] S33. letting all ants start from a starting point, selecting paths according to pheromone concentrations and heuristic information, and gradually constructing a route to a target point;

[0035] S34. updating pheromone concentrations according to path quality;

[0036] S35. Determine whether the maximum iteration number is reached; if yes, output the information of the approximate optimal path; if no, return to step S32, and then execute steps S32-S35 in turn at least once until the maximum iteration number is reached.

[0037] In one of the embodiments, step S32 comprises:

[0038] In the variable range [l i ,μ i ], the initial node of each ant is randomly allocated, and the specific formula is as follows:

[0039]

[0040] wherein τ(k) represents the pheromone concentration of the kth ant at the initial node; F(x) represents the minimum length index function; x represents the initial path whose length is to be calculated; x k represents the random path generated by the kth ant.

[0041] In one of the embodiments, in step S33, all ants select the next node transition according to the state transition probability, and the specific formula is as follows:

[0042]

[0043] wherein represents the probability of the kth ant passing through nodes m and n at time t; τ mn (t) represents the pheromone concentration between nodes m and n; η mn (t) represents the heuristic function; D mn represents the Euclidean distance between nodes m and n; α is the pheromone importance factor; β is the heuristic function importance factor; next k represents the set of next nodes to be selected by the ant;

[0044] In step S34, the pheromone concentration is updated according to the following formula:

[0045] τ(k) = (1-ρ)τ(k) +△τ(k)

[0046]

[0047] wherein △τ(k) represents the increment of the pheromone concentration on the path after the kth iteration; a and b are algorithm parameters, which need to be preset; τ(k) represents the pheromone concentration of the kth ant at the initial node; ρ is the pheromone evaporation coefficient.

[0048] In one of the embodiments, step S4 comprises:

[0049] S41. Construct a Lagrange function to convert the nonlinear optimization problem including the original objective function and the constraint conditions into a quadratic programming sub-problem, the Lagrange function is as follows:

[0050] L(x, λ, μ) = f(x) + ∑λg(x) + ∑μh(x)

[0051] Wherein, x refers to the decision variable, representing the coordinates of the path point; λ is the Lagrange multiplier, corresponding to the inequality constraint function; μ is the Lagrange multiplier, corresponding to the equality constraint function; f(x) is the objective function; g(x) is the inequality constraint function; h(x) is the equality constraint function;

[0052] The quadratic programming sub-problem is as follows:

[0053]

[0054] Wherein, d is the search direction vector, that is, the direction of the iteration step; x k+1 = x k + αd, α is the step size; H k represents the Hessian matrix, specifically the second derivative matrix of the objective function at x k ; is the gradient of the objective function at x k ;

[0055] S42. Iteratively solve the quadratic programming sub-problem.

[0056] In one embodiment, the step S42 comprises:

[0057] S421. Update the Hessian matrix using the BFGS algorithm, involving the formula as follows:

[0058]

[0059] {s k} = x {k+1} - x k

[0060]

[0061] Wherein, H k is the approximation value of the Hessian matrix at the kth iteration; {y k} represents the gradient difference of the objective function f(x) at the adjacent two iteration points x k and x k+1 ; {s k} represents the position difference of the adjacent two iteration points; p(k) represents the search direction; x kx(k) refers to the value of the independent variable x of the objective function f(x) at the kth iteration; f(x k ) is the function value of the objective function f(x) at x k ;

[0062] S422. In the search direction determined by the quasi-Newton method, a suitable step size is found by using a line search method, the independent variable is updated, and it is determined whether the convergence condition is met. If yes, the information of the optimal path is output; if no, it is returned to step S421, and then steps S421-S422 are sequentially executed at least once until the convergence condition is met.

[0063] In one embodiment, step S422 includes:

[0064] In the search direction determined by the quasi-Newton method, a suitable step size a is found by using a line search, the variable x k+1 is updated to x k + a * p(k), p(k) is the search direction, which ensures the objective function to be reduced, and it is checked whether the convergence condition is met. If the norm of the search direction p(k) is less than a tolerance threshold, or the position difference {s k} between adjacent two iteration points is less than a set value, it is considered that the sequence quadratic programming algorithm has met the convergence condition, and the information of the optimal path is output.

[0065] An unmanned aerial vehicle path planning system includes:

[0066] A modeling module is configured to construct an environment model for unmanned aerial vehicle operation.

[0067] A setting module is configured to set an objective function and constraint conditions for unmanned aerial vehicle path planning.

[0068] A first determining module is configured to determine information of an approximately optimal path based on the environment model, the objective function, and the constraint conditions by using an ant colony algorithm.

[0069] A second determining module is configured to apply the information of the approximately optimal path to a sequence quadratic programming algorithm to determine an optimal path.

[0070] The unmanned aerial vehicle path planning method and system provided by the application, on the basis of the ant colony algorithm, secondary optimization is carried out by combining the sequence quadratic programming algorithm (SQP), the problems that the traditional ant colony algorithm is easy to fall into local optimum and slow convergence speed are solved, the path length of the unmanned aerial vehicle path planning is successfully shortened, the iteration number is reduced, higher efficiency and more accurate path optimization are realized, the optimized algorithm has strong adaptability and robustness, the sensitivity to initial parameters and environmental changes is low, the local optimal solution can be avoided to a certain extent, and the complex nonlinear programming problem can be converted into a relatively simple quadratic programming subproblem, the running efficiency is faster and the search quality is better, and the algorithm is particularly suitable for path planning and obstacle avoidance problems in dynamic environment and uncertain complex conditions; the application has an innovative position in numerous research results in the field of unmanned aerial vehicle path planning, and the optimized algorithm can also be extended to multi-unmanned aerial vehicle cooperative path planning and used for multi-task planning problems, which is of great significance for improving the efficiency and safety of the multi-unmanned aerial vehicle system. BRIEF DESCRIPTION OF DRAWINGS

[0071] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiments will be briefly introduced below, and obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor on the premise of the drawings.

[0072] Figure 1 A flow chart of an unmanned aerial vehicle path planning method provided by the embodiment of the present application is provided.

[0073] Figure 2 An unmanned aerial vehicle path planning graph generated based on the ant colony algorithm provided by the embodiment of the present application is provided.

[0074] Figure 3 An unmanned aerial vehicle path planning graph generated based on the ant colony algorithm and the sequence quadratic programming algorithm provided by the embodiment of the present application is provided.

[0075] Figure 4 A structural block diagram of an unmanned aerial vehicle path planning system provided by the embodiment of the present application is provided. DETAILED DESCRIPTION

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

[0077] It should be noted that all the direction indications (such as up, down, left, right, front, back, etc.) in the embodiments of the present application are only used to explain the relative position relationship, movement condition, etc. between components in a certain specific posture (as shown in the drawings), and if the specific posture changes, the direction indications will also change accordingly.

[0078] In addition, the description involving "first", "second" and the like in the present application is only for the purpose of description, and cannot be understood as indicating or implying the relative importance of the indicated technical features or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" can explicitly or implicitly include at least one of the features. In addition, "and / or" throughout the text includes three schemes, for example, A and / or B includes A technical scheme, B technical scheme, and A and B simultaneously meet the technical scheme; in addition, the technical schemes of each embodiment can be combined with each other, but it must be based on the realization of the ordinary skilled in the art, when the combination of technical schemes appears contradictory or cannot be realized, it should be considered that the combination of technical schemes does not exist, nor is it within the protection scope required by the present application.

[0079] For the convenience of description, the following describes some nouns or terms related to the embodiments of the present application:

[0080] Ant colony optimization (ACO): the walking path of an ant represents a feasible solution to the optimization problem, all paths of the entire ant colony represent feasible solutions to the optimization problem, and all paths of the entire ant colony constitute the solution space of the optimization problem. The amount of pheromone released by an ant with a shorter path is more, and with the passage of time, the cumulative pheromone concentration on the shorter path gradually increases, and the number of ants selecting this path also becomes more and more, and the entire ant will concentrate on the best path under the action of positive feedback, which corresponds to the optimal solution of the optimization problem. The algorithm has the defects of many algorithm parameters, complex formula, large amount of calculation, low initial search efficiency, easy to fall into local optimal solution, and greatly affected by the initial value of the parameter.

[0081] The sequence quadratic programming (SQP) algorithm is a powerful and mature iterative optimization technique, the core of which is to approximate the optimal solution by sequentially solving the quadratic programming sub-problems of the approximate original problem. With its fast (usually super-linear) convergence speed, direct and accurate handling of constraints, and solid theoretical foundation, SQP has become the mainstream method for solving small and medium-sized constrained nonlinear optimization problems. Despite the challenges of sensitivity to initial points and large-scale problem computation, by combining line search, trust region, quasi-Newton method and advanced QP solver strategies, modern SQP algorithms perform very well in practice and are widely used.

[0082] For example, as Figure 1As shown, the unmanned aerial vehicle path planning method provided by the application comprises the following steps:

[0083] S1. Constructing an environment model for unmanned aerial vehicle operation;

[0084] S2. Setting a target function and constraint condition for unmanned aerial vehicle path planning;

[0085] S3. Based on the environment model, the target function and the constraint condition, adopting an ant colony algorithm to determine information of an approximately optimal path;

[0086] S4. Applying the information of the approximately optimal path to a sequential quadratic programming algorithm to determine an optimal path.

[0087] The unmanned aerial vehicle path planning method provided by the application, on the basis of the ant colony algorithm, combines the sequential quadratic programming algorithm (SQP) for secondary optimization, solves the problems of the traditional ant colony algorithm, such as easy falling into local optimum and slow convergence speed, successfully shortens the path length of unmanned aerial vehicle path planning, reduces the iteration number, realizes more efficient and more accurate path optimization, and the optimized algorithm has strong adaptability and robustness, low sensitivity to initial parameters and environmental changes, can avoid falling into local optimal solution to a certain extent, and can convert a complex nonlinear programming problem into a relatively simple quadratic programming subproblem, has faster running efficiency and better search quality, and is particularly suitable for path planning and obstacle avoidance problems under dynamic environment and uncertain complex conditions; the application has an innovative position among numerous research results in the field of unmanned aerial vehicle path planning, and the optimized algorithm can also be extended to multi-unmanned aerial vehicle cooperative path planning for multi-task planning problems, which is of great significance for improving the efficiency and safety of the multi-unmanned aerial vehicle system.

[0088] In an embodiment, the reference terrain of the unmanned aerial vehicle operation environment is set as flat by default, and the environment model comprises a static obstacle model, a no-fly zone environment model and a sudden obstacle model.

[0089] Specifically, the unmanned aerial vehicle needs to face the threat of urban buildings, such as high-rise buildings, trees, scenic buildings, etc. in the urban environment, so it is necessary to establish an obstacle model. Considering the obstacles that the unmanned aerial vehicle will face in the actual flight process, a cylindrical obstacle is adopted to simulate the static obstacle in the establishment of the environment model, and the formula related to the static obstacle model is as follows:

[0090]

[0091] Wherein, (x, y) represents the plane position of any point in the static obstacle model at this moment; (x i ,y i ) represents the center coordinates of the static obstacle model; h irepresents the height of any point in the actual static obstacle; z(x, y) represents the elevation value of the corresponding point in the static obstacle model; R represents the footprint radius of the actual static obstacle; and r represents the distance of any point in the static obstacle model from the center of the static obstacle model at the moment.

[0092] Specifically, the unmanned aerial vehicle is mostly used in urban flight environment, and the urban environment will be provided with unmanned aerial vehicle no-fly zones in airport clearance area, large event site, and citizen residential area. Considering that the unmanned aerial vehicle needs to avoid these temporary or fixed no-fly zones in the flight process, in order to further improve the safety of the planned route, all unmanned aerial vehicle no-fly zones are abstracted as hemispheres, and the formula involved in the no-fly zone environment model is as follows:

[0093]

[0094] wherein (x, y) represents the plan position of any point in the no-fly zone environment model at the moment; (x i ,y i ) represents the center coordinates of the i th no-fly zone; z(x, y) represents the vertical height corresponding to the point (x, y); R represents the simulation radius of the no-fly zone; and r represents the distance of the point (x, y) from the center of the no-fly zone model at the moment.

[0095] Specifically, the sphere is used to simulate the sudden obstacle that the unmanned aerial vehicle may encounter in the flight process, and the formula involved in the sudden obstacle model is as follows:

[0096] (x-x0) 2 +(y-y0) 2 +(z-z0) 2 =r 2

[0097] wherein (x, y, z) represents the three-dimensional coordinates of any point in the sudden obstacle model at the moment; (x0, y0, z0) represents the center coordinates of the sudden obstacle model; and r represents the simulation radius of the sudden obstacle.

[0098] Specifically, as shown in Figure 2 and Figure 3 , the red cylinder on the left side of the front view in Figure 2 and Figure 3 represents the static obstacle in the urban environment, the blue hemisphere represents the urban no-fly zone, and the dark blue sphere represents the sudden obstacle; Figure 2 and Figure 3 , the right view is a top view, and the light blue large circle therein represents the urban no-fly zone under the top view angle, and the dark blue circle represents the static obstacle and the sudden obstacle under the top view angle.

[0099] In an embodiment, the objective function comprises a minimum length indicator function, and the constraint condition comprises a minimum path length constraint and a UAV collision constraint;

[0100] Specifically, the minimum length indicator function involves the following formula:

[0101]

[0102] wherein F(x) represents the minimum length indicator function; n represents the total number of path points in the UAV planned path; x i represents the i-th point on the path; (a i ,b i ,z i ) represents the coordinate value of x i ; d(x i ,x i+1 ) represents the Euclidean distance from x i to x i+1 ;

[0103] The minimum path length is the shortest distance that must be flown straight before changing the flight attitude to prevent the UAV from frequently adjusting the attitude, and the minimum path length constraint involves the following formula:

[0104]

[0105] wherein f i represents each path distance; f min represents the minimum path length;

[0106] Specifically, in order to effectively complete the path planning and ensure the safety of the flight route, a UAV collision constraint model is established, a spherical safety zone is set with the UAV center of mass as the center, and when the distance between the UAV and the obstacle is less than the UAV safety radius, there is a collision danger, so the distance is constrained, and the UAV collision constraint involves the following formula:

[0107]

[0108] wherein the spatial obstacle model O i is a sphere with a radius of ; the center coordinates of the spatial obstacle model are (O xi ,O yi ,O zi ); (x, y, z) represents the center of mass coordinates of the UAV in the three-dimensional space; L represents the distance between the UAV and the spatial obstacle; and R represents the UAV safety radius. Specifically, the spatial obstacle includes static obstacles and sudden obstacles.

[0109] In an embodiment, step S3 comprises:

[0110] S31. Initialize the ant colony parameters and the pheromone concentration carried by all ants;

[0111] S32. Assign all ants to the initial node;

[0112] Specifically, the ant colony parameters are initialized, the number of ant colonies and the pheromone concentration carried by all ants are initialized, and the initial node of each ant is randomly assigned within the variable range [l i , μ i ], and the specific formula is as follows:

[0113]

[0114] wherein τ(k) represents the pheromone concentration of the kth ant at the initial node; F(x) represents a minimum length index function; x represents an initial path whose length is to be calculated; x k represents a random path generated by the kth ant.

[0115] S33. Let all ants start from the starting point, select paths according to the pheromone concentration and heuristic information, and gradually build routes to the target point;

[0116] Specifically, all ants select the next node transition according to the state transition probability, and the specific formula is as follows:

[0117]

[0118] wherein represents the probability of the kth ant passing through nodes m and n at time t; τ mn (t) represents the pheromone concentration between nodes m and n; η mn (t) represents a heuristic function; D mn represents the Euclidean distance between nodes m and n; α is an information pheromone importance factor; β is a heuristic function importance factor; next k represents a set of next candidate nodes of the ant;

[0119] It should be noted that the larger the value of the information pheromone importance factor α, the more the ant tends to select the path passed by other ants, and the stronger the cooperation between ants, but it is easy to fall into local optimization; the larger the value of the heuristic function importance factor β, the closer the state transition probability is to the greedy rule; the values of α and β are set as dynamic parameters, represented by α(L) and β(L) respectively, and the expressions of α(L) and β(L) are as follows:

[0120]

[0121] wherein M and N are constant terms; γ is an adjustment factor; H represents the current iteration number; Hmax represents the maximum number of iterations; when the pheromone concentration of each discrete point is not much different at the beginning of the ant colony algorithm, a plays a leading role, increasing the diversity of the search; as the iteration continues, the pheromone concentration of each discrete point will continue to increase, at this time, a is reduced and b is increased, which can prevent the ant colony algorithm from falling into a local optimum, and at the same time, the speed of searching for the optimal solution of the path is increased.

[0122] S34. updating the pheromone concentration according to the path quality;

[0123] Specifically, all ants walk through a selected path once, and the original pheromone of the path will volatilize over time. Through the continuous selection of the pheromone concentration of the path by the ants, the pheromone concentration of the path will increase. Through the pheromone updating process, the ant colony algorithm can realize the dynamic adjustment of the pheromone concentration on the path, thereby guiding the ants to gradually converge to the optimal solution in the search process. The pheromone concentration is updated according to the following formula:

[0124] τ(k) = (1 - p) τ(k) + Δτ(k)

[0125]

[0126] wherein, Δτ(k) represents the increment of the pheromone concentration on the path after the kth iteration; a and b are algorithm parameters, which need to be set in advance; τ(k) represents the pheromone concentration of the kth ant at the initial node; p is the pheromone volatilization coefficient, which can make the pheromone decay with the iteration, thereby avoiding the algorithm falling into a local optimum.

[0127] It should be noted that both calculation formulas of Δτ(k) are correct, is associated by an exponential function, is associated by a fractional function. When the calculation formula of Δτ(k) is selected, it needs to be seen which one will make the convergence speed of the ant colony algorithm faster and more stable under the specified conditions.

[0128] S35. judging whether the maximum number of iterations is reached; if yes, outputting the information of the approximate optimal path; if no, returning to step S32, and then executing steps S32-S35 at least once until the maximum number of iterations is reached.

[0129] Specifically, the transition probability of each ant is calculated, and the ant is moved to the next node according to the state transition rule; the ant position and the pheromone concentration are updated according to the path selection formula, and this step is repeated until all ants move to the target point. The path length of each ant is calculated, and the approximate optimal path of this iteration is obtained. If the maximum number of iterations is reached, the final approximate optimal path is generated. If the maximum number of iterations is not reached, all ants are again distributed to the initial node, and then steps S32-S35 are executed at least once until the maximum number of iterations is reached.

[0130] In an embodiment, step S4 comprises:

[0131] S41. Construct a Lagrangian function to approximate the nonlinear optimization problem containing the original objective function and constraint conditions into a quadratic programming sub-problem, the formula of the Lagrangian function is as follows:

[0132] L(x, λ, μ) = f(x) + ∑λg(x) + ∑μh(x)

[0133] Wherein, x refers to the decision variable, representing the coordinates of the path point; λ is the Lagrange multiplier, corresponding to the inequality constraint function; μ is the Lagrange multiplier, corresponding to the equality constraint function; f(x) is the objective function; g(x) is the inequality constraint function; h(x) is the equality constraint function;

[0134] In each step of the sequence quadratic programming algorithm iteration, a quadratic programming sub-problem containing a quadratic objective function and linearized constraint conditions is constructed, and the form of the quadratic programming sub-problem is as follows:

[0135]

[0136] Wherein, d is the search direction vector, that is, the direction of the iteration step; x k+1 = x k + αd, α is the step size, x k represents the value of x k at the kth iteration of the independent variable x of the objective function f(x); H k represents the Hessian matrix, which is the second derivative matrix of the objective function f(x) at x k ; is the gradient of the objective function f(x) at x k ; represents the gradient of the constraint function g(x) at x k ; represents the gradient of the constraint function h(x) at x k ;

[0137] S42. Iteratively solve the quadratic programming sub-problem.

[0138] Specifically, the step S42 of iteratively solving the quadratic programming sub-problem comprises:

[0139] S421. Update the Hessian matrix using the BFGS method, which involves the following formula:

[0140]

[0141] {s k} = x {k+1} - x k

[0142]

[0143] where H k is the approximation of the Hessian matrix at the kth iteration; {y k} represents the difference of the gradient of the objective function f(x) at the adjacent two iteration points x k and x k+1 ; {s k} represents the difference of the position of the adjacent two iteration points; p(k) represents the search direction; x k refers to the value of the independent variable x of the objective function f(x) at the kth iteration; f(x k ) is the function value of the objective function f(x) at x k ;

[0144] Specifically, the BFGS method and the Hessian matrix are both calculation processes embedded in the sequential quadratic programming algorithm. When the BFGS method is used to update the Hessian matrix, the essence is to approximately replace the second-order information of the objective function, which can make the sequential quadratic programming algorithm avoid direct calculation of high-order derivatives and reduce the calculation cost.

[0145] S422. In the search direction determined by the quasi-Newton method, a line search method is used to find a suitable step size, update the independent variable, and determine whether the convergence condition is met. If yes, the information of the optimal path is output; if no, return to step S421, and then execute steps S421-S422 at least once in turn until the convergence condition is met.

[0146] Specifically, in the search direction determined by the quasi-Newton method, a line search is used to find a suitable step size a, update the variable x k+1 = x k + a * p(k), p(k) is the search direction, which ensures the descent of the objective function, and checks whether the convergence condition is reached. If the norm of the search direction p(k) is less than the tolerance threshold, or the difference {s k} between the positions of the adjacent two iteration points is less than the set value, it is considered that the sequential quadratic programming algorithm has met the convergence condition, and the information of the optimal path is output.

[0147] On the contrary, if the norm of the search direction p(k) is greater than or equal to the tolerance threshold, or the difference {s k} between the positions of the adjacent two iteration points is greater than or equal to the set value, it is considered that the sequential quadratic programming algorithm does not meet the convergence condition. At this time, it is necessary to return to step S421, and then execute steps S421-S422 at least once in turn until the convergence condition is met.

[0148] Specifically, the norm is a mathematical concept for measuring the "length" or "size" of a vector, and the norm of the search direction vector can reflect the span in the iteration step direction. In this embodiment, the norm of the search direction vector p(k) is used to determine whether the change in the iteration step direction is small enough to assist in the convergence condition judgment. The sequence quadratic programming algorithm adjusts the path step by step through iteration, and a small path change means that the optimization tends to be stable and close to convergence. The corresponding parameter s k, that is, the position difference between adjacent two iteration points x k and x k+1 , can be represented by the norm of the vector difference ||x k -x k+1 ||. The smaller the norm, the smaller the path change, and the iteration tends to converge.

[0149] Specifically, in step S422, the formula involved in the line search method is as follows:

[0150]

[0151] Where f(x k ) is the function value of the objective function f(x) at x k ; a is the step size; p(k) represents the search direction; c1 is the sufficient descent condition parameter of the line search, usually c1 e (0, 1); c2 is the curvature condition parameter, usually c2 e (c1, 1).

[0152] It should be noted that in this embodiment, the approximate optimal path information obtained by the ant colony algorithm (essentially a set of path point coordinates and other decision variable values) is applied to the sequence quadratic programming (SQP) algorithm, which is converted into the initial value x0 of the sequence quadratic programming algorithm iteration (i.e. x k , k = 0) as the starting iteration point of the sequence quadratic programming algorithm. The sequence quadratic programming algorithm starts iteration from this initial value x0, and the subsequent x k is updated based on this. When the sequence quadratic programming algorithm constructs a quadratic programming subproblem, the gradient of the objective function ▽f(x k ), the constraint gradient ▽g(x k ), ▽h(x k ), and the Hessian matrix H k are all calculated based on the initial value x0 provided by the ant colony algorithm in the first iteration.

[0153] Figure 2 A UAV path planning graph generated based on an ant colony algorithm provided by an embodiment of the application, Figure 3 A UAV path planning graph generated based on an ant colony algorithm and a sequence quadratic programming algorithm provided by an embodiment of the application, such as Figure 2As shown in the figure, the ant colony algorithm takes a total of 93.18 seconds to calculate a feasible path, and the shortest path length is 610. Although the path successfully avoids obstacles, its path is not completely smooth and has more tortuous changes in space. The ant colony algorithm usually generates unnecessary turning points on the path due to multiple random searches and local iterations, which may lead to higher driving costs in practical applications.

[0154] After obtaining the approximate optimal path optimized by the ant colony algorithm, it was input into the sequential quadratic programming algorithm as the initial value for further refinement. Since there was already a better initial solution, the computational efficiency of the optimization method was significantly improved, and the total time consumed was only 0.93 seconds. After optimization, the length of the final optimal path was 178.23. Compared with the initial path generated by the ant colony algorithm, this path was smoother and straighter, and significantly reduced unnecessary bends. Figure 3 As shown in the figure, the optimized path basically moves along the shortest straight line trend, while successfully avoiding obstacles, showing obvious high efficiency.

[0155] like Figure 4 As shown, the present invention also provides a UAV path planning system 100, which includes:

[0156] Modeling module 110, for building an environment model for the UAV operation;

[0157] Setting module 120, used to set the objective function and constraints of the UAV path planning;

[0158] A first determination module 130 is configured to determine information of an approximately optimal path using an ant colony algorithm based on an environment model, an objective function, and constraints;

[0159] The second determination module 140 is configured to apply the information of the approximate optimal path to a sequential quadratic programming algorithm to determine the optimal path.

[0160] The above description is only a preferred embodiment of the present invention and does not limit the patent scope of the present invention. All equivalent structural transformations made by using the contents of the present invention description and drawings under the inventive concept of the present invention, or direct / indirect application in other related technical fields are included in the patent protection scope of the present invention.

Claims

1. A UAV path planning method, characterized in that: The following steps are involved: S1. Build an environmental model for UAV operation; S2. Set the objective function and constraints for the UAV path planning; S3. Based on the environmental model, the objective function and the constraints, an ant colony algorithm is used to determine the approximate optimal path information; S4. Apply the information of the approximate optimal path to a sequential quadratic programming algorithm to determine the optimal path.

2. The UAV path planning method according to claim 1, characterized in that: The environmental model includes a static obstacle model, a no-fly zone environment model and an unexpected obstacle model; The formula involved in the static obstacle model is as follows: Among them, (x, y) represents the plane position of any point in the static obstacle model at this moment; (x i ,y i ) represents the center coordinate of the static obstacle model; h i represents the height of any point in the actual static obstacle; z(x,y) represents the elevation value of the corresponding point in the static obstacle model; R represents the radius of the actual static obstacle; r represents the distance from any point in the static obstacle model to the center of the static obstacle model at this moment; The formulas involved in the no-fly zone environment model are as follows: Among them, (x, y) represents the plane position of any point in the no-fly zone environment model at this moment; (x i ,y i ) represents the center coordinates of the i-th no-fly zone; z(x,y) represents the vertical height corresponding to the point (x,y); R represents the simulation radius of the no-fly zone; r represents the distance from the point (x,y) to the center of the no-fly zone model at this moment; The formula involved in the sudden obstacle model is as follows: (x-x0) 2 +(y-y0) 2 +(z-z0) 2 =r 2 Among them, (x, y, z) represents the three-dimensional coordinates of any point in the sudden obstacle model at this moment; (x0, y0, z0) represents the center coordinates of the sudden obstacle model; and r represents the simulation radius of the sudden obstacle.

3. The UAV path planning method according to claim 1, characterized in that: The objective function includes a minimum length indicator function, and the constraints include a minimum path length constraint and a drone collision constraint; The formula involved in the minimum length indicator function is as follows: Where F(x) represents the minimum length index function; n represents the total number of path points in the UAV planning path; x i represents the i-th point on the path; (a i ,b i ,z i ) represents x i Coordinate value of d(x i ,x i+1 ) means from x i to x i+1 The Euclidean distance of The formula involved in the minimum path length constraint is as follows: Among them, f i Indicates the distance of each path; f min Indicates the minimum path length; The formula involved in the UAV collision constraint is as follows: Among them, set the spatial obstacle model O i Therefore The center coordinates of the space obstacle model are (O xi ,O yi ,O zi ); (x, y, z) represents the coordinates of the center of mass of the UAV in three-dimensional space; L represents the distance between the UAV and the spatial obstacle; R represents the safety radius of the UAV.

4. The UAV path planning method according to claim 1, characterized in that: Step S3 includes: S31. Initialize the ant colony parameters and the pheromone concentrations carried by all ants; S32. Assign all ants to the initial node; S33. All ants start from the starting point and select a path based on the pheromone concentration and heuristic information, gradually building a route to the target point. S34. Update pheromone concentration according to path quality; S35. Determine whether the maximum number of iterations has been reached; if so, output the information of the approximate optimal path; if not, return to step S32, and then execute steps S32 to S35 at least once in sequence until the maximum number of iterations has been reached.

5. The UAV path planning method according to claim 4, characterized in that: Step S32 includes: In the variable range [l i ,μ i ] to randomly assign the initial node of each ant. The specific formula is as follows: Where τ(k) represents the pheromone concentration of the kth ant at the initial node; F(x) represents the minimum length indicator function; x represents the initial path to be calculated; x k represents the random path generated by the kth ant.

6. The UAV path planning method according to claim 4, characterized in that: In step S33, all ants select the next node to transfer to based on the state transition probability. The formula involved is as follows: in, represents the probability that ant k passes through nodes m and n at time t; τ mn (t) represents the pheromone concentration between two nodes m and n; η mn (t) represents the heuristic function; D mn represents the Euclidean distance between two nodes m and n; α is the pheromone importance factor; β is the heuristic function importance factor; next k Represents the set of nodes to be selected next by the ant; In step S34, the pheromone concentration is updated according to the following formula: τ(k)=(1-ρ)τ(k)+△τ(k) Where △τ(k) represents the increment of pheromone concentration on the path after the kth iteration; a and b are algorithm parameters that need to be set in advance; τ(k) represents the pheromone concentration of the kth ant at the initial node; and ρ is the pheromone volatility coefficient.

7. The UAV path planning method according to claim 1, characterized in that: Step S4 includes: S41. Construct a Lagrangian function to approximately convert the nonlinear optimization problem including the original objective function and the constraints into a quadratic programming subproblem. The Lagrangian function formula is as follows: L(x,λ,μ)=f(x)+∑λg(x)+∑μh(x) Where x is the decision variable, representing the coordinates of the path point; λ is the Lagrange multiplier, corresponding to the inequality constraint function; μ is the Lagrange multiplier, corresponding to the equality constraint function; f(x) is the objective function; g(x) is the inequality constraint function; h(x) is the equality constraint function; The quadratic programming sub-problem is as follows: Among them, d is the search direction vector, that is, the direction of the iteration step; x k+1 =x k +αd, α is the step size; H k Represents the Hessian matrix, specifically the objective function at x k The second-order derivative matrix at ; is the objective function at x k The gradient at S42. Iteratively solve the quadratic programming subproblem.

8. The UAV path planning method according to claim 7, characterized in that: The step S42 includes: S421. Use the BFGS algorithm to update the Hessian matrix. The formula involved is as follows: {s k }=x {k+1} -x k Among them, H k is the approximate value of the Hessian matrix at the kth iteration; {y k } indicates that the objective function f(x) is at two adjacent iteration points x k and x k+1 The gradient difference at {s k } represents the position difference between two adjacent iteration points; p(k) represents the search direction; x k Refers to the value of the independent variable x of the objective function f(x) at the kth iteration; f(x k ) is the objective function f(x) at x k The function value at ; S422. In the search direction determined by the quasi-Newton method, use the line search method to find a suitable step size, update the independent variable, and determine whether the convergence condition is met. If so, output the information of the optimal path; if not, return to step S421, and then execute steps S421 to S422 in sequence at least once until the convergence condition is met.

9. The UAV path planning method according to claim 8, characterized in that: Step S422 includes: In the search direction determined by the quasi-Newton method, use line search to find the appropriate step size α and update the variable x k+1 =x k +α*p(k), p(k) is the search direction, ensuring that the objective function decreases, and checking whether the convergence condition is met. If the norm of the search direction p(k) is less than the tolerance threshold, or the position difference between two adjacent iteration points {s k When} is less than the set value, it is considered that the sequential quadratic programming algorithm has met the convergence condition and the information of the optimal path is output.

10. A UAV path planning system, characterized in that: include: Modeling module, used to build the environment model of UAV operation; The setting module is used to set the objective function and constraints of the UAV path planning; A first determination module is configured to determine information of an approximately optimal path using an ant colony algorithm based on the environment model, the objective function, and the constraint conditions; The second determination module is used to apply the information of the approximate optimal path to a sequential quadratic programming algorithm to determine the optimal path.

Images