Unmanned aerial vehicle trajectory planning method based on improved minimum jump

Abstract

The invention discloses an unmanned aerial vehicle track planning method based on an improved minimum jump. The method comprises the following steps: firstly, establishing a safe flight corridor in a flight space of an unmanned aerial vehicle to restrain discrete waypoints obtained in the previous step; smoothing the trajectory by using an algorithm based on minimum jump; and finally, in order to solve the overall safety problem of the trajectory, selecting and utilizing a multi-section Bezier curve to replace a fifth-order polynomial in a minimum jump algorithm, establishing a series of constraint conditions, and finally generating a smooth safety path meeting the dynamic constraint of the unmanned aerial vehicle. The path generated by trajectory planning conforms to the dynamic constraint of the unmanned aerial vehicle and can effectively avoid obstacles, the safe and smooth path can be planned through the provided obstacle avoidance system in the near-earth complex environment, and the condition that the unmanned aerial vehicle works efficiently and safely in the complex high-density obstacle environment is met.

Description

Technical Field

[0001] The invention relates to a UAV trajectory planning method based on improved minimization of jumps, and belongs to the technical field of communication. Background Art

[0002] The purpose of quadcopter trajectory planning is to solve the problem that the path obtained by the front-end search does not conform to the dynamics of the drone and cannot be directly used for drone flight. Therefore, how to plan a path that meets the safety and smoothness requirements of the drone dynamics is a problem that needs to be solved in drone trajectory planning. The current mainstream solutions are, first, to use obstacle information to form hard constraints that must strictly meet the constraints, such as using flight corridors to impose restrictions on the trajectory, and second, to use a method similar to the artificial potential field method to construct soft constraints such as thrust to keep the drone away from obstacles.

[0003] But obviously, it is difficult to ensure the overall safety of the UAV trajectory with a method based entirely on hard constraints. Even with the use of safe flight corridors, it is difficult to solve the problem of polynomial trajectory out-of-bounds. The method of imposing a safe flight corridor on each waypoint will obviously greatly increase the burden on the computer, especially for the limited computing power of the onboard computer. Therefore, although many scholars have proposed many solutions to this problem, they still cannot balance the problem of safety and computational complexity. Until the emergence of a trajectory planning method based on Bezier curves, the control points of Bezier curves are used to wrap the curve. The safety of the trajectory can be guaranteed with a very small amount of calculation, which perfectly solves the problem of the trajectory exceeding the restricted area. However, there is still the problem that the smoothness of the generated trajectory is insufficient, and the original minimization jump algorithm is not suitable for UAV obstacle avoidance. Summary of the invention

[0004] The purpose of the present invention is to address the defects and shortcomings of the above-mentioned prior art and propose a UAV trajectory planning method based on improved minimization of jumps. The method solves the fitting problem of the original path by expanding the obstacles. Secondly, the polynomial is replaced by the Bezier curve by utilizing the characteristics of the convex hull constraint composed of control points, and a safe flight corridor is constructed to constrain the convex hull. Finally, a smooth and safe path that meets the UAV dynamic constraints is generated to improve the safety and stability of UAV flight.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a UAV trajectory planning method based on improved minimization of jumps, the method comprising the following steps:

[0006] Step 1: First, construct an optimal function to find the optimal specific trajectory among the feasible trajectories, and plan the trajectory of the drone by minimizing the jump;

[0007] Step 2: Ensure that the complete trajectory is completely within the safe space by utilizing the convex hull property of the Bezier curve;

[0008] Step 3: Use the trapezoidal velocity model to allocate time for each Bezier curve.

[0009] Furthermore, the step 1 comprises:

[0010] Step 1-1: Determine the order N of the trajectory polynomial. Take the k-segment trajectory as an example. The unknown value to be solved is (N+1)*k. The number of constraints is 5+k. The parameters of the k-segment trajectory should satisfy N=5 / k. The minimum number of segments k=1. N is greater than or equal to 5. A fifth-order polynomial is used to represent the trajectory:

[0011] p(t)=p0+p1t+p2t 2 +p3t 3 +p4t 4 +p5t 5

[0012] By taking the third-order derivative of the above formula, we can get the jump expression:

[0013] p (3) (t) = 6p3 + 24tp4 + 60t 2 p5

[0014] =[0,0,0,6,24t,60t 2 ]·p

[0015] The derivatives of each order of the polynomial represent velocity and acceleration respectively, and the matrix form is as follows:

[0016] x(0)=p0

[0017] x(T)=p5T 5 +p4T 4 +p3T 3 +p2T 2 +p1T+p0

[0018] x(0)′=p1

[0019] x(T)′=5p5T 4 +4p4T 3 +3p3T 2 +2p2T+p1

[0020] x(0)″=2p2

[0021] x(T)″=20p5T 3 +12p4T 2 +6p3T+2p2

[0022]

[0023] Therefore, the relationship between the parameter p and the specific physical quantity can be constructed, and the actual physical quantity is called d i , the transformation matrix is ​​M i Similarly, we can also construct the objective function J m :

[0024]

[0025] Squaring the jumps gives:

[0026]

[0027] Let A=aa T , calculate A:

[0028]

[0029] Let the integral term equal to Q, and integrate the above formula to obtain:

[0030]

[0031] The rectangular form of the objective function is derived:

[0032]

[0033] Obviously, the matrix Q is a real symmetric matrix of M×M;

[0034] Step 1-2: Add constraints to the quadratic programming problem. First, consider the derivative constraints, that is, the initial state of the starting point of the trajectory, the final state of the end point, and the position constraints. For the initial state, it is expressed as follows:

[0035]

[0036] The above initial conditions are generally given by path planning, that is, at T i At the waypoint position of the drone at the moment, add continuity constraints to ensure that the position, velocity, and acceleration at the connection of the two trajectories are consistent. For the continuity constraint, it is expressed as follows:

[0037]

[0038] Through the conversion relationship between the actual physical quantity and the parameter p, the process of solving the parameter is converted into the process of solving the actual physical quantity. The converted integral term is as follows:

[0039]

[0040] Convert discrete waypoints to a smooth curve in polynomial form.

[0041] Furthermore, the step 2 comprises:

[0042] For a trajectory consisting of multiple Bezier curves, it is expressed as follows:

[0043]

[0044] Among them, f μ (t) is a trajectory composed of m segments of Bezier curves. Since the time t in the Bezier curve is limited to the interval [0,1], a scaling factor z needs to be added i To scale the time, the cost function can still be constructed using the minimization algorithm, which is expressed as follows:

[0045]

[0046] The objective function can still be constructed into a matrix form:

[0047] J(t)=min(c T Qc)

[0048] Using the corresponding relationship between control points and polynomials, the fifth-order polynomial is replaced by the Bezier function, that is, p = M·c, and the sixth-order Bezier curve is:

[0049]

[0050] Based on the convex hull property of the Bezier curve, it is necessary to add constraints to the drone, including:

[0051] Step 2-1: First consider the constraints of the drone's initial state, assuming the starting point position is p s , the speed is v s , the acceleration is a s , the end position p e , the speed is v e , the acceleration is v e , then at time 0, we get the control point:

[0052]

[0053] For the derivatives of various orders at the starting point and the end point of the trajectory, we can get:

[0054]

[0055] Step 2-2: Consider the continuity constraint of the drone, that is, it is necessary to ensure the continuity of all-order derivatives of the connection points. The control point constraint is expressed as follows:

[0056]

[0057] For the maximum speed and acceleration of the drone in dynamics, the derivation of the Bezier curve is still the property of the Bezier curve, and the constraints are as follows:

[0058]

[0059] Furthermore, the step 3 comprises:

[0060] In each trajectory, it is assumed that the UAV has a maximum acceleration a max Accelerate to V max , then V max The uniform motion reaches the moment t2, and then starts with the maximum acceleration -a max Slow down to time T until the speed is 0, and the relationship between speed and time is as follows:

[0061] L=(T+t2-t1)V max / 2

[0062] V max =t1×a max

[0063] Where L is the length of the current Bezier curve.

[0064] Beneficial effects:

[0065] 1. The present invention proposes an obstacle avoidance system for unmanned aerial vehicles (UAVs) in high-density near-ground scenes. Taking into account the possibility that the trajectory of the minimization jump algorithm may cross obstacles, a flight corridor construction method is used to add constraints to the minimization jump algorithm.

[0066] 2. The present invention further improves the minimization jump algorithm, replaces the fifth-order polynomial with the Bezier curve, and realizes safe trajectory planning by taking advantage of the property that the Bezier curve converges to the convex hull formed by the control points.

