A method for aircraft trajectory planning considering environmental adaptive maneuvers

Abstract

The present invention relates to a method for planning an aircraft trajectory considering environmental adaptive maneuvers. First, a global planning is performed in an occupied grid map by a path search method to generate a discrete path located in a relatively large obstacle-free space; then, a safe area around each path point is modeled, and a low-order MPC trajectory optimization model is established to further generate a smooth and safe reference trajectory; finally, a maneuvering model characterizing environmental adaptive behavior is established based on the gradient information of the Euclidean signed distance field and the motion state of the aircraft, and a high-order MPC trajectory optimization model is established to generate a maneuverable adaptive, safe, dynamically feasible local trajectory. The present invention utilizes a three-layer framework from "coarse" to "fine" to plan the trajectory, which not only effectively improves the safety level of the aircraft, but also ensures flexible maneuverability, and provides a new solution to the problem that the current aircraft is difficult to effectively balance rapidity and safety in autonomous flight under complex environments.

Description

Technical Field

[0001] The present invention belongs to the field of trajectory planning of aircraft, and in particular relates to an aircraft trajectory planning method taking environmental adaptive maneuvers into consideration. Background Art

[0002] In recent years, the application scenarios of drones have become increasingly complex, such as industrial inspection, emergency rescue, post-disaster search and rescue, and mine exploration, etc. These scenarios have high requirements for the autonomy, safety, and maneuverability of drones.

[0003] Faced with the unknown and complex real environment, how to effectively plan a reasonable trajectory so that the aircraft can safely avoid various obstacles, reach the target location smoothly, and have flexible maneuverability is an issue that needs in-depth research in aircraft trajectory planning. Existing methods either rely heavily on prior knowledge of known environments, such as the method in Chinese invention patent CN111707269A "A method for unmanned aerial vehicle path planning in a three-dimensional environment" which requires global obstacle information and is not suitable for tasks in unknown complex environments; or only use distance information from obstacles for obstacle avoidance constraints, such as the method in Chinese invention patent CN114594785A "A method for unmanned aerial vehicle obstacle avoidance real-time trajectory planning based on mixed integer second-order cone optimization" which determines the feasible motion state set by constructing a distance matrix and a velocity matrix with obstacles. When the aircraft flies at a faster speed, the errors in state estimation, perception and control will also increase, reducing safety; or are based on deep learning, such as the method in Chinese invention patent CN113848974A "A method and system for aircraft trajectory planning based on deep reinforcement learning" which uses a model composed of a long short-term memory network and a deep neural network as a nonlinear function fitter to fit the planning strategy. The performance of the method relies heavily on the quality of training and is not suitable for complex and changeable application scenarios. Summary of the invention

[0004] In order to solve the above problems, the present invention provides an aircraft trajectory planning method taking environmental adaptive maneuvering into consideration, which can plan a trajectory for a UAV in a larger obstacle-free space and can adaptively adjust the flight speed according to the distribution of obstacles in the environment and the state of the system, thereby improving safety and ensuring flexible maneuverability.

[0005] In order to achieve the above object, the present invention adopts the following technical solution:

[0006] The invention discloses an aircraft trajectory planning method considering environmental adaptive maneuvers, which includes a path search method, a low-order MPC trajectory optimization and a high-order MPC trajectory optimization.

[0007] The path search method uses the posture information provided by the navigation positioning and the Euclidean signed distance field and occupancy grid map that characterize the environment to plan a discrete path; then, based on the path, the low-order MPC trajectory optimization model is constructed, and a reference trajectory is generated after the optimization; finally, using the reference trajectory, the high-order MPC trajectory optimization model is constructed, and a local desired trajectory is generated after the optimization and provided to the underlying controller.

[0008] The path search method performs global planning in the occupancy grid map, specifically comprising the following steps:

[0009] Step A1: Obtain the current position information of the aircraft and the point cloud data of the surrounding environment according to the measurement information of the sensors carried by the aircraft, and establish an occupancy grid map and a Euclidean signed distance field;

[0010] Step A2: Create two tables OpenSet and CloseSet to store nodes in the search process, and add the starting node to OpenSet;

