Track planning method and device, aircraft and computer readable storage medium

Abstract

The invention discloses a trajectory planning method and device, an aircraft and a computer readable storage medium, and the method comprises the steps: generating a key path point set based on a static obstacle corresponding to the aircraft, a local trajectory of a previous frame and a global navigation path; determining a control point set of a cubic B-spline curve based on a user set speed, a key path point set and a cubic B-spline primary function corresponding to the aircraft; determining a dynamic obstacle avoidance penalty function based on the dynamic obstacle and the control point set, determining a static obstacle avoidance penalty function based on the obstacle avoidance path point set, and determining an optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function; and generating a target trajectory corresponding to the aircraft based on the optimization model and the current flight parameters of the aircraft. The target of avoiding static threats and dynamic threats is achieved, the accuracy of the flight path is improved, and the stability, comfort and safety of flight of the aircraft are improved.

Description

Technical Field

[0001] The present application relates to the field of aircraft technology, and in particular to a trajectory planning method, device, aircraft, and computer-readable storage medium. Background Art

[0002] At present, in the field of multi-rotor aircraft, the existence of application scenarios with non-cooperative static and dynamic obstacles poses a huge challenge to the autonomous and safe flight of flying cars, and the limited maneuverability of the aircraft makes it more difficult to avoid obstacles. Therefore, it is necessary to accurately plan the trajectory of the aircraft.

[0003] In the related technology, trajectory planning can be divided into sampling-based, search-based, optimization-based and data-based methods. Among them, the sampling-based method is to randomly scatter points in the configuration space, connect them with line segments to construct a route map or route tree, and then search for a path with the best cost, but the sampling and node update strategies affect the time efficiency and path quality, and it is difficult to handle dynamic obstacles; the search-based method is to discretize the state space into a three-dimensional grid map according to a certain resolution, and use a heuristic search algorithm to obtain a feasible path or an optimal path, but the resolution and heuristic search algorithm affect the search efficiency, it is difficult to handle continuous space and has high spatial and temporal complexity; the optimization-based method is to establish a multi-constrained optimization model and use the gradient descent method to solve the optimal trajectory. The goals and constraints are clearly defined and have a wide range of applicability, but non-convex optimization problems are time-consuming and the quality of the solution is greatly affected by the initial value; the data-based method is to construct a deep neural network large model through deep learning and reinforcement learning methods and use a large amount of data for training, and then input the aircraft state and environmental information into the large model to generate a trajectory. It has strong adaptability and generalization ability, but its trajectory quality is greatly affected by data quality and the model interpretability is poor.

[0004] Therefore, how to improve the accuracy of the aircraft's flight trajectory is a problem that urgently needs to be solved. Summary of the invention

[0005] The main purpose of this application is to provide a trajectory planning method, device, aircraft and computer-readable storage medium, aiming to solve the technical problem of how to improve the accuracy of the aircraft's flight trajectory.

[0006] To achieve the above objectives, the present application provides a trajectory planning method, comprising:

[0007] Generate a key path point set based on the static obstacle corresponding to the aircraft, the local trajectory of the previous frame, and the global navigation path, wherein the key path point set includes an obstacle avoidance path point set corresponding to the static obstacle;

[0008] Determining a control point set of a cubic B-spline curve based on a user-set speed corresponding to the aircraft, a key path point set, and a cubic B-spline basis function;

[0009] Based on the dynamic obstacles and the control point set, a dynamic obstacle avoidance penalty function is determined, based on the obstacle avoidance path point set, a static obstacle avoidance penalty function is determined, and an optimization model is determined based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function;

[0010] Based on the optimization model and the current flight parameters of the aircraft, a target trajectory corresponding to the aircraft is generated.

[0011] In addition, to achieve the above-mentioned purpose, the present application also provides an aircraft, the aircraft comprising:

[0012] A first generating module is used to generate a key path point set based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path, wherein the key path point set includes an obstacle avoidance path point set corresponding to the static obstacles;

[0013] A first determination module is used to determine a control point set of a cubic B-spline curve based on a user-set speed corresponding to the aircraft, a key path point set, and a cubic B-spline basis function;

[0014] A second determination module is used to determine a dynamic obstacle avoidance penalty function based on a dynamic obstacle and a control point set, determine a static obstacle avoidance penalty function based on an obstacle avoidance path point set, and determine an optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function;

[0015] The second generating module is used to generate a target trajectory corresponding to the aircraft based on the optimization model and the current flight parameters of the aircraft.

[0016] In addition, to achieve the above-mentioned purpose, the present application also provides a trajectory planning device, which includes: a memory, a processor, and a trajectory planning program stored in the memory and executable on the processor, and the trajectory planning program implements the steps of the aforementioned trajectory planning method when executed by the processor.

[0017] In addition, to achieve the above-mentioned purpose, the present application also provides a computer-readable storage medium, on which a trajectory planning program is stored, and when the trajectory planning program is executed by a processor, the steps of the aforementioned trajectory planning method are implemented.

[0018] The present application generates a key path point set based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path, and then determines the control point set of the cubic B-spline curve based on the user-set speed corresponding to the aircraft, the key path point set and the cubic B-spline basis function; then determines the dynamic obstacle avoidance penalty function based on the dynamic obstacles and the control point set, determines the static obstacle avoidance penalty function based on the obstacle avoidance path point set, and determines the optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function; then generates the target trajectory corresponding to the aircraft based on the optimization model and the current flight parameters of the aircraft; generates a safe topological path by using an obstacle avoidance path for static threats to establish a static obstacle avoidance penalty function, uses the data of dynamic obstacles to establish a dynamic obstacle avoidance penalty function that can reduce the impact of downwash interference for dynamic threats, and constructs a multi-constrained optimization model through static and dynamic obstacle avoidance requirements to generate a local trajectory for safe execution, thereby achieving the goal of avoiding static and dynamic threats, improving the accuracy of the flight trajectory, and improving the stability, comfort and safety of the aircraft flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0020] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0021] Figure 1 A schematic diagram of a process flow provided for the first embodiment of the trajectory planning method of the present application;

[0022] Figure 2 A schematic diagram of a scenario of a backup intersection in an embodiment of the trajectory planning method of this application;

[0023] Figure 3 A schematic diagram of a scene of a key path point set in an embodiment of the trajectory planning method of the present application;

[0024] Figure 4 A schematic diagram of a scenario provided in yet another embodiment of the trajectory planning method of the present application;

[0025] Figure 5 A schematic diagram of a scenario provided in another embodiment of the trajectory planning method of the present application;

[0026] Figure 6 A schematic diagram of a scenario provided in yet another embodiment of the trajectory planning method of the present application;

[0027] Figure 7 This is a schematic diagram of the module structure of the aircraft in this application;

