An online path planning method for multi-rotor unmanned aerial vehicle in unknown environment

Abstract

The application discloses a kind of unknown environment under multi-rotor unmanned aerial vehicle online path planning method, belong to unmanned aerial vehicle technical field, to solve the optimization process of current EGO_planner planning algorithm (hereinafter referred to as EGO) in, trajectory in obstacle needs to carry out multiple iterations, and in nearby obstacle bounces multiple times to generate the final trajectory in safe area, spend more time to carry out the process of trajectory re-planning problem;Specific steps are as follows: step 1. generate safe guide trajectory;Step 2. collision control point replacement: the control point Q i corresponding path point on the corresponding safe guide path is replaced as a new control point Q inew ;Step 3. gradient information definition strategy and collision penalty function construction.The method of the application makes the initial path in the obstacle not need to carry out multiple iterations to quickly and efficiently find a safe, smooth, dynamic feasible trajectory, improve the speed of local re-planning.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicles, and particularly relates to a multi-rotor unmanned aerial vehicle online path planning method in an unknown environment. BACKGROUND

[0002] When a multi-rotor unmanned aerial vehicle flies at a high speed in an unknown environment, in order to cope with unpredictable risks, a planning algorithm must complete a new trajectory generation through a short-time re-planning to ensure safety. Therefore, local trajectory re-planning plays an important role in autonomous navigation.

[0003] Most local trajectory re-planning methods provide gradient information through a pre-constructed ESDF (Euclidean Signed Distance Field) map to optimize the initial path. However, due to the non-smooth characteristics of the ESDF map itself and further spatial discretization, the convergence rate and the iterative calculation rate in the optimization process are significantly reduced. The construction of the ESDF occupies 70% of the overall planning time, and only part of the constructed ESDF map is directly used for trajectory optimization. Therefore, the construction of the ESDF is a bottleneck for improving the planning efficiency of the gradient-based optimization method.

[0004] To solve this problem, Zhou et al. proposed a local path planning EGO_planner (hereinafter referred to as EGO, reference: Zhou, X.; Wang, Z.; Ye, H.; Xu, C.; Gao, F. EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors. IEEE Robot. Autom. Lett. 2020.) without ESDF gradient, which uses A* to perform local search to obtain rough obstacle gradient information, parameterizes the trajectory as a B-spline, judges the trajectory segment inside the obstacle, and applies repulsive force from the inside of the obstacle to the control points of the B-spline. The initial trajectory in the obstacle is gradually pushed away until it is in the safe area through the collision penalty function. Compared with several state-of-the-art local path planning methods, the planning time is improved by more than one order of magnitude.

[0005] In the optimization process of the EGO planning algorithm, the trajectory in the obstacle needs to be iterated multiple times, and it bounces in the nearby obstacles multiple times before a final trajectory is generated in the safe area, which spends a lot of time in the trajectory re-planning process. SUMMARY

[0006] To address the aforementioned technical shortcomings, the purpose of this invention is to provide an online trajectory planning method for multi-rotor UAVs in unknown environments. This method, with the aid of a guided path, can generate trajectories more quickly and efficiently, and requires less time for replanning compared to the EGO method.

[0007] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0008] This invention provides an online trajectory planning method for multi-rotor unmanned aerial vehicles (UAVs) in unknown environments, characterized by the following specific steps:

[0009] Step 1. Generate a safe guidance trajectory:

[0010] In the trajectory generation phase of flight path planning, an initial trajectory is planned that only satisfies the terminal constraints and does not consider any obstacles. Specifically, when the UAV's initial position is close to a local target point, a straight line connecting the two points is used as the global trajectory to ensure the shortest path is generated. When the distance between the two points is large, path points are continuously added between them, and the minimum snap trajectory is used to smoothly connect all path points as the initial trajectory. The initial trajectory is parameterized as a uniform B-spline curve Φ, which is P b A piecewise polynomial of order N has decision variables N. c There are 1 control point with a node span of Δt, and each control point records its own environmental information; the curve Φ is referred to as the initial trajectory below. Collision detection is continuously performed on the initial trajectory, with a pair of control points corresponding to Q. in and Q out To record the beginning and end positions of each collision trajectory and find the collision set Q consisting of collision control points. col For each collision trajectory, use the A* path search algorithm to find a path from Q. in To Q out The safe guidance path is obtained, resulting in the path point set A.

[0011] Step 2. Collision control point replacement strategy:

[0012] Using a collision control point replacement strategy, find the collision set Q. col Each control point Q in i For each path point in path point set A, perform a replacement operation and use it as the new control point Q. inew This allows for the rapid removal of obstacles from the initial trajectory, thus avoiding multiple iterations during optimization and consuming a large amount of planning time.

[0013] For each control point Q i The corresponding tangent vector is R. i It can be obtained through the following formula:

[0014]

[0015] In the formula, Δt is the time interval between two control points on the uniform B-spline curve, and this value is the same between any two adjacent control points:

[0016] Δt=t i+1 -t i

[0017] To make control point Q, which is in the obstacle i Push away the current obstacle with the fastest speed and shortest distance, and find a path perpendicular to R in the generated set of path points A of the safe guidance path. i Path point A mi ;

[0018] QA i For Q i With A mi The vector formed when QA i With R i When the inner product is 0, the path point A can be obtained. mi ,Right now:

[0019] QA i =A mi -Q i

[0020] R i QA i =0

[0021] To avoid at control point Q i Search for A one by one in the path point set A. mi However, this consumes a large amount of computing power, making each control point Q... i The index a is the position where half the total number of path points in path point set A is obtained. m The search begins, that is, the first search for A. mi When using position indexing, a mi =a m According to formula a mi ,

[0022]

[0023] Determine R i The value of QA determines the next step in searching for A. mi The location index of the path point a mi Finally, Q is obtained. i Corresponding A mi Path point, then Q i With A mi The path point is replaced and used as the new control point Q. inew ;

[0024] Step 3. Gradient information definition strategy:

[0025] By using the collision control point replacement strategy in step 2, new control points and new trajectories were obtained. Since the safe guidance path is generated using the A* algorithm, which has the characteristic of being close to the surface of the obstacle, it is easy to find the base point of the obstacle surface corresponding to the new control point.

[0026] Using the new control point as the initial position, quickly find the first occupied grid cell in the direction of the original position, with a step size equal to the resolution of the occupied grid map. This grid cell is the base point p of the control point on the obstacle surface. ij ;

[0027] Using the base point p on the surface of the obstacle ij The repulsion direction vector v between the control points of the collision trajectory and the target point ij To construct gradient information, the direction is the direction of the gradient; p ij With v ij It is a one-to-one correspondence, simply referred to as {P,V} pairs. The index of the control point is represented by j∈N, which represents the index of the {P,V} pair.

[0028] To mitigate the local minima problem during trajectory planning, v is reversed. r =-v, to construct distance fields in different directions, and to find the base point p of the obstacle in reverse. r , forming a new pair {p r ,v r Yes, this generates two topological trajectories, which are then optimized in parallel by balancing smoothness, safety, and dynamic feasibility.

[0029] Step 4: Design of Collision Penalty Function

[0030] After replacing the collision control points, gradient information is constructed, strictly limiting the magnitude of the gradient to a positive range to avoid negative gradient values ​​and simplifying collision penalty estimation; control point Q i The corresponding PV function for collision cost is:

[0031]

[0032] d ij =(Q i -p ij )·v ij

[0033] Where, d thr This is a safe distance threshold for drone flight. If the distance is less than this threshold, the collision penalty will begin to increase rapidly. ij For control point Qinew To the corresponding p ij Distance; evaluate each Q i The collision cost should be for all corresponding N p The total collision cost J is obtained by summing the costs of each PV and then summing the costs of all control points. c ;

[0034]

[0035]

[0036] Finally, through J c Q i Perform differentiation to obtain the gradient value;

[0037]

[0038] The beneficial effects of this invention are as follows: It proposes a faster obstacle avoidance strategy, which uses the guiding path generated by A* and adopts a replacement strategy to replace the control points located in the obstacle in the initial trajectory with the path points corresponding to the guiding path. At the same time, it designs a more effective gradient information definition strategy and penalty function. This enables the initial path in the obstacle to quickly and efficiently find a safe, smooth, and dynamically feasible trajectory without multiple iterations, thereby improving the speed of local planning and replanning. Attached Figure Description

[0039] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0040] Figure 1 This is a schematic diagram illustrating the search for corresponding path points in step 2 of the online trajectory planning method for multi-rotor UAVs in an unknown environment provided by the present invention.

[0041] Figure 2 This is a schematic diagram illustrating the search for corresponding path points in step 2 of the online trajectory planning method for multi-rotor UAVs in an unknown environment provided by the present invention.