[0011] Step A3: Take the node n with the minimum overall cost from the non-empty OpenSet table min , determine whether it is equal to the target node, if so, obtain a series of discrete path points and end, otherwise obtain n min neighbor nodes, and n min Remove from OpenSet and add to CloseSet;

[0012] The total cost f = λ1g e +λ2g v +λ3g f +g p , where g e Represents the Euclidean distance cost between the current node and the parent node, g v represents the cost of the obstacle-free space volume around the current node, g f represents the heuristic distance cost between the current node and the target point, g p Represents the total cost of the parent node of the current node, λ i (i=1,2,3) represents weight;

[0013] Step A4: Calculate the g of the neighboring nodes of the current node e and g v , using Euclidean distance to calculate g e : where n n,i (i=x,y,z) is the three-dimensional coordinate of the neighbor node of the current node in the inertial system, n c,i(i=x, y, z) is the three-dimensional coordinate of the current node in the inertial system; g v The mathematical expression is where r thr is the safety threshold, r(n n ) is the distance between the neighbor node and the nearest obstacle in the Euclidean signed distance field, n n is the three-dimensional coordinate of the neighbor node in the inertial system;

[0014] Step A5: Calculate the g of the neighboring nodes of the current node f The mathematical expression is:

[0015] Δ=n n -g

[0016]

[0017]

[0018]

[0019] Among them, g is the three-dimensional coordinate of the target node in the inertial system, Δ is the vector difference between the two coordinates, and Δ m is the shortest distance among the three coordinate axes, Δ n is the second shortest distance among the three coordinate axes, Δ {x,y,z}\{m,n} is the longest distance among the three coordinate axis distances, min is the minimum value function, |·| is the absolute value function, and β is the adjustment coefficient;

[0020] Step A6: If the neighbor node is in the CloseSet or has passed an obstacle, it is discarded without processing; otherwise, the following process is continued for the neighbor node:

[0021] (i) If it is not in OpenSet, add it to OpenSet. The current node n c is its parent node; (ii) if the current node is already in the OpenSet, then use f to determine whether this path is a path with a lower cost. If so, update the information of the neighboring nodes;

[0022] Step A7: Repeat the above steps A3-A6 until the end.

[0023] Further, the model of low-order MPC trajectory optimization is established as:

[0024]

[0025] stx k+1 =f d (x k ,u k )

[0026] x 0 =x0

[0027] u k ∈Ω,x k ∈X

[0028] Among them, J u (·) represents the smooth penalty function, J s (·) represents the penalty function for tracking discrete paths, J c (·) represents the static obstacle avoidance penalty function, J v (·) represents the obstacle-free space volume penalty function around the path point, f d (·) is the state transition function, x k is the kth system state, u k is the kth system input, x0 is the starting state, N is the number of discrete path points, and λ u ,λ s ,λ c ,λ v are weighted coefficients, Ω and X represent the state set.

[0029] In the low-order MPC trajectory optimization model, the smoothing, tracking and static obstacle avoidance cost functions of the j-th reference trajectory are defined as:

[0030] J u (u k+1 ,u k )=||u k+1 -u k || 2

[0031]

[0032]

[0033] Among them, c thr is the safety threshold, c(p k ) is p k The distance p to the nearest obstacle in the Euclidean signed distance field k is the kth optimized trajectory point, is the kth path point on the discrete path.

[0034] In the low-order MPC trajectory optimization model, the obstacle-free space volume cost function of the j-th reference trajectory is defined as:

[0035]

[0036]

[0037]

[0038]

[0039] Among them, ρ is the coefficient that balances safety, mobility and solution efficiency, w k is the weighted coefficient of the cost of the kth trajectory point, r k is the radius of the spherical safety area around the kth trajectory point, k t is a constant coefficient, p 0 is the starting trajectory point, max{} is the maximum value function, and tanh() is the hyperbolic tangent function.

[0040] The model of high-order MPC trajectory optimization is established as:

[0041]

[0042]

[0043]

[0044] Among them, J d (·) represents the penalty function that is dynamically feasible, J t (·) represents the penalty function for tracking the reference trajectory, J c (·) represents the static obstacle avoidance penalty function, J e (·) represents the penalty function for environment adaptive maneuvers, f d (·) is the state transfer function, N1 represents the number of optimized trajectory points, x k and θ k is the kth system state, u k and is the kth system input, λ d ,λ t ,λ c ,λ e are weighting coefficients, Ω x and Ω θ is the set of system inputs, X x and X θ A collection of system states.