[0028] Figure 8 Schematic diagram of the module structure of the trajectory planning device according to an embodiment of the present application.

[0029] The realization of the purpose, functional features and advantages of this application will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0030] It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0031] In order to better understand the technical solution of the present application, a detailed description will be given below in conjunction with the accompanying drawings and specific implementation methods.

[0032] The main solution of the present application is: based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path, a key path point set is generated, wherein the key path point set includes the obstacle avoidance path point set corresponding to the static obstacles; based on the user-set speed corresponding to the aircraft, the key path point set and the cubic B-spline basis function, a control point set of the cubic B-spline curve is determined; based on the dynamic obstacles and the control point set, a dynamic obstacle avoidance penalty function is determined, based on the obstacle avoidance path point set, a static obstacle avoidance penalty function is determined, and an optimization model is determined based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function; based on the optimization model and the current flight parameters of the aircraft, a target trajectory corresponding to the aircraft is generated.

[0033] At present, in the field of multi-rotor aircraft, the existence of application scenarios with non-cooperative static and dynamic obstacles poses a huge challenge to the autonomous and safe flight of flying cars, and the limited maneuverability of the aircraft makes it more difficult to avoid obstacles. Therefore, it is necessary to accurately plan the trajectory of the aircraft.

[0034] In the related technology, trajectory planning can be divided into sampling-based, search-based, optimization-based and data-based methods. Among them, the sampling-based method is to randomly scatter points in the configuration space, connect them with line segments to construct a route map or route tree, and then search for a path with the best cost, but the sampling and node update strategies affect the time efficiency and path quality, and it is difficult to handle dynamic obstacles; the search-based method is to discretize the state space into a three-dimensional grid map according to a certain resolution, and use a heuristic search algorithm to obtain a feasible path or an optimal path, but the resolution and heuristic search algorithm affect the search efficiency, it is difficult to handle continuous space and has high spatial and temporal complexity; the optimization-based method is to establish a multi-constrained optimization model and use the gradient descent method to solve the optimal trajectory. The goals and constraints are clearly defined and have a wide range of applicability, but non-convex optimization problems are time-consuming and the quality of the solution is greatly affected by the initial value; the data-based method is to construct a deep neural network large model through deep learning and reinforcement learning methods and use a large amount of data for training, and then input the aircraft state and environmental information into the large model to generate a trajectory. It has strong adaptability and generalization ability, but its trajectory quality is greatly affected by data quality and the model interpretability is poor. Therefore, how to improve the accuracy of the aircraft's flight trajectory is a problem that urgently needs to be solved.

[0035] The present application generates a safe topological path by adopting an obstacle-avoiding path for static threats to establish a static obstacle avoidance penalty function, and uses the data of dynamic obstacles to establish a dynamic obstacle avoidance penalty function that can reduce the impact of downwash interference for dynamic threats. A multi-constraint optimization model is constructed through static and dynamic obstacle avoidance requirements to generate a local trajectory for safe execution, thereby achieving the goal of avoiding static and dynamic threats, improving the accuracy of the flight trajectory, and improving the stability, comfort and safety of the aircraft's flight.

[0036] It should be noted that the execution subject of this embodiment may be a trajectory planning device, or a computing service device with data processing, network communication and program running functions, such as a tablet computer, a personal computer, a mobile phone, etc., or a trajectory planning device capable of realizing the above functions, etc., and this embodiment does not specifically limit this. The following takes the trajectory planning device as the execution subject as an example to illustrate this embodiment and the following embodiments.

[0037] Based on this, this application proposes a trajectory planning method of the first embodiment, please refer to Figure 1 , the trajectory planning method includes steps S101 to S104:

[0038] Step S101, generating a key path point set based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame, and the global navigation path, wherein the key path point set includes an obstacle avoidance path point set corresponding to the static obstacles;

[0039] In the present application, when performing trajectory planning, the static obstacle information of the static obstacle, the local trajectory of the previous frame corresponding to the aircraft, and the global navigation path are obtained based on the perception module of the aircraft, and a key path point set is generated based on the obtained parameters. Specifically, the current position and current speed corresponding to the aircraft are obtained, and the global navigation path of the aircraft and the local trajectory of the previous frame are obtained at the same time. Based on the current position, current speed, local trajectory of the previous frame and global navigation path, a backup path (Fallback path) is determined, the static obstacle information of the static obstacle corresponding to the aircraft is obtained, and according to the static obstacle information and the backup path, the obstacle bypass path point set corresponding to the static obstacle (the set formed by the points in the obstacle bypass path) is determined, and the key path point set is generated according to the obstacle bypass path point set and the initial path point set corresponding to the backup path, and the initial path point set is the set formed by the path points in the backup path.

[0040] Step S102, determining a control point set of a cubic B-spline curve based on a user-set speed corresponding to the aircraft, a key path point set, and a cubic B-spline basis function;

[0041] After obtaining the key path point set, the user-set speed and the cubic B-spline basis function are obtained, and based on the user-set speed, the key path point set and the cubic B-spline basis function, the control point set of the cubic B-spline curve is determined. Specifically, the target limit speed and the key path point moment corresponding to each key path point in the key path point set are first determined according to the user-set speed, and then the control point set of the cubic B-spline curve is determined based on the cubic B-spline basis function, the target limit speed corresponding to each key path point and the key path point moment.

[0042] Step S103, determining a dynamic obstacle avoidance penalty function based on the dynamic obstacle and the control point set, determining a static obstacle avoidance penalty function based on the obstacle avoidance path point set, and determining an optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function;

[0043] After obtaining the control point set of the cubic B-spline curve, the dynamic obstacle avoidance penalty function is determined based on the dynamic obstacles and the control point set. Specifically, the minimum and maximum times corresponding to each dynamic obstacle are determined based on the predicted trajectory of each dynamic obstacle and the aircraft trajectory corresponding to the key path point set. The dynamic obstacle avoidance penalty function is determined based on the current position of the aircraft, the dynamic obstacle, the minimum time and the maximum time.

[0044] At the same time, a static obstacle avoidance penalty function is determined based on the obstacle avoidance path point set. In a feasible implementation, step S103 may further include steps S1031 to S1033:

[0045] Step S1031, based on the obstacle surface points and the movement direction in the obstacle avoidance path point set, as well as the key path points and the safety distance, determine the first static parameters corresponding to the key path points;

[0046] Step S1032, determining a second static parameter corresponding to each key path point based on the first static parameter, each key path point and the corresponding movement direction;

[0047] Step S1033: determining a static obstacle avoidance penalty function based on the second static parameter.

[0048] When determining the static obstacle avoidance penalty function, the obstacle surface points and movement directions in the obstacle avoidance path point set are obtained, and the first static parameter is determined based on the obstacle surface points and movement directions in the obstacle avoidance path point set, as well as each key path point and the safety distance.