[0042] Figure 3 A schematic diagram showing the completion of step 2 after replacement in the online trajectory planning method for multi-rotor UAVs in unknown environments provided by the present invention;

[0043] Figure 4 This is a schematic diagram illustrating step 3 of the online trajectory planning method for a multi-rotor UAV in an unknown environment, provided by the present invention, which involves finding aiming points on the surface of obstacles.

[0044] Figure 5 This invention provides a schematic diagram of step 3 {p,v} generated in an online trajectory planning method for multi-rotor UAVs in an unknown environment.

[0045] Figure 6 This is a schematic diagram illustrating step 3 of the online trajectory planning method for multi-rotor UAVs in unknown environments, provided by the present invention, which involves reversely searching for aiming points on the surface of obstacles.

[0046] Figure 7 This is a schematic diagram illustrating step 3 of the online trajectory planning method for multi-rotor UAVs in an unknown environment, provided by the present invention, which generates two topological trajectories.

[0047] Figure 8 This is a schematic diagram showing the comparison results between the method of this invention and the EGO method in terms of total replanning time;

[0048] Figure 9 This diagram illustrates the comparison between the method of this invention and the EGO method in terms of single replanning time. Detailed Implementation

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

[0050] like Figures 1-5 As shown, an online trajectory planning method for multi-rotor UAVs in unknown environments includes the following specific steps:

[0051] Step 1. Generate a safe guidance trajectory:

[0052] In the trajectory generation phase of flight path planning, an initial trajectory is planned that only satisfies the terminal constraints and does not consider any obstacles. Specifically, when the UAV's initial position is close to a local target point, a straight line connecting the two points is used as the global trajectory to ensure the shortest path is generated. When the distance between the two points is large, path points are continuously added between them, and the minimum snap trajectory is used to smoothly connect all path points as the initial trajectory. The initial trajectory is parameterized as a uniform B-spline curve Φ, which is P b A piecewise polynomial of order N has decision variables N. c There are 1 control point with a node span of Δt, and each control point records its own environmental information; the curve Φ is referred to as the initial trajectory below. Collision detection is continuously performed on the initial trajectory, with a pair of control points corresponding to Q. in and Q outTo record the beginning and end positions of each collision trajectory and find the "collision set" Q composed of collision control points. col For each collision trajectory, use the A* path search algorithm to find a path from Q. in To Q out The safe guidance path is obtained, resulting in the path point set A.

[0053] Step 2: Collision Control Point Replacement Strategy:

[0054] When optimizing a trajectory, the EGO_planner algorithm requires multiple iterations and bounces multiple times among nearby obstacles before generating the final trajectory in a safe area, which takes a considerable amount of time to optimize.

[0055] To avoid this situation and to quickly and efficiently find a safe, smooth, and dynamically feasible trajectory, this application employs a collision control point replacement strategy to find the collision set Q. col Each control point Q in i For each path point in path point set A, perform a replacement operation and use it as the new control point Q. inew This allows for the rapid removal of obstacles from the initial trajectory, thus avoiding multiple iterations during optimization and the resulting significant time consumption in planning.

[0056] The specific path point search is as follows: Figure 1 and Figure 2 As shown.

[0057] In the diagram, the dark black curve represents the initial trajectory, the light gray curve represents the safe boot path generated by A*, and R... i For Q i tangent vector,

[0058] For each control point Q i The corresponding tangent vector is R. i It can be obtained through the following formula:

[0059]

[0060] In the formula, Δt is the time interval between two control points on the uniform B-spline curve, and this value is the same between any two control points:

[0061] Δt=t i+1 -t i ;

[0062] To make control point Q, which is in the obstacle i Push away the current obstacle with the fastest speed and shortest distance, and find a path perpendicular to R in the generated set of path points A of the safe guidance path. i Path point A mi .

[0063] QA i For Q i With A mi The vector formed when QA i With R i When the inner product is 0, the path point A can be obtained. mi ,Right now:

[0064] QA i =A mi -Q i ;

[0065] R i QA i =0;

[0066] To avoid at control point Q i Search for A one by one in the path point set A. mi However, this consumes a large amount of computing power, making each control point Q... i The index a is the position where half the total number of path points in path point set A is obtained. m The search begins, that is, the first search for A. mi When using position indexing, a mi =a m According to formula a mi ,

[0067]

[0068] Determine R i The value of QA determines the next step in searching for A. mi The location index of the path point a mi Finally, Q is obtained. i Corresponding A mi Path point, then Q i With A mi The path point is replaced and used as the new control point Q. inew The result after the replacement is shown in the image. Figure 3 As shown.