[0067] 3. The path generated by the trajectory planning of the present invention complies with the dynamic constraints of the UAV and can effectively avoid obstacles. In complex near-ground environments, a safe and smooth path can be planned according to the proposed obstacle avoidance system, meeting the conditions for the UAV to work efficiently and safely in complex and high-density obstacle environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 This is a diagram of the algorithm for minimizing jumps according to the present invention.

[0069] Figure 2 This is the trapezoidal speed curve algorithm diagram of the present invention.

[0070] Figure 3 This is a simulation structure diagram of the system of the present invention.

[0071] Figure 4 It is a discrete path diagram in the three-dimensional space of the simulation experiment of the present invention.

[0072] Figure 5 This is a map of different obstacle densities for simulation experiments of the present invention.

[0073] in, Figure 5 (a) is a map with an obstacle density of 0.1; Figure 5 (b) is the map with obstacle density of 0.2; Figure 5 (c) is a map with an obstacle density of 0.3; Figure 5 (d) is the map with obstacle density of 0.4.

[0074] Figure 6 This is a comparison chart of flight trajectories in different density maps of the simulation experiment of the present invention.

[0075] in, Figure 6 (a) is the flight trajectory diagram with obstacle density of 0.1; Figure 6 (b) is the flight trajectory diagram with obstacle density of 0.2; Figure 6 (c) is the flight trajectory diagram with obstacle density of 0.3; Figure 6 (d) is the flight trajectory diagram when the obstacle density is 0.4.

[0076] Figure 7 This is a curve diagram of the position of the UAV in the simulation experiment of the present invention.

[0077] in, Figure 7 (a) is the position diagram in the x-axis direction; Figure 7 (b) is the position diagram in the y-axis direction; Figure 7 (c) is the position diagram in the z-axis direction.

[0078] Figure 8 It is the speed curve diagram of the UAV in the simulation experiment of the present invention.

[0079] in, Figure 8 (a) is the velocity curve in the x-axis direction; Figure 8 (b) is the velocity curve diagram in the y-axis direction; Figure 8 (c) is the velocity curve in the z-axis direction.

[0080] Fig. 9 This is a comparison chart of trajectory generation time under different obstacle densities in the simulation experiment of the present invention.

[0081] Fig.10 This is a comparison chart of trajectory lengths under different obstacle densities in the simulation experiment of the present invention. DETAILED DESCRIPTION

[0082] The invention will be further described in detail below in conjunction with the accompanying drawings.

[0083] like Figure 1 As shown, the present invention proposes a UAV trajectory planning method based on improved minimization of jumps, the method comprising the following steps:

[0084] Step 1: First, construct an optimal function to find the optimal specific trajectory among the feasible trajectories, and plan the trajectory of the drone by minimizing the jump;

[0085] Based on polynomial trajectory planning, there are usually countless trajectories that meet the constraints. In practical problems, a specific trajectory is often required, so it is necessary to construct an optimal function to find the "optimal" specific trajectory among the feasible trajectories. The minimization Snap method proposed by Mellinger is an optimization algorithm for drone trajectory planning. Its core idea is to minimize the above cost function:

[0086] min J(p)

[0087] sA eq p=b eq ,

[0088] A ieq p≤b ieq

[0089] In the minimization Snap algorithm, the constructed J m The cost function is the square integral of Snap:

[0090]

[0091] Since the drone is shuttling at high speed in a complex environment, the present invention gives priority to the stability and safety of the drone. Although minimizing Snap can minimize the overall energy consumption of the drone, it is not friendly to the payload stability and camera image quality of the drone. The positioning and mapping of drones in complex environments are crucial. Minimizing the jump can make the acceleration change of the drone as smooth as possible, and can make the drone's payload and depth camera work more stable. Therefore, in order to ensure the safety of drones flying in complex environments, the drone trajectory is planned by minimizing the jump.

[0092] The above step 1 of the present invention specifically comprises:

[0093] Step 1-1: Determine the order N of the trajectory polynomial. Take the k-segment trajectory as an example. The unknown value to be solved is (N+1)*k. The number of constraints is 5+k. The parameters of the k-segment trajectory should satisfy N=5 / k. The minimum number of segments k=1. N is greater than or equal to 5. A fifth-order polynomial is used to represent the trajectory:

[0094] p(t)=p0+p1t+p2t 2 +p3t 3 +p4t 4 +p5t 5

[0095] By taking the third-order derivative of the above formula, we can get the jump expression:

[0096] p (3) (t) = 6p3 + 24tp4 + 60t 2 p5