[0045] In the high-order MPC trajectory optimization model, the cost function of the j-th reference trajectory for dynamic feasibility and tracking the reference trajectory is defined as:

[0046]

[0047]

[0048]

[0049] Among them, f μ () is a piecewise function, d μ is the variable on the μ axis, d max is the upper bound of the variable, d min is the lower bound of the variable, v k represents the velocity vector of the kth trajectory point, a k represents the acceleration vector of the kth trajectory point, j k The vector representing the acceleration derivative of the kth trajectory point, is the kth reference trajectory point, is the velocity vector of the kth reference trajectory point, and γ is used to balance the tracking error and tracking progress.

[0050] In the high-order MPC trajectory optimization model, the cost function of the environment adaptive maneuver of the j-th reference trajectory is defined as:

[0051]

[0052]

[0053]

[0054]

[0055] Among them, <, > represents vector dot product, is the gradient of the Euclidean signed distance field, α1 and α2 are positive coefficients, v thr is the initial safety speed threshold, is the safe speed threshold of the kth trajectory point, η(β) represents the sigmoid function, β represents the cosine function, and ||·|| represents the modulus of the vector.

[0056] The advantages of the present invention compared with the prior art are:

[0057] (1) The path search method in the aircraft trajectory planning method considering environmental adaptive maneuvers disclosed in the present invention comprehensively considers the search efficiency and the safety of the aircraft. The searched path is located in a relatively large obstacle-free space, thereby improving the safety of the aircraft.

[0058] (2) In the aircraft trajectory planning method considering environmental adaptive maneuvers disclosed in the present invention, a low-order MPC trajectory optimization model is constructed that comprehensively considers smoothness, obstacle avoidance, tracking and safety area constraints, further optimizes the spatial position of each path point, and generates a smooth and safe reference path.

[0059] (3) In the method for trajectory planning of an aircraft considering environmental adaptive maneuvers disclosed in the present invention, a high-order MPC trajectory optimization model is constructed that comprehensively considers tracking, obstacle avoidance, dynamic feasibility, and adaptive maneuver constraints to generate a desired local trajectory. The aircraft can adaptively adjust the flight speed according to the distribution of obstacles in the environment and the state of the system, which not only improves safety but also ensures flexible maneuverability.

[0060] (4) The present invention discloses a three-layer planning framework from "coarse" to "fine" in a method for aircraft trajectory planning that takes environmental adaptive maneuvers into consideration, which can plan trajectories online in real time in an unknown and complex environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 A three-layer framework diagram from "coarse" to "fine" of an aircraft trajectory planning method considering environmental adaptive maneuvers of the present invention;

[0062] Figure 2 A schematic diagram of a path search method of an aircraft trajectory planning method considering environmental adaptive maneuvers according to the present invention;

[0063] Figure 3 A schematic diagram of low-order MPC trajectory optimization of an aircraft trajectory planning method considering environmental adaptive maneuvers according to the present invention;

[0064] Figure 4 A schematic diagram of high-order MPC trajectory optimization of an aircraft trajectory planning method taking environmental adaptive maneuvers into consideration according to the present invention. DETAILED DESCRIPTION

[0065] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0066] like Figure 1 As shown, the present invention discloses a method for aircraft trajectory planning considering environmental adaptive maneuvers. The method consists of a three-layer framework of a path search method, a low-order MPC trajectory optimization, and a high-order MPC trajectory optimization. The workflow is as follows: the path search method uses the posture information provided by the navigation positioning and the Euclidean signed distance field and occupancy grid map that characterize the environment to plan a discrete path; then, based on the path, the low-order MPC trajectory optimization model is constructed, and a reference trajectory is generated after the optimization; finally, the reference trajectory is used to construct the high-order MPC trajectory optimization model, and a local desired trajectory is generated after the optimization and provided to the underlying controller.

[0067] Furthermore, if Figure 2 As shown, in an unknown and complex environment, a path connecting a starting point and a target point is searched out. The path search method specifically includes the following steps:

[0068] Step A1: Obtain the current position information of the aircraft and the point cloud data of the surrounding environment according to the measurement information of the sensors carried by the aircraft, and establish an occupancy grid map and a Euclidean signed distance field;

[0069] Step A2: Create two tables OpenSet and CloseSet to store nodes in the search process, and add the starting node to OpenSet;

[0070] Step A3: Take the node n with the minimum overall cost from the non-empty OpenSet table min , determine whether it is equal to the target node, if so, obtain a series of discrete path points and end, otherwise obtain n min neighbor nodes, and n min Remove from OpenSet and add to CloseSet;

[0071] The total cost f = λ1g e +λ2g v +λ3g f +g p , where g e Represents the Euclidean distance cost between the current node and the parent node, g v represents the cost of the obstacle-free space volume around the current node, g f represents the heuristic distance cost between the current node and the target point, g p Represents the total cost of the parent node of the current node, λ i (i=1,2,3) represents weight;

[0072] Step A4: Calculate the g of the neighboring nodes of the current node e and g v , using Euclidean distance to calculate g e : where n n,i (i=x,y,z) is the three-dimensional coordinate of the neighbor node of the current node in the inertial system, n c,i (i=x, y, z) is the three-dimensional coordinate of the current node in the inertial system; g v The mathematical expression is where r thr is the safety threshold, r(n n ) is the distance between the neighbor node and the nearest obstacle in the Euclidean signed distance field, nn is the three-dimensional coordinate of the neighbor node in the inertial system;

[0073] Step A5: Calculate the g of the neighboring nodes of the current node f The mathematical expression is:

[0074] Δ=n n -g

[0075]

[0076]

[0077]

[0078] Among them, g is the three-dimensional coordinate of the target node in the inertial system, Δ is the vector difference between the two coordinates, and Δ m is the shortest distance among the three coordinate axes, Δ n is the second shortest distance among the three coordinate axes, Δ {x,y,z}\{m,n} is the longest distance among the three coordinate axis distances, min is the minimum value function, |·| is the absolute value function, and β is the adjustment coefficient;

[0079] Step A6: If the neighbor node is in the CloseSet or passes through an obstacle, it is discarded without processing; otherwise, the following process is continued for the neighbor node:

[0080] (i) If it is not in OpenSet, add it to OpenSet. The current node n c is its parent node; (ii) if the current node is already in the OpenSet, then use f to determine whether this path is a path with a lower cost. If so, update the information of the neighboring nodes;

[0081] Step A7: Repeat the above steps A3-A6 until the end.

[0082] Furthermore, if Figure 3 As shown in the figure, based on the searched discrete path, a low-order MPC optimization model is established to further optimize the spatial position of each path point and generate a smooth and safe reference trajectory.

[0083] The model of low-order MPC trajectory optimization is established as:

[0084]

[0085] stx k+1 =f d (x k ,u k )

[0086] x 0=x0

[0087] u k ∈Ω,x k ∈X

[0088] Among them, J u (·) represents the smooth penalty function, J s (·) represents the penalty function for tracking discrete paths, J c (·) represents the static obstacle avoidance penalty function, J v (·) represents the obstacle-free space volume penalty function around the path point, f d (·) is the state transition function, x k is the kth system state, u k is the kth system input, x0 is the starting state, N is the number of discrete path points, and λ u ,λ s ,λ c ,λ v are weighted coefficients, Ω and X represent the state set.

[0089] In the low-order MPC trajectory optimization model, the smoothing, tracking and static obstacle avoidance cost functions of the j-th reference trajectory are defined as:

[0090] J u (u k+1 ,u k )=||u k+1 -u k || 2

[0091]

[0092]

[0093] Among them, c thr is the safety threshold, c(p k ) is p k The distance to the nearest obstacle in the Euclidean signed distance field, p k is the kth optimized trajectory point, is the kth path point on the discrete path.

[0094] In the low-order MPC trajectory optimization model, the obstacle-free space volume cost function of the j-th reference trajectory is defined as:

[0095]

[0096]

[0097]

[0098]