[0069] Step 3: Define gradient information strategy and collision penalty function

[0070] By employing a collision control point replacement strategy, information Q of the new control point was obtained. inew And a new trajectory. Furthermore, the safe guidance path is generated using the A* algorithm, which closely follows the surface of obstacles, making it easy to find new control points Q. inew The aiming point on the corresponding obstacle surface.

[0071] With the new control point Q inew Starting from the initial position, move towards the original position Q. iThe direction quickly finds the first occupied grid cell using the resolution of the occupied grid map as the step size; this is the aiming point p on the obstacle surface corresponding to the control point. ij ,like Figure 4 As shown.

[0072] The target point p was found ij With control point Q inew There is a corresponding repulsion direction vector v between them ij p ij With v ij It is a one-to-one correspondence, simply referred to as {P,V} pairs. In the above, This represents the index of the control point, where j∈N represents the index of the {P,V} pair, such as... Figure 5 As shown.

[0073] To avoid local minima during trajectory replanning, v is reversed. r =-v, to construct distance fields in different directions, and to find the aiming point p on the obstacle in reverse. r , forming a new pair {p r ,v r},like Figure 6 As shown.

[0074] This process generates two topological trajectories, as follows: Figure 7 As shown, the next step is to optimize the two trajectories in parallel, balancing smoothness, safety, and dynamic feasibility, and finally execute the trajectory with the lowest cost.

[0075] Control point Q on the trajectory after control point replacement inew To the corresponding p ij The distance can be defined as:

[0076] d ij =(Q i -p ij )·v ij

[0077] Step 4: Design of Collision Penalty Function

[0078] In EGO, gradients are generated between control points and base points. When a control point is inside an obstacle during optimization, the gradient is negative, making collision avoidance more complex. In our work, gradient information is constructed after replacing the collision control point, strictly limiting the gradient's magnitude to a positive range. This avoids negative gradient values ​​and simplifies collision penalty estimation.

[0079] Therefore, control point Q i The corresponding PV function for collision cost is:

[0080]

[0081] Where, d thr This is a safe distance threshold for drone flight. If the distance is less than this threshold, the collision penalty will begin to increase rapidly. ij For control point Q inew To the corresponding p ij The distance. Evaluate each Q. i The collision cost should be for all corresponding N p The total collision cost J is obtained by summing the costs of each PV and then summing the costs of all control points. c ;

[0082]

[0083] Where, N p d represents the number of PV pairs corresponding to the control points. thr This is the safe distance threshold for drone flight. If the distance is less than this threshold, the collision penalty will begin to increase rapidly.

[0084] In the initial trajectory, the sum of the collision costs of all control points within obstacles equals the total cost J. c ,Right now

[0085]

[0086] In this application, instead of using the traditional trilinear interpolation method to calculate the collision gradient, the J... c Q i The gradient is obtained by taking the derivative.

[0087]

[0088] To verify the effectiveness of the proposed local trajectory replanning method, the method of this application was compared with EGO under a randomly deployed 30*30*3 map.

[0089] To ensure fairness, this paper uses EGO's default parameter settings, with the maximum velocity and maximum acceleration set to 2 m / s² and 3 m / s², respectively. 2 Obstacle density is a key parameter for simulating the complexity of real-world scenarios. The higher the obstacle density, the more complex the environment, and the higher the performance requirements for local replanning algorithms.

[0090] Therefore, the effectiveness of the replanning algorithms under different obstacle densities on the map was specifically compared. Each planner was run twenty times from the same starting point to the end point under each obstacle density. The average performance statistics and computation time are shown in Table 1. The experimental results include the average flight speed, the total replanning time for each flight, and the computation time for each replanning operation.

[0091] In a random forest map scenario using random toroidal and columnar structures, 20 trials were conducted each time at 6 different obstacle densities. The average performance statistics and computation time are shown in Table I.

[0092] Table I

[0093]

[0094]

[0095] Figure 4 and Figure 5 The results show the comparison between the method of this application and the EGO method in terms of total replanning time and single replanning time.

[0096] As shown in Table I, the proposed method outperforms EGO in average flight speed under different obstacle densities on the random forest map. Figure 4 , Figure 5 As shown, the more the obstacle density increases, the greater the reduction in time required for single replanning and overall replanning compared to EGO, and the greater the advantage of the proposed method.