[0097] =[0,0,0,6,24t,60t 2 ]·p

[0098] The derivatives of each order of the polynomial represent velocity and acceleration respectively, and the matrix form is as follows:

[0099] x(0)=p0

[0100] x(T)=p5T 5 +p4T 4 +p3T 3 +p2T 2 +p1T+p0

[0101] x(0)′=p1

[0102] x(T)′=5p5T 4 +4p4T 3 +3p3T 2 +2p2T+p1

[0103] x(0)″=2p2

[0104] x(T)″=20p5T 3 +12p4T 2 +6p3T+2p2

[0105]

[0106] Therefore, the relationship between the parameter p and the specific physical quantity can be constructed, and the actual physical quantity is called d i , the transformation matrix is ​​M i Similarly, we can also construct the objective function J m :

[0107]

[0108] Squaring the jumps gives:

[0109]

[0110] Let A=aa T , calculate A:

[0111]

[0112] Let the integral term equal to Q, and integrate the above formula to obtain:

[0113]

[0114] The rectangular form of the objective function is derived:

[0115]

[0116] Obviously, the matrix Q is a real symmetric matrix of M×M;

[0117] Step 1-2: Add constraints to the quadratic programming problem. First, consider the derivative constraints, that is, the initial state of the starting point of the trajectory, the final state of the end point, and the position constraints. For the initial state, it is expressed as follows:

[0118]

[0119] The above initial conditions are generally given by path planning, that is, at T i At the waypoint position of the drone at the moment, add continuity constraints to ensure that the position, velocity, and acceleration at the connection of the two trajectories are consistent. For the continuity constraint, it is expressed as follows:

[0120]

[0121] Through the conversion relationship between the actual physical quantity and the parameter p, the process of solving the parameter is converted into the process of solving the actual physical quantity. The converted integral term is as follows:

[0122]

[0123] Convert discrete waypoints into smooth curves in polynomial form, such as Figure 1 As shown, the broken line is the discrete waypoints mentioned above, and the curve is the curve after trajectory planning. After smoothing, the obtained path will greatly save the overall energy consumption of the drone.

[0124] Step 2: Ensure that the complete trajectory is completely within the safe space by utilizing the convex hull property of the Bezier curve;

[0125] In the above space corridor, although the waypoints can be constrained within the safe area through the safe flight corridor, it is impossible to ensure that the complete trajectory is completely within the safe space. To this end, the midpoint of the line between the waypoints is taken as the new waypoint, and the polynomial trajectory generation is repeated until the trajectory no longer collides with obstacles. Obviously, the above method is not applicable to all situations. When the drone is in a narrow and complex environment, the method may be iterated multiple times, which is obviously not suitable for real-time planning of drones.

[0126] Therefore, the present invention uses the Bezier curve to replace the above-mentioned quintic polynomial. By using the convex hull property of the Bezier curve, the trajectory can be restricted to the safe space. The basic Bezier curve principle is summarized, and the properties of the Bezier curve are as follows:

[0127] (1) For an n-order Bezier curve, there are n+1 control points. The curve only passes through the starting point and the end point and does not pass through other control points. Therefore, the path points obtained in the front-end path planning can be naturally used as control points.

[0128] (2) The Bezier curve is strictly confined to the convex hull formed by the control points.

[0129] (3) The Bezier curve is still a continuous and smooth curve, which enables the UAV to achieve a smooth transition during flight, avoiding sudden turns and sharp acceleration or deceleration, and helping to improve flight comfort and stability.

[0130] (4) The derivatives of the Bezier curve are still Bezier curves, that is, its velocity, acceleration and other curves still satisfy the properties of the Bezier curve.

[0131] (5) The Bezier curve is tangent to the convex hull formed by its control points, that is, multiple Bezier curves can still maintain the continuity of the curve.

[0132] Obviously, by using the above properties, the polygon formed by the control points can be placed in a safe space where the drone can fly, which can effectively ensure the safety of the trajectory. Then, by using the properties of its derivatives, it can be ensured that its acceleration and speed are continuous, which obviously meets the dynamic constraints.

[0133] Gao converted the minimization of Snap into B-spline trajectory optimization based on Bezier curve, as shown in the following formula:

[0134]

[0135] The above formula is the expression of a Bezier curve. The set of is the control point set of the j-th Bezier curve. is the Bernstein polynomial basis function.

[0136] The above step 2 of the present invention specifically includes:

[0137] For a trajectory consisting of multiple Bezier curves, it is expressed as follows:

[0138]

[0139] Among them, f μ (t) is a trajectory composed of m segments of Bezier curves. Since the time t in the Bezier curve is limited to the interval [0,1], a scaling factor z needs to be added i To scale the time, the cost function can still be constructed using the minimization algorithm, which is expressed as follows:

[0140]

[0141] The objective function can still be constructed into a matrix form:

[0142] J(t)=min(c T Qc)

[0143] Using the corresponding relationship between control points and polynomials, the fifth-order polynomial is replaced by the Bezier function, that is, p = M·c, and the sixth-order Bezier curve is:

[0144]

[0145] Based on the convex hull property of the Bezier curve, it is necessary to add constraints to the drone, including:

[0146] Step 2-1: First consider the constraints of the drone's initial state, assuming the starting point position is p s , the speed is v s , the acceleration is a s , the end position p e , the speed is v e , the acceleration is v e , then at time 0, we get the control point:

[0147]

[0148] For the derivatives of various orders at the starting point and the end point of the trajectory, we can get:

[0149]

[0150] Step 2-2: Consider the continuity constraint of the drone, that is, it is necessary to ensure the continuity of all-order derivatives of the connection points. The control point constraint is expressed as follows:

[0151]

[0152] For the maximum speed and acceleration of the drone in dynamics, the derivation of the Bezier curve is still the property of the Bezier curve, and the constraints are as follows:

[0153]

[0154] Step 3: Use the trapezoidal velocity model to allocate time for each Bezier curve;

[0155] Although the segmented smooth trajectory of the drone is obtained above, the time for each segment of the trajectory is not allocated. The present invention uses a trapezoidal velocity model to allocate the time for each Bezier curve, that is, it is assumed that the drone first accelerates to the maximum speed in each segment of the trajectory, and then decelerates to a speed of 0 at the maximum acceleration.

[0156] The above step 3 of the present invention specifically comprises: Figure 2 As shown, in each trajectory, it is assumed that the drone has a maximum acceleration a max Accelerate to V max , then V max The uniform motion reaches the moment t2, and then starts with the maximum acceleration -a max Slow down to time T until the speed is 0, and the relationship between speed and time is as follows:

[0157] L=(T+t2-t1)V max / 2

[0158] V max =t1×a max

[0159] Where L is the length of the current Bezier curve.

[0160] The simulation experiment and data of the present invention are as follows:

[0161] like Figure 3As shown, the simulation process of the present invention can be mainly divided into the following key parts: first, the front-end path search part, which uses the JA* algorithm for high-speed search. Secondly, the back-end flight corridor generation and trajectory optimization part. At this stage, the system will further generate a flight corridor, i.e., a channel that takes into account flight safety and feasibility, based on the path obtained by the front-end search. Then, the system will optimize the trajectory of the path to ensure that the flight of the drone is smoother and more efficient, and to minimize energy consumption and flight time. Finally, the output collision-free safe path is input to the controller part. After the controller receives the optimized flight path, it will calculate the corresponding control instructions, such as speed, acceleration, and attitude angle, according to the current state and target position of the drone. These control instructions will directly act on the drone, enabling it to fly according to a predetermined trajectory, and effectively avoid obstacles to achieve obstacle avoidance.

[0162] In terms of algorithm indicators, the present invention uses the time consumption of the path planning algorithm and the trajectory planning algorithm as real-time indicators, and the length and safety of the trajectory as feasibility indicators.

[0163] The simulation experiment of the present invention mainly includes the following:

[0164] (1) Verify whether the obstacle avoidance system proposed in this paper can solve the trajectory safety problem in the present invention, that is, whether it can ensure the generation of a safe and collision-free trajectory that conforms to the dynamics of the drone.

[0165] (2) Verify the effectiveness of the algorithm proposed in this paper in complex three-dimensional space, that is, whether it can generate a safe trajectory in a short time in a space with a large obstacle density.

[0166] (3) Compared with other advanced algorithms, the algorithm evaluation indicators shown above are used to evaluate and compare the algorithms.

[0167] In order to quickly verify the simulation results, the present invention uses the Matlab simulation system framework to complete the simulation, and the processor is AMD Ryzen 7 5800H. In Matlab, the obstacle information is pre-set according to the obstacle density and map size, and the constraints of the maximum flight speed of the drone 2m / s and the maximum acceleration 2m / s2 are set. Since the drone perception system is not added, the flight distance is set to a single perception range, that is, within a range of 9m. That is, for the visual camera, the perception range is the sphere range of a radius of 9m.