[0049] Obtain each key path point and the corresponding movement direction, and determine the second static parameter based on the first static parameter, each key path point and the corresponding movement direction, and determine the static obstacle avoidance penalty function based on the second static parameter corresponding to each static obstacle information, wherein the specific formula is:

[0050]

[0051] d so =(P i -P surf，i ) T V surf，i )-d safe

[0052]

[0053]

[0054] Among them, J so is the static obstacle avoidance penalty function, P i is the i-th key path point, Q j is the jth control point in the control point set, N j,3 (.) is the cubic B-spline basis function, d so is the first static parameter, P surf,i is the obstacle surface point, i.e., the i-th key path point, V surf,i is the critical path point P surf,i The corresponding direction of motion, d safe is the safe distance, G i It is the second static parameter.

[0055] After the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function are obtained, an optimization model is determined based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function. Further, in a feasible implementation, step S103 may also include steps S1031 to S1032:

[0056] Step S1031, determining a trajectory smoothness penalty function based on a set of key path points, and determining a dynamic feasibility penalty function based on a set of control points of a cubic B-spline curve;

[0057] Step S1032, determining an optimization model based on the dynamic obstacle avoidance penalty function, the static obstacle avoidance penalty function, the trajectory smoothness penalty function and the dynamic feasibility penalty function.

[0058] After obtaining the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function, the aircraft determines the trajectory smoothness penalty function based on the key path point set, and specifically calculates the trajectory smoothness penalty function according to the acceleration of each key path point. Further, in a feasible implementation, step S1031 may also include step a:

[0059] Step a: determining a trajectory smoothness penalty function based on the jerk corresponding to each key path point in the key path point set.

[0060] Specifically, the jerk corresponding to each key path point in the key path point set is obtained, and the trajectory smoothness penalty function is calculated based on each jerk, wherein the formula of the trajectory smoothness penalty function is:

[0061]

[0062] Among them, J s is the trajectory smoothness penalty function, j i is the jerk corresponding to the i-th critical path point, n is the number of control points in the control point set, and n-3=l is the number of critical path points.

[0063] At the same time, a dynamic feasibility penalty function is determined based on the control point set of the cubic B-spline curve, and specifically the three-dimensional velocity, Z-axis velocity, three-dimensional acceleration, three-dimensional and velocity upper limit, Z-axis maximum velocity and three-axis combined acceleration maximum value corresponding to each control point in the control point set are obtained, and the dynamic feasibility penalty function is calculated based on the obtained parameters. Further, in a feasible implementation, step S1031 may also include steps b1 to b2:

[0064] Step b1, obtaining the 3D velocity, Z-axis velocity, 3D acceleration, 3D and velocity upper limit, Z-axis maximum velocity and three-axis combined acceleration maximum value corresponding to each control point in the control point set;

[0065] Step b2, determining a first penalty function based on the three-dimensional velocity and the three-dimensional velocity upper limit, determining a second penalty function based on the Z-axis velocity and the Z-axis maximum velocity, and determining a third penalty function based on the three-dimensional acceleration and the maximum value of the three-axis combined acceleration;

[0066] Step b3: determining a dynamic feasibility penalty function based on the first penalty function, the second penalty function and the third penalty function.

[0067] When determining the dynamic feasibility penalty function, the three-dimensional velocity, Z-axis velocity, three-dimensional acceleration, three-dimensional and velocity upper limits, Z-axis maximum velocity, and three-axis combined acceleration maximum value corresponding to each control point in the control point set are obtained.

[0068] Next, the first penalty function is determined based on the three-dimensional speed and the three-dimensional and speed upper limits, the second penalty function is determined based on the Z-axis speed and the maximum Z-axis speed, and the third penalty function is determined based on the three-dimensional acceleration and the maximum value of the three-axis combined acceleration; and based on the first penalty function, the second penalty function and the third penalty function, the dynamic feasibility penalty function is determined, and the aircraft Z-axis tracking capability limitation is considered in the speed decision and the three-axis combined speed and Z-axis speed penalty functions are established to ensure that the aircraft can track the optimized trajectory of steep rise and fall in altitude; the specific formula is:

[0069]

[0070]

[0071]

[0072] J d =J υ +J υ，z +J a ;

[0073] Among them, J d is the dynamic feasibility penalty function, J v is the first penalty function, J v,z is the second penalty function, J a is the third penalty function, v i is the three-dimensional velocity of the ith control point in the control point set, v upper,i is the three-dimensional acceleration of the ith control point in the control point set, v z,i is the Z-axis velocity of the ith control point in the control point set, v max,z is the maximum speed of Z axis, a i is the three-dimensional acceleration of the ith control point in the control point set, a max is the maximum value of the three-axis combined acceleration.

[0074] After obtaining the dynamic feasibility penalty function, the optimization model is determined based on the dynamic obstacle avoidance penalty function, static obstacle avoidance penalty function, trajectory smoothness penalty function and dynamic feasibility penalty function. The penalty functions of path smoothness and dynamic feasibility are established based on non-uniform B-spline control points, and a multi-constrained optimization model is constructed in combination with static and dynamic obstacle avoidance penalty items to generate a smooth, safe and executable local trajectory, thereby improving the stability and comfort of flight. The specific formula of the optimization model is:

[0075] J=w s J s +w d J d +w do J do +w so J so ;

[0076] Among them, J is the optimization model, J s is the trajectory smoothness penalty function, J d is the dynamic feasibility penalty function, J do is the dynamic obstacle avoidance penalty function, J so is the static obstacle avoidance penalty function, w s 、w d 、w do 、w so They are the weight coefficients corresponding to the trajectory smoothness penalty function, dynamic feasibility penalty function, dynamic obstacle avoidance penalty function and static obstacle avoidance penalty function respectively.

[0077] Step S104: generating a target trajectory corresponding to the aircraft based on the optimization model and the current flight parameters of the aircraft.

[0078] After obtaining the optimization model, the current flight parameters of the aircraft are obtained, and the corresponding target trajectory of the aircraft is generated based on the current flight parameters and the optimization model. Specifically, the current flight parameters are calculated by a general calculation method in related technologies to obtain relevant parameters of the optimization model, and the corresponding target trajectory of the aircraft is generated based on the optimization model through the relevant parameters.