[0097] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A method for online trajectory planning of a multi-rotor unmanned aerial vehicle (UAV) in an unknown environment, characterized in that, The specific steps are as follows: Step 1. Generate a safe guidance trajectory: In the trajectory generation phase of flight path planning, an initial trajectory is planned that only satisfies the terminal constraints and does not consider any obstacles. Specifically, when the UAV's initial position is close to a local target point, a straight line connecting the two points is used as the global trajectory to ensure the shortest path is generated. When the distance between the two points is large, path points are continuously added between them, and the minimum snap trajectory is used to smoothly connect all path points as the initial trajectory. The initial trajectory is parameterized as a uniform B-spline curve Φ, which is P b A piecewise polynomial of order N has decision variables N. c There are 1 control point with a node span of Δt, and each control point records its own environmental information; the curve Φ is referred to as the initial trajectory below. Collision detection is continuously performed on the initial trajectory, with a pair of control points corresponding to Q. in and Q out To record the beginning and end positions of each collision trajectory and find the collision set Q consisting of collision control points. col For each collision trajectory, use the A* path search algorithm to find a path from Q. in To Q out The safe guidance path is obtained, resulting in path point set A; Step 2. Collision control point replacement strategy: Using a collision control point replacement strategy, find the collision set Q. col Each control point Q in i For each path point in path point set A, perform a replacement operation and use it as the new control point Q. inew This allows for the rapid removal of obstacles from the initial trajectory, thus avoiding multiple iterations during optimization and consuming a large amount of planning time. For each control point Q i The corresponding tangent vector is R. i It can be obtained through the following formula: In the formula, Δt is the time interval between two control points on the uniform B-spline curve, and this value is the same between any two adjacent control points: Δt=t i+1 -t i To make control point Q, which is in the obstacle i Push away the current obstacle with the fastest speed and shortest distance, and find a path perpendicular to R in the generated set of path points A of the safe guidance path. i Path point A mi ; QA i For Q i With A mi The vector formed when QA i With R i When the inner product is 0, the path point A can be obtained. mi ,Right now: QA i =A mi -Q i R i ·QA i =0 To avoid at control point Q i Search for A one by one in the path point set A. mi However, this consumes a large amount of computing power, making each control point Q... i The index a is the position where half the total number of path points in path point set A is obtained. m The search begins, that is, the first search for A. mi When using position indexing, a mi =a m According to formula a mi , Determine R i The value of QA determines the next step in searching for A. mi The location index of the path point a mi Finally, Q is obtained. i Corresponding A mi Path point, then Q i With A mi The path point is replaced and used as the new control point Q. inew ; Step 3. Gradient information definition strategy: By using the collision control point replacement strategy in step 2, new control points and new trajectories were obtained. Since the safe guidance path is generated using the A* algorithm, which has the characteristic of being close to the surface of the obstacle, it is easy to find the base point of the obstacle surface corresponding to the new control point. Using the new control point as the initial position, quickly find the first occupied grid cell in the direction of the original position, with a step size equal to the resolution of the occupied grid map. This grid cell is the base point p of the control point on the obstacle surface. ij ; Using the base point p on the surface of the obstacle ij The repulsion direction vector v between the control points of the collision trajectory and the target point ij To construct gradient information, the direction is the direction of the gradient; p ij With v ij It is a one-to-one correspondence, simply referred to as {P,V} pairs. The index of the control point is represented by j∈N, which represents the index of the {P,V} pair. To mitigate the local minima problem during trajectory planning, v is reversed. r =-v, to construct distance fields in different directions, and to find the base point p of the obstacle in reverse. r , forming a new pair {p r ,v r Yes, this generates two topological trajectories, which are then optimized in parallel by balancing smoothness, safety, and dynamic feasibility. Step 4: Design of Collision Penalty Function After replacing the collision control points, gradient information is constructed, strictly limiting the magnitude of the gradient to a positive range to avoid negative gradient values ​​and simplifying collision penalty estimation; control point Q i The corresponding PV function for collision cost is: d ij =(Q i -p ij )v ij Where, d thr This is a safe distance threshold for drone flight. If the distance is less than this threshold, the collision penalty will begin to increase rapidly. ij For control point Q inew To the corresponding p ij Distance; evaluate each Q i The collision cost should be for all corresponding N p The total collision cost J is obtained by summing the costs of each PV and then summing the costs of all control points. c ; Finally, through J c Q i Perform differentiation to obtain the gradient value;

Images