[0168] Q = {Q(xx′) 2 +(yx′) 2 +(zx′) 2 ≤R 2}

[0169] Among them, (x, y, z) represents the current position of the drone, (x′, y′, z′) represents the current position to be detected, and R is the perception radius.

[0170] In Matlab, the PD controller can be used to compare the actual flight trajectory of the UAV with the preset trajectory. Figure 4 As shown in the figure, the simulation environment of this paper adopts a 6m×8m×4m grid map, the starting point is Pstart(3.0,0.0,1.0), and the end point Pend is (3.0,8.0,2.0). The obstacle density in the figure is 0.4, the black cylinders are obstacles, the red path is the discrete path of the original JA* algorithm, and the black path is the simplified discrete waypoint. Figure 5 (a) to Figure 5 As shown in (d), a simulation comparison analysis was performed on maps with obstacle densities ranging from 0.1 to 0.4.

[0171] In order to verify the effectiveness of the obstacle avoidance system in complex environments, the obstacle avoidance system was tested using the same starting point and end point in the above-mentioned maps with different obstacle densities. Each map was tested 50 times. The thick red line in the figure is the trajectory of the JA* algorithm, the thick black line is the curve after pruning, the red dot is the curve generated by the improved minimization jump algorithm, and the blue dotted line is the curve tracked by the controller.

[0172] like Figure 6 (a) to Figure 6 As shown in (d), after multiple tests on maps with obstacle density of 0.1 to 0.4, the final trajectories obtained are smooth and safe. It is proved that the improved minimization jump algorithm can effectively avoid the trajectory of the original algorithm from colliding. In order to verify whether it is smooth in speed, a simple PD controller is used to simulate the algorithm. Taking the map with obstacle density of 0.4 as an example, the final position and speed curves are as follows Figure 7 (a) to 7(c), Figure 8 As shown in (a) to 8(c): the red color is the expected trajectory, the blue color is the actual flight trajectory, the two line segment trajectories basically overlap, and the PD controller achieves the tracking effect of the input trajectory. The above speed curve is continuous and smooth, and meets the dynamic constraints of the maximum acceleration and maximum speed given by the present invention, which proves the effectiveness of trajectory planning based on polynomials.

[0173] The proposed improved algorithm is simulated and compared with the current advanced algorithm through the Matlab simulation platform. This simulation also uses an obstacle environment with a density of 0.1 to 0.4 for simulation, and the number of simulations is set to 50 times, recording its trajectory length, trajectory generation time, motion time and other indicators.

[0174] By using the RRT* algorithm as the front-end path planning, the back-end trajectory planning algorithm uses the basic minimization Snap algorithm for trajectory planning. Since there are no constraints on the trajectory, its trajectory is not absolutely safe. When the trajectory collides with an obstacle, it will be replanned by interpolating the trajectory. By using the A* algorithm to search for discrete paths on Octomap, after obtaining the discrete safe path, the natural grid formed by Octomap is used as the flight corridor, and then the generated flight corridor is used to generate constraints. The cost function uses the minimization Snap, but it only constrains the waypoints of the trajectory. Although the planning framework can greatly improve the success rate of planning, there is obviously still a probability of failure, and it does not have the completeness of the safety of the entire trajectory. Therefore, once the trajectory crosses an obstacle, it is necessary to continuously detect the extreme value of the trajectory, resulting in an extension of the planning time.

[0175] Trajectory generation time comparison Fig. 9 As shown: Obviously, the improved minimization jump algorithm proposed in the present invention generates trajectories faster in an environment with high obstacle density. The algorithm of the present invention can fully guarantee the safety of the generated trajectory through the properties of the Bezier curve. At the same time, the optimization method of the Bezier curve can save a lot of resources. When the obstacle density is 0.1, the time to generate the trajectory is basically the same.

[0176] The trajectory length comparison is Fig.10 As shown in the figure: The energy consumption of drones is extremely high, so the trajectory planning length is an important indicator to measure a trajectory planning algorithm, and the path generated by the front end will naturally be optimal.

[0177] The above description is only a specific implementation of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by a technician familiar with the technical field within the technical scope disclosed by the present invention should be covered by the protection scope of the present invention.

Claims