[0079] It should be noted that after the target trajectory is generated, the speed and acceleration of the optimal control point corresponding to the target trajectory are determined, and it is determined whether the speed and acceleration of the optimal control point meet the corresponding constraints. If so, the target trajectory is output, and the target trajectory is sampled to obtain multiple local trajectory sampling points, and it is determined whether each local trajectory sampling point meets the safety distance constraints with static obstacles and dynamic obstacles in time and space. If all are satisfied, the aircraft is controlled to fly based on the target trajectory. If there are local trajectory sampling points that do not meet the safety distance constraints of static obstacles or the safety distance constraints of dynamic obstacles, a braking trajectory is generated and sent to the control module of the aircraft for execution. If the speed and acceleration of the optimal control point do not meet the corresponding constraints, the reallocation time interval is obtained, and the time node vector U of the control point set Q is adjusted based on the reallocation time interval. For example, if the reallocation time interval is Δt, the adjusted U=[t0+Δt,t1+Δt,…,t m-1 +Δt,t m +Δt], and returns to step S102 to obtain a new target trajectory.

[0080] A key path point set is generated based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path, and then a control point set of a cubic B-spline curve is determined based on the user-set speed corresponding to the aircraft, the key path point set and the cubic B-spline basis function; then a dynamic obstacle avoidance penalty function is determined based on the dynamic obstacles and the control point set, a static obstacle avoidance penalty function is determined based on the obstacle avoidance path point set, and an optimization model is determined based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function; then a target trajectory corresponding to the aircraft is generated based on the optimization model and the current flight parameters of the aircraft; a safe topological path is generated by using an obstacle avoidance path for static threats to establish a static obstacle avoidance penalty function, a dynamic obstacle avoidance penalty function that can reduce the influence of downwash interference is established by using the data of dynamic obstacles for dynamic threats, and a multi-constrained optimization model is constructed by static and dynamic obstacle avoidance requirements to generate a local trajectory for safe execution, thereby achieving the goal of avoiding static and dynamic threats, improving the accuracy of the flight trajectory, and improving the stability, comfort and safety of the aircraft flight.

[0081] In a feasible implementation manner, step S101 may further include steps S201 to S202:

[0082] Step S201, determining an alternate path based on the current position, current speed, local trajectory of the previous frame and global navigation path corresponding to the aircraft;

[0083] Step S202: Generate a critical path point set based on the static obstacles corresponding to the aircraft and the initial path point set corresponding to the backup path.

[0084] When performing trajectory planning, the current position and current speed of the aircraft are obtained, and the global navigation path of the aircraft and the local trajectory of the corresponding previous frame are obtained. Based on the current position, current speed, the local trajectory of the previous frame and the global navigation path, a fallback path is determined. Specifically, in a feasible implementation, step S201 may include steps S2011 to S2013:

[0085] Step S2011, determining a first projection point in the local trajectory of the previous frame based on the current position and current speed of the aircraft;

[0086] Step S2012, determining the second projection point corresponding to the last local trajectory point in the local trajectory of the previous frame in the global navigation path of the aircraft;

[0087] Step S2013, determining the backup path based on the first projection point, the second projection point, the local trajectory of the previous frame, and the global navigation path.

[0088] After obtaining the current position, current speed and the local trajectory of the previous frame, the current position is projected onto the local trajectory of the previous frame according to the current position and current speed, and the first projection point corresponding to the current aircraft is determined in the local trajectory of the previous frame.

[0089] At the same time, the last local track point in the local track of the previous frame is obtained, and the second projection point corresponding to the last local track point is determined in the global navigation path, wherein the second projection point may be the intersection point between the local track of the previous frame and the global navigation path.

[0090] After obtaining the first projection point and the second projection point, the backup path is determined based on the first projection point, the second projection point, the local trajectory of the previous frame and the global navigation path, such as Figure 2 As shown in the figure, the issued curve is the global navigation path, the middle yellow curve is the local trajectory of the previous frame, and the path corresponding to the purple discrete point set is the backup path. When generating the backup path, the path length limit of the aircraft is considered, that is, the backup path is less than or equal to the path length limit.

[0091] After the backup path is obtained, the static obstacle information of the static obstacle corresponding to the aircraft is obtained, and the obstacle bypassing path point set corresponding to the static obstacle (the set formed by the points in the obstacle bypassing path) is determined according to the static obstacle information and the backup path, and the key path point set is generated according to the obstacle bypassing path point set and the initial path point set corresponding to the backup path, where the initial path point set is the set formed by the path points in the backup path. Specifically, in a feasible implementation manner, step S202 may include steps S2021 to S2024:

[0092] Step S2021, sampling the backup paths according to the distances of the key path points to obtain an initial path point set;

[0093] Step S2022, based on the static grid map corresponding to the static obstacle information, determining whether there is a collision segment in the initial path point set;

[0094] Step S2023, determining an obstacle avoidance path point set based on the collision segment, wherein the obstacle avoidance path point set includes obstacle surface points and corresponding movement directions;

[0095] Step S2024, generating the key path point set based on the obstacle avoidance path point set and the initial path point set.

[0096] After obtaining the backup path, the corresponding key path point distance is obtained, and the backup path is sampled according to the key path point distance to obtain an initial path point set, that is, discrete initial path points are generated to obtain an initial path point set.

[0097] After obtaining the initial path point set, obtain the static obstacle information of the static obstacle corresponding to the aircraft, establish a static grid map according to the static obstacle information, and determine whether there is a collision segment in the initial path point set according to the static grid map. Specifically, check whether each initial path point in the initial path point set is in the static grid map corresponding to the static obstacle. If there is an initial path point in the initial path point set that is in the static grid map, connect the initial path points in the static grid map to form a collision segment, and determine the obstacle avoidance path point set based on the collision segment. The obstacle avoidance path point set includes obstacle surface points and corresponding movement directions, such as Figure 3 As shown in the figure, the dotted line is the obstacle avoidance path formed by the obstacle avoidance path point set, the red solid points in the dotted line are the obstacle avoidance path points corresponding to each initial path point in the collision segment, the green curve is the backup path, and the arrows corresponding to the red solid points are the movement directions corresponding to the obstacle avoidance path points.

[0098] Next, based on the obstacle avoidance path point set and the initial path point set, a key path point set is generated, the key path point set including the obstacle avoidance path points in the obstacle avoidance path point set and the path points in the initial path point set except the initial path points included in the collision segment.

[0099] By detecting the collision segment and searching for the safe obstacle avoidance path corresponding to the collision segment in the static occupancy grid map, the time consumption is reduced. The point set of "obstacle surface point-motion direction" (obstacle avoidance path point set) is used to establish a static obstacle avoidance penalty function to achieve the goal of avoiding static threats.

[0100] The trajectory planning method proposed in this embodiment determines an alternative path based on the current position, current speed, local trajectory of the previous frame and global navigation path corresponding to the aircraft; then generates a key path point set based on the static obstacles corresponding to the aircraft and the initial path point set corresponding to the alternative path, and obtains the key path point set by detecting the collision segment and searching for the safe obstacle avoidance path corresponding to the collision segment in the static occupancy grid map, thereby improving the accuracy of the flight trajectory and further improving the stability, comfort and safety of the aircraft flight.