[0099] Among them, ρ is the coefficient that balances safety, mobility and solution efficiency, w k is the weighted coefficient of the cost of the kth trajectory point, r k is the radius of the spherical safety area around the kth trajectory point, k t is a constant coefficient, p 0 is the starting trajectory point, max{} is the maximum value function, and tanh() is the hyperbolic tangent function.

[0100] Furthermore, if Figure 4 As shown, based on the generated reference trajectory, a model characterizing the environment-adaptive maneuvering behavior is established, and a high-order MPC optimization model is established to generate a safe and dynamically feasible local trajectory for maneuvering adaptation.

[0101] The model of high-order MPC trajectory optimization is established as:

[0102]

[0103]

[0104]

[0105] Among them, J d (·) represents the penalty function that is dynamically feasible, J t (·) represents the penalty function for tracking the reference trajectory, J c (·) represents the static obstacle avoidance penalty function, J e (·) represents the penalty function for environment adaptive maneuvers, f d (·) is the state transfer function, N1 represents the number of optimized trajectory points, x k and θ k is the kth system state, u k and is the kth system input, λ d ,λ t ,λ c ,λ e are weighting coefficients, Ω x and Ω θ is the set of system inputs, X x and X θ A collection of system states.

[0106] In the high-order MPC trajectory optimization model, the cost function of the j-th reference trajectory for dynamic feasibility and tracking the reference trajectory is defined as:

[0107]

[0108]

[0109]

[0110] Among them, f μ () is a piecewise function, d μ is the variable on the μ axis, d max is the upper bound of the variable, d min is the lower bound of the variable, v k represents the velocity vector of the kth trajectory point, a k represents the acceleration vector of the kth trajectory point, j k The vector representing the acceleration derivative of the kth trajectory point, is the kth reference trajectory point, is the velocity vector of the kth reference trajectory point, and γ is used to balance the tracking error and tracking progress.

[0111] In the high-order MPC trajectory optimization model, the cost function of the environment adaptive maneuver of the j-th reference trajectory is defined as:

[0112]

[0113]

[0114]

[0115]

[0116] Among them, <, > represents vector dot product, is the gradient of the Euclidean signed distance field, α1 and α2 are positive coefficients, v thr is the initial safety speed threshold, is the safe speed threshold of the kth trajectory point, η(β) represents the sigmoid function, β represents the cosine function, and ||·|| represents the modulus of the vector.

[0117] The contents not described in detail in the specification of the present invention belong to the prior art known to the professional and technical personnel in the field. It is easy for the technical personnel in the field to understand that the above description is only the preferred embodiment of the present invention and is not intended to limit the present invention. Any modification, equivalent replacement and improvement made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for aircraft trajectory planning considering environmental adaptive maneuvers, characterized in that: Including path search methods, low-order MPC trajectory optimization and high-order MPC trajectory optimization; The path search method uses the position information provided by the navigation positioning and the Euclidean signed distance field and occupancy grid map representing the environment to plan a discrete path in a large obstacle-free space; Subsequently, based on this path, a low-order MPC trajectory optimization model is constructed, and a reference trajectory is generated after solution optimization; Finally, using the reference trajectory, based on the gradient information of the Euclidean signed distance field and the motion state of the aircraft, a maneuver model that characterizes the environmental adaptive behavior is established, and a high-order MPC trajectory optimization model is constructed. After the optimization, a local desired trajectory is generated and provided to the underlying controller.