1. A UAV trajectory planning method based on improved minimization of jumps, characterized in that: The method comprises the following steps: Step 1: First, construct an optimal function to find the optimal specific trajectory among the feasible trajectories, and plan the trajectory of the drone by minimizing the jump; Step 2: Ensure that the complete trajectory is completely within the safe space by utilizing the convex hull property of the Bezier curve, including: For a trajectory consisting of multiple Bezier curves, it is expressed as follows: Among them, f μ (t) is a trajectory composed of m segments of Bezier curves. Since the time t in the Bezier curve is limited to the interval [0,1], a scaling factor z needs to be added i To scale the time, the cost function can still be constructed using the minimization algorithm, which is expressed as follows: The objective function can still be constructed into a matrix form: J(t)=min(c T Qc) Using the corresponding relationship between control points and polynomials, the fifth-order polynomial is replaced by the Bezier function, that is, p = M·c, and the sixth-order Bezier curve is: Based on the convex hull property of the Bezier curve, it is necessary to add constraints to the drone, including: Step 2-1: First consider the constraints of the drone's initial state, assuming the starting point position is p s , the speed is v s , the acceleration is a s , the end position p e , the speed is v e , the acceleration is v e , then at time 0, we get the control point: For the derivatives of various orders at the starting point and the end point of the trajectory, we can get: Step 2-2: Consider the continuity constraint of the drone, that is, it is necessary to ensure the continuity of all-order derivatives of the connection points. The control point constraint is expressed as follows: For the maximum speed and acceleration of the drone in dynamics, the derivation of the Bezier curve is still the property of the Bezier curve, and the constraints are as follows: Step 3: Use the trapezoidal velocity model to allocate time for each Bezier curve.

2. The UAV trajectory planning method based on improved minimization of jumps according to claim 1 is characterized in that: The step 1 comprises: Step 1-1: Determine the order N of the trajectory polynomial. Take the k-segment trajectory as an example. The unknown value to be solved is (N+1)*k. The number of constraints is 5+k. The parameters of the k-segment trajectory should satisfy N=5 / k. The minimum number of segments k=1. N is greater than or equal to 5. A fifth-order polynomial is used to represent the trajectory: p(t)=p0+p1t+p2t 2 +p3t 3 +p4t 4 +p5t 5 By taking the third-order derivative of the above formula, we can get the jump expression: p (3) (t)=6p3+24tp4+60t 2 p5 =[0,0,0,6,24t,60t 2 ]·p The derivatives of each order of the polynomial represent velocity and acceleration respectively, and the matrix form is as follows: x(0)=p0 x(T)=p5T 5 +p4T 4 +p3T 3 +p2T 2 +p1T+p0 x(0)′=p1 x(T)′=5p5T 4 +4p4T 3 +3p3T 2 +2p2T+p1 x(0)″=2p2 x(T)″=20p5T 3 +12p4T 2 +6p3T+2p2 Therefore, the relationship between the parameter p and the specific physical quantity can be constructed, and the actual physical quantity is called d i , the transformation matrix is ​​M i Similarly, we can also construct the objective function J m : Squaring the jumps gives: Let A=aa T , calculate A: Let the integral term equal to Q, and integrate the above formula to obtain: The rectangular form of the objective function is derived: Obviously, the matrix Q is a real symmetric matrix of M×M; Step 1-2: Add constraints to the quadratic programming problem. First, consider the derivative constraints, that is, the initial state of the starting point of the trajectory, the final state of the end point, and the position constraints. For the initial state, it is expressed as follows: The above initial conditions are generally given by path planning, that is, at T i At the waypoint position of the drone at the moment, add continuity constraints to ensure that the position, velocity, and acceleration at the connection of the two trajectories are consistent. For the continuity constraint, it is expressed as follows: Through the conversion relationship between the actual physical quantity and the parameter p, the process of solving the parameter is converted into the process of solving the actual physical quantity. The converted integral term is as follows: Convert discrete waypoints to a smooth curve in polynomial form.

3. The UAV trajectory planning method based on improved minimization of jumps according to claim 1 is characterized in that: The step 3 comprises: In each trajectory, it is assumed that the UAV has a maximum acceleration a max Accelerate to V max , then V max The uniform motion reaches the moment t2, and then starts with the maximum acceleration -a max Slow down to time T until the speed is 0, and the relationship between speed and time is as follows: L=(T+t2-t1)V max / 2 In max =t1×a max Where L is the length of the current Bezier curve.

Images