[0101] In a feasible implementation, step S102 may include steps S301 to S302:

[0102] Step S301, based on the user set speed, determining the target speed limit and the critical path point time corresponding to each key path point in the key path point set;

[0103] Step S302 , determining a control point set of a cubic B-spline curve based on a cubic B-spline basis function, a target speed limit corresponding to each key path point, and the key path point time.

[0104] After the key path point set is obtained, the user set speed is obtained, and based on the user set speed, the target speed limit and the key path point time corresponding to each key path point in the key path point set are determined, wherein the target speed limit is the speed upper limit corresponding to each key path point, and the key path point time is the time when the aircraft reaches each key path point obtained according to the target speed limit. Specifically, in a feasible implementation, step S301 may include steps S3011 to S3014:

[0105] Step S3011, determining a first upper speed limit corresponding to each key path point based on the user-set speed and the upper speed limit of steep ascent and descent;

[0106] Step S3012, determining the speed upper limit of the current key path point based on the speed upper limit of the previous key path point in the first speed upper limit and the corresponding acceleration upper limit, and obtaining the second speed upper limit corresponding to each key path point;

[0107] Step S3013, based on the speed upper limit of the next key path point in the second speed upper limit and the corresponding acceleration upper limit, determine the speed upper limit of the current key path point, and obtain the target speed limit corresponding to each key path point;

[0108] Step S3014, determining a time set based on the key path point distance and the target limit speed, wherein the time set includes the key path point moments corresponding to each key path point.

[0109] After obtaining the user-set speed, obtain the steep rise and steep fall speed upper limit, and determine the first speed upper limit corresponding to each key path point based on the user-set speed and the steep rise and steep fall speed upper limit, such as Figure 4 As shown, Figure 4 The speed upper limit of the steep rise and steep fall calculation is the steep rise and steep fall speed upper limit. The yellow curve is the user-set speed. For each key path point, the first speed upper limit of the key path point is less than or equal to the steep rise and steep fall speed upper limit, and the first speed upper limit of the key path point is less than or equal to the user-set speed.

[0110] Next, based on the speed upper limit and the corresponding acceleration upper limit of the previous key path point in the first speed upper limit, the speed upper limit of the next key path point is determined, and the second speed upper limit corresponding to each key path point is obtained; specifically, for each key path point in the key path points, the speed upper limit of the previous key path point is obtained in the first speed upper limit, and the speed upper limit of the current key path point is calculated based on the speed upper limit of the previous key path point and the acceleration upper limit of the aircraft, and the second speed upper limit is obtained, such as Figure 5 shown.

[0111] After obtaining the second speed limit corresponding to each key path point, the speed limit of the current key path point is determined based on the speed limit of the next key path point in the second speed limit and the corresponding acceleration limit, and the target speed limit corresponding to each key path point is obtained. Specifically, for each key path point in the key path points, the speed limit of the next key path point is obtained in the second speed limit, and the speed limit of the current key path point is calculated based on the speed limit of the next key path point and the acceleration limit of the aircraft, and the target speed limit is obtained, such as Figure 6 shown.

[0112] After obtaining the target speed limit, the time set is determined based on the key path point distance and the target speed limit. Specifically, for each key path point, for each key path point in the key path points, the key path point moment corresponding to the key path point is calculated according to the speed limit of the previous key path point in the target speed limit, the speed limit of the key path point and the key path point distance, and then the time set is obtained through the key path point moments corresponding to each key path point.

[0113] After obtaining the target speed limit corresponding to each key path point and the key path point time, obtain the cubic B-spline basis function, and determine the control point set of the cubic B-spline curve based on the cubic B-spline basis function, the target speed limit corresponding to each key path point and the key path point time. Specifically, in a feasible implementation, step S302 may include steps S3021 to S3022:

[0114] Step S3021, based on the target speed limit, obtaining a first speed and a first acceleration of a first key path point in the key path point set, and a second speed and a second acceleration of a last key path point in the key path point set;

[0115] Step S3022, determining a control point set of a cubic B-spline curve based on a set of key path points, a first velocity, a first acceleration, a second velocity and a second acceleration at key path points.

[0116] After obtaining the target speed limit, the first speed V0 of the first key path point in the key path point set and the second speed V0 of the last key path point in the key path point set are obtained based on the target speed limit. l , and obtain the first acceleration A0 of the first key path point in the key path point set, and the second acceleration A of the last key path point in the key path point set l .

[0117] Next, based on the key path point set, the first speed V0, the first acceleration A0, and the second speed V l With the second acceleration A l , determine the control point set Q of the cubic B-spline curve, where the cubic B-spline basis function N i,3 (t) is:

[0118]

[0119] The control point set of the cubic B-spline curve is Q = [Q1, Q2, ..., Q n-1 ,Q n ], the time node vector of the control point set Q is U=[t0,t1,…,t m-1 ,t m ], the key path point set is P = [P1, P2, ..., P l-1 ,P l ], the time set is T=[T1,T2,…,T l-1 ,T l ], where n = l + 2, m = n + 4, T i =t i+4 -t i+3 ,but:

[0120]

[0121] Then the non-uniform B-spline parameterization equation can be obtained as:

[0122]

[0123] The above formula can be written as BQ = P. The matrix B can be obtained by Pi, V0, A0, V l With Al The control point set Q of the cubic B-spline curve can be obtained by solving the equation using QR decomposition.

[0124] The trajectory planning method proposed in this embodiment determines the target limit speed and the critical path point moment corresponding to each key path point in the key path point set based on the user-set speed; then determines the control point set of the cubic B-spline curve based on the cubic B-spline basis function, the target limit speed and the critical path point moment corresponding to each key path point, and uses the theory of non-uniform B-spline parameterization and the generated high-quality control point set as the initial solution, thereby improving the speed of solving the optimization problem and the quality of generating the trajectory, thereby improving the accuracy of the flight trajectory and further improving the stability, comfort and safety of the aircraft flight.

[0125] In a feasible implementation, step S103 may include steps S401 to S403:

[0126] Step S401, based on the first start time and the first trajectory duration corresponding to the predicted trajectory of each dynamic obstacle, and the second start time and the second trajectory duration corresponding to the aircraft trajectory of the key path point set, respectively determine the minimum time and the maximum time corresponding to each dynamic obstacle;

[0127] Step S402, determining the collision cost corresponding to each dynamic obstacle based on the current position of the aircraft, the minimum time, the maximum time, the bounding box corresponding to each dynamic obstacle, and the safety distance;

[0128] Step S403: determining the dynamic obstacle avoidance penalty function based on the collision cost corresponding to each dynamic obstacle.