2. The method for aircraft trajectory planning considering environmental adaptive maneuvers according to claim 1, characterized in that: The path search method performs global planning in the occupancy grid map, specifically comprising the following steps: Step A1: Obtain the current position information of the aircraft and the point cloud data of the surrounding environment according to the measurement information of the sensors carried by the aircraft, and establish an occupancy grid map and a Euclidean signed distance field; Step A2: Create two tables and , used to store nodes during the search process, add the starting node to middle; Step A3: From non-empty Take out the node with the smallest overall cost from the table , determine whether it is equal to the target node, if so, obtain a series of discrete path points and end, otherwise obtain The neighbor nodes of from Remove from and add to middle; Among them, the overall cost ,in Represents the Euclidean distance cost between the current node and the parent node, Represents the cost of the obstacle-free space volume around the current node, represents the heuristic distance cost between the current node and the target point, Represents the total cost of the parent node of the current node, represents weight; ; Step A4: Calculate the neighbor nodes of the current node and , using Euclidean distance calculation : :in is the three-dimensional coordinate of the neighboring node of the current node in the inertial system, is the three-dimensional coordinate of the current node in the inertial system; The mathematical expression is :in is the safety threshold, is the distance between the neighbor node and the nearest obstacle in the Euclidean signed distance field, is the three-dimensional coordinate of the neighbor node in the inertial system; ; Step A5: Calculate the neighbor nodes of the current node The mathematical expression is: ； in, is the three-dimensional coordinate of the target node in the inertial system, is the vector difference of two coordinates, is the shortest distance among the three coordinate axes. is the second shortest distance among the three coordinate axis distances, is the longest distance among the three coordinate axes. is the minimum function, is the absolute value function, is the adjustment coefficient; Step A6: If the neighbor node is If the node is in the middle of a node or passes through an obstacle, it is discarded without any further processing. Otherwise, the following process is continued for the neighboring node: (i) If the node is not in the middle of a node, it is discarded without any further processing. , add it to , current node is its parent node; (ii) if the current node is already In Determine whether this path is a path with a lower cost. If so, update the information of the neighboring nodes. Step A7: Repeat the above steps A3-A6 until the end.

3. The method for aircraft trajectory planning considering environmental adaptive maneuvers according to claim 1, characterized in that: The model of low-order MPC trajectory optimization is established as: ； in, represents a smooth penalty function, represents the penalty function for tracking discrete paths, represents the static obstacle avoidance penalty function, represents the obstacle-free space volume penalty function around the path point, is the state transfer function, For the system status, For the System input, is the starting state, represents the number of discrete path points, are weighting coefficients, and Represents a state collection.

4. The method for aircraft trajectory planning considering environmental adaptive maneuvers according to claim 3, characterized in that: The first The cost function for smoothing, tracking, and static obstacle avoidance of the segment reference trajectory is defined as: ； in, is the safety threshold, for The distance to the nearest obstacle in the Euclidean signed distance field, For the Optimized trajectory points, is the discrete path waypoints.

5. The method for aircraft trajectory planning considering environmental adaptive maneuvers according to claim 4, characterized in that: The first The cost function of the obstacle-free space volume of the segment reference trajectory is defined as: ； in, To balance the coefficients of safety, maneuverability and solution efficiency, For the The weighted coefficient of the cost of each trajectory point, For the The radius of the spherical safety zone around the trajectory points, is a constant coefficient, is the starting point of the trajectory, is the maximum value function, is the hyperbolic tangent function.

6. The method for aircraft trajectory planning considering environmental adaptive maneuvers according to claim 1, characterized in that: The model of high-order MPC trajectory optimization is established as: ； in, represents the penalty function that is dynamically feasible, represents the penalty function for tracking the reference trajectory, represents the static obstacle avoidance penalty function, represents the penalty function for environment-adaptive maneuvers, is the state transfer function, represents the number of optimized trajectory points, and For the system status, and For the System input, are weighting coefficients, and is the set of system inputs, and A collection of system states.

7. The method for aircraft trajectory planning considering environmental adaptive maneuvers according to claim 6, characterized in that: The first The dynamic feasibility of the segment reference trajectory and the cost function of tracking the reference trajectory are defined as: ； in, is a piecewise function, for The variables on the axis, is the upper bound of the variable, is the lower bound of the variable, Indicates The velocity vector of each trajectory point is Indicates The acceleration vector of each trajectory point, Indicates The vector of the acceleration derivatives of the trajectory points, For the Reference trajectory points, For the The velocity vector of the reference trajectory point, Used to balance tracking error and tracking progress.

8. The method for aircraft trajectory planning considering environmental adaptive maneuvers according to claim 6, characterized in that: The first The cost function of the environment-adaptive maneuver for a segment reference trajectory is defined as: ； in, represents vector dot product, is the gradient of the Euclidean signed distance field, and is a positive coefficient, is the initial safety speed threshold, For the The safe speed threshold of each trajectory point is represents the sigmoid function, represents the cosine function, Represents the magnitude of a vector.

Images