[0129] After obtaining the control point set of the cubic B-spline curve, the size information and predicted trajectory of the dynamic obstacle are obtained through the perception module of the aircraft. Based on the predicted trajectory of each dynamic obstacle, the corresponding first start time and first trajectory duration are obtained. For example, there are K dynamic obstacles at present. For the kth dynamic obstacle, k∈[1,K], at the first start time t k,start The corresponding predicted trajectory Φ k , the first trajectory duration is T k , the trajectory ends at time t k,end =T k -t k,start The key path point set corresponding to the aircraft trajectory can be the aircraft at the second starting time t o,start The generated trajectory, that is, the aircraft at the second starting time t o,start The generated trajectory is the aircraft trajectory Φ o , the duration of its second trajectory is T o, the trajectory ends at time t o,end =T o -t o,start Specifically, in a feasible implementation, step S401 may include steps S4011 to S4012:

[0130] Step S4011, based on the first start time and the second start time, respectively determine the minimum time corresponding to each dynamic obstacle;

[0131] Step S4012: Determine the maximum time corresponding to each dynamic obstacle based on the first start time, the first trajectory duration, the second start time, and the second trajectory duration.

[0132] For the kth dynamic obstacle, there are three situations between it and the aircraft: (1) t k,end ≤t ostart or kstart >t oend , then there is no intersection between the kth dynamic obstacle and the aircraft, and thus there is no collision risk; (2), t o,start ≤t k,start And t k,start <t o,end And t k，end ≥t o,end , or, t o,start ≤t k,start And t k，start <t o，end And t k，ennd <t o，end , then the minimum time t min =t k，start , maximum time t max =min{t o，end , t k,end}; (3), t o，start ≥t k，start And t o，start <t k，ennd And t k,end ≥t o,end , or, t o,start ≥t k，start And t o,start <t k，end And t k，end <t o,end , then the minimum time t min =t o,start , maximum time t max =min{t o,end , t k，end}.

[0133] Then, after obtaining the first start time, the second start time, the second start time and the second trajectory duration, the minimum time t corresponding to each dynamic obstacle is determined based on the first start time and the second start time. min Based on the first start time, the first trajectory duration, the second start time and the second trajectory duration, determine the maximum time t corresponding to each dynamic obstacle max , where t min =max{t o，start , t k,star}, t max =min{t o，start +T o ,t k，star +T k}.

[0134] After obtaining the minimum time and maximum time corresponding to each dynamic obstacle, the collision cost corresponding to each dynamic obstacle is determined based on the current position of the aircraft, the minimum time, the maximum time, the bounding box corresponding to each dynamic obstacle, and the safety distance. Specifically, in a feasible implementation, step S402 may include steps S4021 to S4023:

[0135] Step S4021, based on the current position of the aircraft and the bounding boxes corresponding to the dynamic obstacles, respectively determine the closest points between the aircraft and the dynamic obstacles, and determine the three-dimensional relative position information between the closest points and the current position of the aircraft;

[0136] Step S4022, determining dynamic parameters based on the three-dimensional relative position information and the safety distance;

[0137] Step S4023: determining the collision cost corresponding to each dynamic obstacle based on the dynamic parameters, minimum time and maximum time of each dynamic obstacle.

[0138] The formula for collision cost is:

[0139] t min =max{t o , t k}

[0140] t max =min{t o +T o , t k +T k}

[0141]

[0142]

[0143]

[0144]

[0145] Among them, J do,k is the collision cost corresponding to the kth dynamic obstacle, is the three-dimensional relative position information of the current position of the aircraft and the nearest point between the bounding boxes corresponding to the kth dynamic obstacle, Φ o (t) is the t in the trajectory of the aircraft k The current position at the moment, F(.) is a function that finds the nearest point from the current position to the bounding box and calculates the three-dimensional relative position of the given point and the nearest point, r is the radius of the spherical model corresponding to the aircraft, d safe is a safe distance, E represents the conversion of the Euclidean distance into an ellipsoidal distance with a shorter Z axis to reduce downwash interference, and d k (t) is t k The first dynamic parameter at time, G k (t) is t k The second dynamic parameter at time t min is the minimum time, t max The maximum moment.

[0146] After the collision cost is obtained, the dynamic obstacle avoidance penalty function is determined based on the collision cost corresponding to each dynamic obstacle. The formula of the dynamic obstacle avoidance penalty function is:

[0147]

[0148] Among them, J do is the dynamic obstacle avoidance penalty function, and K is the number of dynamic obstacles.

[0149] The trajectory planning method proposed in this embodiment determines the minimum time and maximum time corresponding to each dynamic obstacle based on the first starting time and the first trajectory duration corresponding to the predicted trajectory of each dynamic obstacle, and the second starting time and the second trajectory duration corresponding to the trajectory of the aircraft of the key path point set; then determines the collision cost corresponding to each dynamic obstacle based on the current position of the aircraft, the minimum time, the maximum time, the bounding box corresponding to each dynamic obstacle, and the safety distance; then determines the dynamic obstacle avoidance penalty function based on the collision cost corresponding to each dynamic obstacle, and uses the predicted trajectory to establish a dynamic obstacle avoidance penalty function that can reduce the impact of downwash interference, thereby achieving the goal of avoiding dynamic threats, improving the accuracy of the flight trajectory, and further improving the stability, comfort and safety of the aircraft flight.

[0150] The present application also provides an aircraft, please refer to Figure 7 , the aircraft comprises:

[0151] A first generating module 10 is used to generate a key path point set based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path, wherein the key path point set includes an obstacle avoidance path point set corresponding to the static obstacles;

[0152] A first determination module 20, configured to determine a control point set of a cubic B-spline curve based on a user-set speed corresponding to the aircraft, a key path point set, and a cubic B-spline basis function;

[0153] A second determination module 30 is used to determine a dynamic obstacle avoidance penalty function based on the dynamic obstacle and the control point set, determine a static obstacle avoidance penalty function based on the obstacle avoidance path point set, and determine an optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function;

[0154] The second generating module 40 is used to generate a target trajectory corresponding to the aircraft based on the optimization model and the current flight parameters of the aircraft.

[0155] The trajectory planning device provided in the embodiment of the present application adopts the trajectory planning method in the above embodiment, which can solve the technical problem of how to improve the accuracy of the flight trajectory of the aircraft. Compared with the prior art, the beneficial effects of the trajectory planning device provided in the embodiment of the present application are the same as the beneficial effects of the trajectory planning method provided in the above embodiment, and the other technical features in the trajectory planning device are the same as the features disclosed in the above embodiment method, which will not be repeated here.

[0156] The present application provides a trajectory planning device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the trajectory planning method in the above-mentioned embodiment one.

[0157] Reference below Figure 8 , which shows a schematic diagram of the structure of a trajectory planning device suitable for implementing the embodiment of the present application. The trajectory planning device in the embodiment of the present application may include but is not limited to mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Figure 8The trajectory planning device shown is merely an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0158] like Figure 8 As shown, the trajectory planning device may include a processing device 1001 (e.g., a central processing unit, a graphics processor, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM: Read Only Memory) 1002 or a program loaded from a storage device 1003 to a random access memory (RAM: Random Access Memory) 1004. In RAM1004, various programs and data required for the operation of the trajectory planning device are also stored. The processing device 1001, ROM1002, and RAM1004 are connected to each other via a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Generally, the following systems can be connected to the I / O interface 1006: an input device 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; an output device 1008 including, for example, a liquid crystal display (LCD: Liquid Crystal Display), a speaker, a vibrator, etc.; a storage device 1003 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 1009. The communication device 1009 can allow the trajectory planning device to communicate with other devices wirelessly or wired to exchange data. Although the trajectory planning device with various systems is shown in the figure, it should be understood that it is not required to implement or have all the systems shown. More or fewer systems can be implemented or provided instead.

[0159] In particular, according to the embodiments disclosed in the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present application include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication device, or installed from a storage device 1003, or installed from a ROM 1002. When the computer program is executed by the processing device 1001, the above-mentioned functions defined in the method of the embodiment disclosed in the present application are executed.

[0160] The trajectory planning device provided by the present application adopts the trajectory planning method in the above embodiment to solve the technical problem of how to improve the accuracy of the flight trajectory of the aircraft. Compared with the prior art, the beneficial effects of the trajectory planning device provided by the present application are the same as the beneficial effects of the trajectory planning method provided by the above embodiment, and the other technical features in the trajectory planning device are the same as the features disclosed in the method of the previous embodiment, which will not be repeated here.

[0161] It should be understood that the various parts disclosed in this application can be implemented by hardware, software, firmware or a combination thereof. In the description of the above embodiments, specific features, structures, materials or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0162] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

[0163] The present application provides a computer-readable storage medium having computer-readable program instructions (ie, computer programs) stored thereon, and the computer-readable program instructions are used to execute the trajectory planning method in the above-mentioned embodiment.

[0164] The computer-readable storage medium provided in the present application may be, for example, a USB flash drive, but is not limited to electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, systems or devices, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination with an instruction execution system, system or device. The program code contained on the computer-readable storage medium may be transmitted using any appropriate medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination of the above.

[0165] The computer-readable storage medium may be included in the trajectory planning device; or may exist independently without being assembled into the trajectory planning device.

[0166] The computer-readable storage medium carries one or more programs. When the one or more programs are executed by the trajectory planning device, the trajectory planning device: generates a key path point set based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path, wherein the key path point set includes the obstacle avoidance path point set corresponding to the static obstacles; determines the control point set of the cubic B-spline curve based on the user-set speed corresponding to the aircraft, the key path point set and the cubic B-spline basis function; determines a dynamic obstacle avoidance penalty function based on the dynamic obstacles and the control point set, determines a static obstacle avoidance penalty function based on the obstacle avoidance path point set, and determines an optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function; generates a target trajectory corresponding to the aircraft based on the optimization model and the current flight parameters of the aircraft.

[0167] Computer program code for performing the operations of the present application may be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0168] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present application. In this regard, each square box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, the program segment or a part of the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two square boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0169] The modules involved in the embodiments described in this application may be implemented by software or hardware, wherein the name of the module does not constitute a limitation on the unit itself in some cases.

[0170] The readable storage medium provided by the present application is a computer-readable storage medium, which stores computer-readable program instructions (i.e., computer programs) for executing the above-mentioned trajectory planning method, and can solve the technical problem of how to improve the accuracy of the flight trajectory of the aircraft. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided by the present application are the same as the beneficial effects of the trajectory planning method provided by the above-mentioned embodiment, and will not be repeated here.

[0171] An embodiment of the present application provides a computer program product, including a computer program, which implements the steps of the trajectory planning method as described above when the computer program is executed by a processor.

[0172] The computer program product provided in this application can solve the technical problem of how to improve the accuracy of the flight trajectory of an aircraft. Compared with the prior art, the beneficial effects of the computer program product provided in the embodiment of this application are the same as the beneficial effects of the trajectory planning method provided in the above embodiment, which will not be repeated here.

[0173] The above are only preferred embodiments of the present application, and are not intended to limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A trajectory planning method, characterized in that: include: Generate a key path point set based on the static obstacle corresponding to the aircraft, the local trajectory of the previous frame, and the global navigation path, wherein the key path point set includes an obstacle avoidance path point set corresponding to the static obstacle; Determining a control point set of a cubic B-spline curve based on a user-set speed corresponding to the aircraft, a key path point set, and a cubic B-spline basis function; Based on the dynamic obstacles and the control point set, a dynamic obstacle avoidance penalty function is determined, based on the obstacle avoidance path point set, a static obstacle avoidance penalty function is determined, and an optimization model is determined based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function; Based on the optimization model and the current flight parameters of the aircraft, a target trajectory corresponding to the aircraft is generated.

2. The trajectory planning method according to claim 1, characterized in that: The step of determining the control point set of the cubic B-spline curve based on the user-set speed corresponding to the aircraft, the key path point set and the cubic B-spline basis function comprises: Based on the user-set speed, determine the target speed limit and critical path point time corresponding to each critical path point in the critical path point set; Based on the cubic B-spline basis function, the target speed limit corresponding to each key path point and the key path point time, the control point set of the cubic B-spline curve is determined.

3. The trajectory planning method according to claim 2, characterized in that: The step of determining the target speed limit corresponding to each key path point in the key path point set and the key path point time based on the user set speed includes: Based on the user-set speed and the steep rise and fall speed upper limit, determine the first speed upper limit corresponding to each key path point; Based on the speed upper limit of the previous key path point in the first speed upper limit and the corresponding acceleration upper limit, the speed upper limit of the current key path point is determined, and the second speed upper limit corresponding to each key path point is obtained; Based on the speed upper limit of the next key path point in the second speed upper limit and the corresponding acceleration upper limit, the speed upper limit of the current key path point is determined, and the target speed limit corresponding to each key path point is obtained; Based on the key path point distance and the target speed limit, a time set is determined, wherein the time set includes the key path point moments corresponding to each key path point.

4. The trajectory planning method according to claim 3, characterized in that: The step of determining the control point set of the cubic B-spline curve based on the cubic B-spline basis function, the target speed limit corresponding to each key path point, and the key path point time includes: Based on the target limit speed, obtain a first speed and a first acceleration of a first key path point in the key path point set, and a second speed and a second acceleration of a last key path point in the key path point set; A control point set of a cubic B-spline curve is determined based on a key path point set, a first velocity, a first acceleration, a second velocity and a second acceleration at key path point moments.

5. The trajectory planning method according to claim 1, characterized in that: The step of determining the dynamic obstacle avoidance penalty function based on the dynamic obstacle and the control point set comprises: Based on the first start time and the first trajectory duration corresponding to the predicted trajectory of each dynamic obstacle, and the second start time and the second trajectory duration corresponding to the trajectory of the aircraft of the key path point set, respectively determine the minimum time and the maximum time corresponding to each dynamic obstacle; Determine the collision cost of each dynamic obstacle based on the current position of the aircraft, the minimum time, the maximum time, the bounding box of each dynamic obstacle, and the safety distance; The dynamic obstacle avoidance penalty function is determined based on the collision cost corresponding to each dynamic obstacle.

6. The trajectory planning method according to claim 5, characterized in that: The step of determining the collision cost corresponding to each dynamic obstacle based on the current position of the aircraft, the minimum time, the maximum time, the bounding box corresponding to each dynamic obstacle, and the safety distance includes: Based on the current position of the aircraft and the bounding boxes corresponding to each dynamic obstacle, respectively determine the closest point between the aircraft and each dynamic obstacle, and determine the three-dimensional relative position information between the closest point and the current position of the aircraft; Determine dynamic parameters based on three-dimensional relative position information and safety distance; Based on the dynamic parameters, minimum time and maximum time of each dynamic obstacle, the collision cost corresponding to each dynamic obstacle is determined.

7. The trajectory planning method according to claim 5, characterized in that: The step of determining the minimum time and the maximum time corresponding to each dynamic obstacle based on the first starting time and the first trajectory duration corresponding to the predicted trajectory of each dynamic obstacle, and the second starting time and the second trajectory duration corresponding to the aircraft trajectory of the aircraft, respectively, comprises: Based on the first start time and the second start time, respectively determine the minimum time corresponding to each dynamic obstacle; Based on the first starting time, the first trajectory duration, the second starting time and the second trajectory duration, the maximum time corresponding to each dynamic obstacle is determined.

8. The trajectory planning method according to claim 1, characterized in that: The step of determining a static obstacle avoidance penalty function based on an obstacle avoidance path point set comprises: Based on the obstacle surface points and the movement direction in the obstacle avoidance path point set, as well as the key path points and the safety distance, determine the first static parameters corresponding to the key path points; Determine a second static parameter corresponding to each key path point based on the first static parameter, each key path point and the corresponding motion direction; Based on the second static parameter, a static obstacle avoidance penalty function is determined.

9. The trajectory planning method according to claim 1, characterized in that: The step of determining the optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function comprises: Determine the trajectory smoothness penalty function based on the key path point set, and determine the dynamic feasibility penalty function based on the control point set of the cubic B-spline curve; An optimization model is determined based on a dynamic obstacle avoidance penalty function, a static obstacle avoidance penalty function, a trajectory smoothness penalty function, and a dynamic feasibility penalty function.

10. The trajectory planning method according to claim 9, characterized in that: The step of determining the trajectory smoothness penalty function based on the key path point set comprises: A trajectory smoothness penalty function is determined based on the jerk corresponding to each key path point in the key path point set.

11. The trajectory planning method according to claim 9, characterized in that: The step of determining the dynamic feasibility penalty function based on the control point set of the cubic B-spline curve comprises: Get the 3D velocity, Z-axis velocity, 3D acceleration, 3D and velocity upper limit, Z-axis maximum velocity and the maximum value of the three-axis combined acceleration corresponding to each control point in the control point set; Determine a first penalty function based on the three-dimensional speed and the three-dimensional and speed upper limits, determine a second penalty function based on the Z-axis speed and the Z-axis maximum speed, and determine a third penalty function based on the three-dimensional acceleration and the maximum value of the three-axis combined acceleration; Based on the first penalty function, the second penalty function, and the third penalty function, a dynamic feasibility penalty function is determined.

12. The trajectory planning method according to claim 1, characterized in that: The step of generating a key path point set based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path includes: Determine an alternate path based on the current position, current speed, local trajectory of the previous frame, and global navigation path corresponding to the aircraft; A critical path point set is generated based on the static obstacles corresponding to the aircraft and the initial path point set corresponding to the backup path.

13. The trajectory planning method according to claim 12, characterized in that: The step of determining the backup path based on the current position, current speed, local trajectory of the previous frame and global navigation path corresponding to the aircraft includes: Based on the current position and current speed of the aircraft, determining a first projection point in the local trajectory of the previous frame; Determine the second projection point corresponding to the last local trajectory point in the local trajectory of the previous frame in the global navigation path of the aircraft; The backup path is determined based on the first projection point, the second projection point, the local trajectory of the previous frame, and the global navigation path.

14. The trajectory planning method according to claim 12, characterized in that: The step of generating a critical path point set based on the static obstacles corresponding to the aircraft and the initial path point set corresponding to the backup path comprises: Sample the backup paths according to the distances of the key path points to obtain an initial path point set; Based on the static grid map corresponding to the static obstacle information, determine whether there is a collision segment in the initial path point set; Determine an obstacle avoidance path point set based on the collision segment, wherein the obstacle avoidance path point set includes obstacle surface points and corresponding movement directions; The key path point set is generated based on the obstacle avoidance path point set and the initial path point set.

15. An aircraft, characterized in that: The aircraft comprises: A first generating module is used to generate a key path point set based on the static obstacles corresponding to the aircraft, the local trajectory of the previous frame and the global navigation path, wherein the key path point set includes an obstacle avoidance path point set corresponding to the static obstacles; A first determination module is used to determine a control point set of a cubic B-spline curve based on a user-set speed corresponding to the aircraft, a key path point set, and a cubic B-spline basis function; A second determination module is used to determine a dynamic obstacle avoidance penalty function based on a dynamic obstacle and a control point set, determine a static obstacle avoidance penalty function based on an obstacle avoidance path point set, and determine an optimization model based on the dynamic obstacle avoidance penalty function and the static obstacle avoidance penalty function; The second generating module is used to generate a target trajectory corresponding to the aircraft based on the optimization model and the current flight parameters of the aircraft.

16. A trajectory planning device, characterized in that: The trajectory planning device includes: a memory, a processor, and a trajectory planning program stored in the memory and executable on the processor. When the trajectory planning program is executed by the processor, the steps of the trajectory planning method according to any one of claims 1 to 14 are implemented.

17. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a trajectory planning program, and when the trajectory planning program is executed by a processor, the steps of the trajectory planning method according to any one of claims 1 to 14 are implemented.

Images