Unmanned aerial vehicle trajectory planning method based on three-dimensional tangent graph method in urban low-altitude environment

Abstract

The present invention discloses a method for unmanned aerial vehicle (UAV) trajectory planning based on a three-dimensional tangent graph method in an urban low-altitude environment. Whenever an obstacle is encountered, three obstacle avoidance sub-paths can be obtained by using the tangent graph method to assist the UAV in bypassing the obstacle from both sides or above the obstacle. According to the designed heuristic rules, a better sub-path is selected from the three sub-paths. The UAV will fly along the path and use the tangent graph method to avoid obstacles until the UAV successfully reaches the destination. The present invention is applied to the field of UAV trajectory planning. Compared with traditional trajectory planning methods, the present invention can efficiently search for a feasible obstacle avoidance trajectory that is approximately optimal, and can help the UAV escape from a simple maze within a limited time.

Description

Technical Field

[0001] The invention relates to the technical field of unmanned aerial vehicle (UAV) trajectory planning, and in particular to a UAV trajectory planning method based on a three-dimensional tangent graph method in an urban low-altitude environment. Background Art

[0002] E-commerce has become one of the most popular shopping methods in the world today, thanks to its convenient shopping, great discounts, transparent prices, and a wide range of goods. However, the various logistics and transportation costs generated by e-commerce have become increasingly expensive, especially the last-mile logistics distribution. Due to the advantages of low energy consumption, high flight speed, and no restrictions on ground traffic conditions and terrain, drones can be used for last-mile logistics distribution to effectively reduce logistics distribution costs and transportation time. Track planning is one of the key technologies for drones to autonomously complete logistics distribution. Compared with the track planning tasks of military operations, the low-altitude environment in cities is generally narrow, with dense obstacles and intensive human activities. Therefore, it requires real-time track planning capabilities and the ability to quickly plan a feasible flight track in narrow alleys.

[0003] Domestic and foreign scholars have conducted a lot of research and exploration on the problem of UAV trajectory planning in three-dimensional space. So far, three-dimensional trajectory planning methods are mainly divided into graph-based methods and non-graph-based methods. Specifically, the graph-based method refers to modeling obstacles in the attitude space of the UAV, converting complex scenes into simple, vivid, and operable maps, and then performing trajectory search on this map. As one of the classic graph-based trajectory planning methods, the sparse A* algorithm (SAS) is based on the principle of the A* algorithm and takes into account the maneuverability of the UAV, such as the minimum step length, minimum pitch angle, maximum yaw angle, etc., and can directly plan a trajectory that conforms to the dynamics of the UAV. In addition, Liu et al. proposed a three-dimensional jumping point search algorithm to improve the trajectory search speed by pruning the neighbors of the searched nodes. Therefore, compared with the A* algorithm, the planning time of this algorithm is greatly reduced. As another graph-based method, the tangent graph method was originally proposed by Rohnert, and later many scholars made improvements on this basis, but the tangent graph method is currently mainly oriented to two-dimensional trajectory planning.

[0004] Non-graph theory methods mainly include random sampling-based methods, potential field methods, and intelligent optimization methods. As a common sampling-based method, rapidly-exploring random tree (RRT) performs very well in path search speed. However, some scholars have proved that RRT is not asymptotically optimal. In order to overcome this difficulty, Gammell et al. reselected parent nodes for new nodes and rewired random trees, and proposed Informed RRT*. At the same time, the search space of Informed RRT* is limited to the planning domain of an ellipsoid, which can improve the quality of the solution and speed up the search. Wen et al. introduced rolling time domain optimization in the RRT* algorithm, which has the ability of real-time trajectory planning.

[0005] Artificial potential field (APF) has good trajectory planning performance in conventional environments, but in environments with dense and cluttered obstacles, the trajectory may oscillate continuously in front of obstacles and eventually fall into a local optimum. In recent years, intelligent optimization methods have attracted much attention due to their global optimization capabilities. Roberge et al. combined genetic algorithms and particle swarm optimization algorithms to generate feasible approximate optimal trajectories in complex three-dimensional environments. In order to improve the efficiency of trajectory planning, Kok et al. proposed a path planner based on genetic algorithms, which fully implemented all modules of the proposed architecture. Wu et al. used an adaptive pheromone evaporation coefficient to improve the optimization performance of the ant colony algorithm in three-dimensional path planning.

[0006] However, most of the existing graph-based methods and intelligent optimization methods rely on traversing points in space to find obstacle avoidance paths, which can lead to a sharp increase in trajectory planning time in three-dimensional dense and cluttered environments. Compared with traversing points in space to find the optimal trajectory, the tangent graph method obtains the ideal obstacle avoidance trajectory by repeatedly finding the tangent lines between obstacles. However, so far, they are still applicable to two-dimensional scenes. Summary of the invention

[0007] In view of the above-mentioned deficiencies in the prior art, the present invention provides a UAV trajectory planning method based on the three-dimensional tangent graph method in an urban low-altitude environment, which can not only efficiently search for a nearly optimal feasible obstacle avoidance trajectory, but also help the UAV escape from a simple maze within a limited time.

[0008] To achieve the above object, the present invention provides a UAV trajectory planning method based on a three-dimensional tangent graph method in an urban low-altitude environment, comprising the following steps:

[0009] Whenever the UAV encounters an obstacle, the tangent graph method is used to generate three obstacle avoidance tracks that can bypass the obstacle from both sides and above;

[0010] Based on the heuristic rules, an optimal obstacle avoidance track is selected, the UAV is controlled to fly along the obstacle avoidance track, and the track is adjusted by the tangent graph method during the flight to avoid obstacles;

[0011] This cycle repeats until the drone can successfully reach the destination.

[0012] In one embodiment, the UAV trajectory planning method specifically includes the following steps:

[0013] Step 1: Establish an obstacle model containing all obstacle coordinate information, and initialize the track point set Pa, the candidate track point set Ca and the processed obstacle set Ba to be empty sets;

[0014] Step 2: Add the drone flight starting point S to the point set Pa, and add the drone flight end point E to the point set Ca;

[0015] Step 3: Take the last track point in the point set Pa as the starting point O, and the last track point in the point set Ca as the target point D, and determine whether there is an obstacle intersecting the straight path OD:

[0016] If it exists, record the first intersecting obstacle and go to step 4;

[0017] If it does not exist, add point D to the point set Pa, delete point D from the point set Ca, and go to step 7;

[0018] Step 4: Using the starting point O and the target point D as the starting point and the end point of the sub-path respectively, the tangent graph method is used to generate two horizontal sub-paths that bypass the obstacle from both sides of the first intersecting obstacle, and based on the heuristic rule, a better horizontal sub-path h_path is selected from the two horizontal sub-paths;

[0019] Step 5: Use the starting point O and the target point D as the starting point and the end point of the sub-path respectively, use the tangent graph method to generate a vertical sub-path that bypasses the obstacle from the top of the first intersecting obstacle, and select one from the vertical sub-path and the horizontal sub-path h_path as the optimal sub-path p based on the heuristic rule;

[0020] Step 6: Take the tangent point between the best subpath p and the first intersecting obstacle as the track point, and add the track points on the best subpath p to the point set Ca in reverse order of the best subpath p, and add the first intersecting obstacle to the set Ba;

[0021] Step 7: Determine whether point E is in the point set Pa:

[0022] If so, output the point set Pa;

[0023] Otherwise, return to step 3.

[0024] In one embodiment, in step 4, the two generated horizontal sub-paths are both located in the plane where the straight line SE is located, and the altitude of the drone continuously rises or continuously falls or remains unchanged when flying along the two horizontal sub-paths.

[0025] In one embodiment, in step 4, the step of selecting a better horizontal sub-path h_path from two horizontal sub-paths based on a heuristic rule is specifically:

[0026] The following four heuristic rules with priorities are used to select a better horizontal subpath h_path from two horizontal subpaths:

[0027] When selecting a horizontal sub-path, the starting point tangent and the target point tangent of the selected horizontal sub-path do not intersect with the latest obstacle added in the set Ba;

[0028] Select a horizontal subpath whose tangent line from the starting point intersects with fewer obstacles;

[0029] Select a horizontal subpath where the tangent line of the target point intersects with fewer obstacles;

[0030] Select the horizontal subpath with the shorter path length.

[0031] In one embodiment, in step 5, the horizontal projection of the vertical sub-path overlaps with the horizontal projection of the spatial straight line SE.

[0032] In one embodiment, in step 5, the method of selecting one of the vertical sub-path and the horizontal sub-path h_path as the best sub-path p based on the heuristic rule is as follows:

[0033] The following three heuristic rules with priorities are used to select one of the vertical subpath and the horizontal subpath h_path as the best subpath p:

[0034] If the tangent line of the starting point of the vertical subpath intersects more obstacles, the horizontal subpath is selected;

[0035] If the target point tangent of the vertical subpath intersects more obstacles, the horizontal subpath is selected;

[0036] Select the subpath with the shorter path length.

[0037] In one embodiment, the UAV trajectory planning method further includes:

[0038] Step 8, obtain the point set Pa output in step 7;

[0039] Step 9: randomly select three consecutive track points from the extraction point set Pa, and determine whether the line between the first track point and the third track point intersects with the obstacle:

[0040] If yes, return to step 9 until all track points in the point set Pa are traversed, and the point set Pa is output as the UAV track planning result;

[0041] Otherwise, delete the second track point from the point set Pa and return to step 9.

[0042] The present invention provides a method for unmanned aerial vehicle (UAV) trajectory planning based on a three-dimensional tangent graph method in an urban low-altitude environment. Whenever an obstacle is encountered, the tangent graph method can be used to obtain three obstacle avoidance sub-paths to assist the UAV in bypassing the obstacle from both sides or above the obstacle. According to the designed heuristic rules, a better sub-path is selected from the three sub-paths. The UAV will fly along the path and use the tangent graph method to avoid obstacles until the UAV successfully reaches the destination. Compared with traditional trajectory planning methods, the present invention can efficiently search for a feasible obstacle avoidance trajectory that is approximately optimal, and can help the UAV escape from a simple maze within a limited time. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] In order to more clearly illustrate the embodiments of the present invention 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, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying creative work.

[0044] Figure 1 This is a schematic diagram of drone logistics delivery in an urban environment in an embodiment of the present invention;

[0045] Figure 2 This is a schematic diagram of a cube representing an obstacle in an embodiment of the present invention;

[0046] Figure 3 Schematic diagram of three obstacle avoidance sub-paths generated by 3D-TG in an embodiment of the present invention;

[0047] Figure 4 3D-STG is a top view of the obstacle avoidance subpath generated by 3D-STG in an embodiment of the present invention, where: (a) is a track planning diagram under a conventional environment, (b) and (c) are track planning diagrams under a simple maze;

[0048] Figure 5 Flow chart of the UAV trajectory planning method in an embodiment of the present invention;

[0049] Figure 6 This is a simplified schematic diagram of the track in an embodiment of the present invention;

[0050] Figure 7 A schematic diagram of cubic B-spline curve fitting in an embodiment of the present invention;

[0051] Figure 8 A schematic diagram of the local energy reduction of a cubic B-spline curve in an embodiment of the present invention;

[0052] Fig. 9 This is a top view of the trajectory planning of 3D-DTG in a completely unknown environment in an embodiment of the present invention, where: (a) is a diagram of environmental perception and trajectory planning at the starting point, (b) and (c) are online trajectory planning diagrams during flight;

[0053] Fig.10 Schematic diagrams of two simulation scenarios in the embodiments of the present invention, wherein: (a) is a schematic diagram of scenario E1, and (b) is a schematic diagram of scenario E2;

[0054] Fig.11 Schematic diagram of comparative experimental results under conventional environment in an embodiment of the present invention, wherein: (a) is a schematic diagram of track length, (b) is a schematic diagram of running time;

[0055] Fig.12 3D-STG in a maze environment in an embodiment of the present invention, wherein: (a) is a trajectory planning map of case C13, (b) is a trajectory planning map of case C14, (c) is a trajectory planning map of case C15, (d) is a trajectory planning map of case C16, (e) is a trajectory planning map of case C17, (f) is a trajectory planning map of case C18, (g) is a trajectory planning map of case C19, and (h) is a trajectory planning map of case C20;

[0056] Fig.13 Graphs of trajectory replanning in the case of sudden obstacles in an embodiment of the present invention, where: (a)-(d) are trajectories generated by APF for cases C21-C24, (e)-(h) are trajectories generated by 3D-DTG for cases C21-C24, light-colored cubes represent known obstacles, dark-colored cubes represent unknown obstacles, black lines represent offline trajectories, and lines represent replanned trajectories;

[0057] Fig.14 Figure 1 is a trajectory planning diagram in a completely unknown environment in an embodiment of the present invention, where: (a) and (d) are trajectories generated by RHRRT* for case C27-C28, (b) and (e) are trajectories generated by APF for case C27-C28, (c) and (f) are trajectories generated by 3D-DTG for case C27-C28, light-colored cubes represent detected obstacles, black cubes represent unknown obstacles, and lines represent trajectories;

[0058] Fig.15 A top view of a building in a real scene in an embodiment of the present invention;

[0059] Fig.16 The figures are the trajectory planning diagrams of 3D-TG in static and dynamic environments in the real scene of the embodiment of the present invention, where: (a) is the trajectory planning diagram in static environment, and (b) is the trajectory planning diagram in dynamic environment.

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

[0061] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0062] It should be noted that all directional indications in the embodiments of the present invention (such as up, down, left, right, front, back, etc.) are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0063] In addition, in the present invention, descriptions such as "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" or "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0064] In the present invention, unless otherwise clearly specified and limited, the terms "connection", "fixation", etc. should be understood in a broad sense. For example, "fixation" can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection, an electrical connection, a physical connection, or a wireless communication connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be the internal connection of two elements or the interaction relationship between two elements, unless otherwise clearly defined. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0065] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in the field can implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0066] Aiming at the problem of UAV trajectory planning in urban low-altitude environments, a tangent-based method in 3-D urban environments (3D-TG) is proposed, which includes the following steps:

[0067] Whenever the UAV encounters an obstacle, the tangent graph method is used to generate three obstacle avoidance tracks that can bypass the obstacle from both sides and above;

[0068] Based on the heuristic rules, an optimal obstacle avoidance track is selected, the UAV is controlled to fly along the obstacle avoidance track, and the track is adjusted by the tangent graph method during the flight to avoid obstacles;

[0069] This cycle repeats until the drone can successfully reach the destination.

[0070] Different from the traditional point-to-point traversal method, the method in this embodiment generates an obstacle avoidance track by gradually connecting the tangents of obstacles, which has high planning efficiency. Compared with the two-dimensional tangent graph method, the three-dimensional tangent graph method has the following three challenges:

[0071] (1) Model construction: The obstacles in the urban low-altitude environment are mainly buildings. The constructed model is required to well describe the characteristics of the buildings and support the subsequent trajectory planning;

[0072] (2) Track generation and search strategy: Due to the increase in dimensions, there will be more alternative obstacle avoidance tracks in three-dimensional scenes, which will increase the computational complexity of the algorithm to a certain extent. Therefore, the design of a sophisticated track generation and search strategy is of great significance to the final track quality and track planning efficiency.

[0073] (3) The maneuverability of the UAV, such as the maximum pitch angle.

[0074] In view of the above challenges, the solution of this embodiment is as follows:

[0075] (1) Using cubes to represent buildings is a simple model that can effectively support subsequent trajectory planning;

[0076] (2) Each time an obstacle is encountered, three obstacle avoidance tracks are generated from both sides and above the obstacle using the tangent graph method, and one of them is selected for subsequent extension according to the heuristic rule;

[0077] (3) The maximum pitch angle is considered in the trajectory generation. For example, the two obstacle avoidance trajectories from both sides of the obstacle are constrained to a plane where all track points satisfy the maximum pitch angle constraint.

[0078] like Figure 1 As shown in Figure 1, trajectory planning is one of the prerequisites for autonomous UAV logistics delivery. In an environment with small space and dense obstacles, the problem of UAV trajectory planning becomes more complicated. In order to effectively solve this problem, it is necessary to model the environment before performing trajectory planning. Since most of the buildings in the urban low-altitude environment are regular buildings, this embodiment uses a cube to represent obstacles, such as Figure 2 shown.

[0079] Assuming that the starting point and the end point of the UAV flight are denoted as S and E respectively, and obstacles are represented by cubes, then the N obstacles are represented as B = {B1, B2, ..., B k , …, B N}.like Figure 2 As shown, the normal vector of plane j on the obstacle cube can be expressed as:

[0080]

[0081] Among them, U1, U2, and U3 refer to three adjacent vertices arranged in counterclockwise order on the surface j of the obstacle cube. According to the right-hand rule, the direction of the plane normal vector is from the inside of the obstacle to the outside.

[0082] Assume that point P (x, y, z) is an arbitrary point in space, then the plane j where point P is located can be expressed as Plane j can divide the space into three parts: positive half space, negative half space and the plane itself, which can be expressed as f j (x, y, z)>0,f j (x, y, z) < 0 and f j (x, y, z) = 0. A point in the negative half-space and plane j can be described as H j = {(x, y, z)|f j (x, y, z)≤0}. Therefore, the point set used to construct the cubic obstacle can be expressed as:

[0083] B k =H1∩H2∩…∩H6

[0084] For all obstacles, each track point F on the obstacle avoidance track i (x i ,y i , zi ) should satisfy the following formula:

[0085]

[0086] At this point, the obstacle model containing all obstacle coordinate information has been established. It should be noted that in the subsequent UAV trajectory planning method, all generated track points are kept at a certain distance from the obstacle cube to ensure the safety of the UAV. In addition, the maneuverability of the UAV should be considered during trajectory planning, mainly including the minimum turning radius, maximum pitch angle, etc.

[0087] In order to more clearly describe the 3D-TG method process in this embodiment, the parameter symbols in this embodiment are defined as shown in Table 1:

[0088] Table 1 Main symbols

[0089]

[0090]

[0091] This embodiment also defines two sub-paths: horizontal sub-path and vertical sub-path. It should be noted that the horizontal sub-path does not refer to a path that is completely on the same horizontal plane, but only refers to a path that bypasses the obstacle from both sides of the obstacle cube. Figure 3 As shown:

[0092] The horizontal subpath refers to two subpaths that bypass the obstacle from both sides generated by the tangent graph method, as shown in h1 and h2, and both horizontal subpaths are located in the plane (Plane A) where the straight line SE is located;

[0093] A vertical subpath refers to a subpath generated by the tangent graph method that bypasses the top of the obstacle, as shown in v1;

[0094] In addition, this embodiment further defines the tangent lines starting from the starting point O and the target point D as the starting point tangent line and the target point tangent line, respectively. Figure 3 SF1 is the starting point tangent, and F2E is the target point tangent.

[0095] from Figure 3It can be clearly seen that each horizontal subpath (or each vertical subpath) consists of a starting point tangent (origin-tangent), several intermediate tangents (middle-tangent) and a destination point tangent (destination-tangent). In the process of trajectory planning, all tangent points are initially stored in the candidate track point set Ca. Once point O can be connected to a track point in the set Ca without collision, the track point will be added to the set Pa and removed from Ca. It can be seen that the UAV trajectory planning method in this embodiment can be divided into two steps: track point generation and updating of the starting point and target point (or reference direction selection). Initially, the starting point and the end point are regarded as temporary starting point and target point, respectively. At the same time, the starting point and the end point are added to the sets Pa and Ca, respectively.

[0096] Since the straight path from the start point to the end point may collide with obstacles, multiple track points are generated to avoid obstacles. Using the tangent graph method, three tangents can be generated from the left and right sides and the top of the first intersecting obstacle, that is, three sub-paths. According to the designed heuristic rules, one of the sub-paths is selected. Figure 4 As shown in (a), points S and E represent the starting point and the end point, respectively. Obstacle B2 prevents the UAV from flying along the straight path SE, so the tangent graph method is used to generate two horizontal sub-paths and one vertical sub-path to bypass obstacle B2, as shown in Figure 3 At this time, according to the heuristic rule, assuming that the horizontal sub-path h2 is selected among the three sub-paths, the track points F2 and F1 on the horizontal sub-path will be stored in the set Ca in sequence;

[0097] The next step is to update the starting point and target point to avoid the next obstacle. Update the latest points of the sets Pa and Ca as the starting point and target point respectively. Figure 4 As shown in (a), the newly added track point F1 to Ca will be regarded as a new target point. Then, the accessibility of track point F1 is determined based on the obstacle avoidance of path SF1. If path SF1 does not collide with any obstacle, track point F1 is added to Pa and eliminated from Ca. Track point F1 will be used as the new starting point for the next obstacle avoidance process. Otherwise, a collision-free subpath from S to F1 will be generated by the tangent graph method;

[0098] The above two steps are iterated continuously until the track can be successfully extended to the end point, and finally an obstacle avoidance track is generated.

[0099] In summary, reference Figure 5 In the specific implementation process, the UAV trajectory planning method specifically includes the following steps:

[0100] Step 1: Establish an obstacle model containing all obstacle coordinate information, and initialize the track point set Pa, the candidate track point set Ca and the processed obstacle set Ba to be empty sets;

[0101] Step 2: Add the drone flight starting point S to the point set Pa, and add the drone flight end point E to the point set Ca;

[0102] Step 3: Take the last track point in the point set Pa as the starting point O, and the last track point in the point set Ca as the target point D, and determine whether there is an obstacle intersecting the straight path OD:

[0103] If it exists, record the first intersecting obstacle and go to step 4;

[0104] If it does not exist, add point D to the point set Pa, delete point D from the point set Ca, and go to step 7;

[0105] Step 4, using the starting point O and the target point D as the starting point and the end point of the sub-path respectively, and using the tangent graph method to generate two horizontal sub-paths that bypass the obstacles from both sides of the first intersecting obstacle, and the two generated horizontal sub-paths are both located in the plane where the straight line SE is located, and the altitude of the UAV when flying along the two horizontal sub-paths continuously rises or continuously falls or remains unchanged. Considering the path length and obstacle avoidance, select one from the two horizontal sub-paths. In particular, this embodiment also considers the relevant conditions of the tangent of the target point (including the path length and obstacle avoidance), so as to ensure the accuracy of the flight heading of the UAV. Based on this, this embodiment designs the following four heuristic rules with priorities to select a better horizontal sub-path h_path from the two horizontal sub-paths. The four heuristic rules with priorities are mainly designed for maze-like environments, specifically:

[0106] (1) When selecting a horizontal sub-path, the tangent line of the starting point and the tangent line of the target point of the selected horizontal sub-path do not intersect with the latest obstacle added to the set Ba (the obstacle avoided last time), thereby preventing the drone from going back;

[0107] (2) Select a horizontal subpath where the tangent line of the starting point intersects with fewer obstacles;

[0108] (3) Select a horizontal subpath where the tangent line of the target point intersects with fewer obstacles;

[0109] (4) Select the horizontal sub-path with shorter path length;

[0110] For example Figure 4As shown in (b), in order to avoid obstacle B1, the tangent graph method is used to generate the sub-path S→F3→F2→F1→E. It is not difficult to find that the sub-path SF3 intersects with obstacle B2. Therefore, in the next obstacle avoidance process, the track point F3 is regarded as the new target point D;

[0111] Another example Figure 4 As shown in (c), the tangent graph method is used to generate two temporary horizontal sub-paths, namely S→F7→F6→F5→F3 and S→F4→F1→F2→F3. Obviously, the starting point tangent SF4 of the second sub-path collides with the obstacle B1 avoided last time, and this path guides the drone back. Therefore, according to the heuristic rule (1), the sub-path S→F7→F6→F5→F3 is selected as the horizontal sub-path h_path output in step 4;

[0112] Step 5, using the starting point O and the target point D as the starting point and the end point of the sub-path respectively, and using the tangent graph method to generate a vertical sub-path that bypasses the obstacle from the top of the first intersecting obstacle, and based on the heuristic rule, select one of the vertical sub-path and the horizontal sub-path h_path as the optimal sub-path p. Unlike two-dimensional trajectory planning, in three-dimensional space, the drone can bypass the obstacle from the top of the obstacle. The tangent graph method can generate a vertical sub-path from point O to point D, namely v_path, and the horizontal projection of the vertical sub-path v_path overlaps with the horizontal projection of the spatial straight line SE; in order to select the best sub-path p from the horizontal sub-path h_path and the vertical sub-path v_path, this embodiment designs the following three heuristic rules with priorities, which are:

[0113] 1) If the tangent line of the starting point of the vertical sub-path intersects with more obstacles, the horizontal sub-path is selected;

[0114] 2) If the target point tangent of the vertical subpath intersects more obstacles, the horizontal subpath is selected;

[0115] 3) Select the sub-path with shorter path length;

[0116] As is known to all, climbing or descending at a constant speed in the z-axis direction is usually more energy-efficient than reciprocating up and down, so this embodiment is more inclined to select the horizontal subpath. However, sometimes bypassing an obstacle from both sides consumes more energy than climbing over it, so the above three heuristic rules with priorities are designed for selection.

[0117] Step 6: Take the tangent point between the best subpath p and the first intersecting obstacle as the track point, and add the track points on the best subpath p to the point set Ca in reverse order of the best subpath p, and add the first intersecting obstacle to the set Ba;

[0118] Step 7, determine whether point E is in the point set Pa:

[0119] If so, output the point set Pa;

[0120] Otherwise, return to step 3.

[0121] The track points in the point set Pa output in step 7 all originate from the intersection points of the cubic obstacle. Therefore, in the final avoidance track, it is inevitable that some track points will be unnecessary and lead to detours. In order to solve this problem, this embodiment also simplifies the track to determine whether two adjacent edges can be reduced to one edge. If feasible, the two adjacent edges are reduced to one edge, and a simplified new track is obtained; otherwise, continue to determine whether the next two adjacent edges can be reduced to one edge. Repeat the above steps until the most streamlined track is generated. Figure 6 As shown, the waypoints F2 and F3 in the sub-path F1→F2→F3→F4 will produce a detour. Deleting the two track points can obtain a shorter sub-path F1F4. That is, in the specific implementation process, the UAV track planning method also includes:

[0122] Step 8, obtain the point set Pa output in step 7;

[0123] Step 9: randomly select three consecutive track points from the extraction point set Pa, and determine whether the line between the first track point and the third track point intersects with the obstacle:

[0124] If yes, return to step 9 until all track points in the point set Pa are traversed, and the point set Pa is output as the UAV track planning result;

[0125] Otherwise, delete the second track point from the point set Pa and return to step 9.

[0126] The UAV trajectory planning result output in step 9 is composed of multiple line segments. The angles between the line segments are large, and the tortuous trajectory is untraceable for the UAV. Therefore, this embodiment uses a cubic B-spline curve to smooth the trajectory. The trajectory points obtained in step 9 of 3D-TG are used as control points of the B-spline curve. Every four adjacent control points can determine a B-spline curve. In this way, a flight trajectory with smooth curvature can be obtained based on the initial broken line trajectory. In order to meet the minimum turning radius constraint r min , by modifying the position of the control point to reduce the local energy. Specifically, Figure 7 As shown, by ΔC i-1 C i C i+1 The radius of curvature of the determined curve r i ,for:

[0127]

[0128] Among them, l i It is side C i-1 C i+1 The length of h i It is the bottom edge C i-1 C i+1 of high.

[0129] like Figure 8 As shown, if r i <r min , update the control point C according to the following formula i , that is, C′ i :

[0130]

[0131] It should be noted that Figure 5 The implementation premise of the UAV trajectory planning shown is that the environmental information of the obstacles is completely known, so it can be understood as a static tangent graph method (a static tangent graph in 3-Denvironments, 3D-STG). In order to expand the UAV trajectory planning. This embodiment also provides a dynamic tangent graph method (a dynamic tangent graph in 3-D environments, 3D-DTG). Specifically, the dynamic tangent graph method is suitable for partially known scenarios, where the environmental model in 3D-DTG is updated in real time based on the information perceived in real time by the onboard sensors. The dynamic tangent graph method 3D-DTG is mainly for trajectory planning in partially unknown environments and completely unknown environments: the first scenario is that there are sudden obstacles, and the offline obstacle avoidance trajectory is generated by 3D-STG. Once the onboard sensor detects that a sudden obstacle is preventing the UAV from flying along the offline track, the 3D-DTG method will re-plan the infeasible offline sub-path to bypass the sudden obstacle. The specific planning process of the 3D-DTG method is the same as that of 3D-STG, except that the coordinate information of the sudden obstacle must be detected by the onboard sensor and then modeled, and then the same means as steps 2-9 in the 3D-DTG method can be used.

[0132] like Fig. 9As shown in , the second scenario of the 3D-STG method is trajectory planning in a completely unknown environment. Only the starting point and the end point are known, and the airborne sensors are used to obtain and update the environmental information. Based on the updated environmental information, the tangent graph method is used for trajectory planning. It is worth noting that there are two differences between 3D-STG and 3D-DTG: one is the acquisition of environmental information, and the other is that 3D-DTG limits the distance between two adjacent track points, which must be less than the detection distance of the sensor, thereby ensuring the safety of the drone during flight. Therefore, after obtaining a feasible track point, it should be judged whether the track point is feasible based on whether the distance between adjacent track points is less than the detection distance of the sensor. As Fig. 9 As shown in (a), obstacles B2 and B3 are detected at the starting point S. It is found that obstacle B2 hinders the drone's progress, so the tangent graph method is used to generate a track point F1 to bypass obstacle B2. Since the straight path SF1 does not intersect any obstacle and its length satisfies the finite flight distance constraint, the track point F1 is feasible. On the contrary, if Fig. 9 As shown in (c), path F3E is collision-free, but its length cannot meet the limited flight distance constraint. Therefore, the track is not extended directly to the end point E, but temporarily extended to the feasible waypoint F4 (the track point is on F3E and the distance from F3 is the maximum detection distance).

[0133] The present invention is further described below in conjunction with a specific simulation experiment. In the simulation experiment, the methods involved are all implemented in MATLAB language and tested on a computer with Windows 10 operating system, Intel i5-8400 2.80 GHz CPU, and 8 GB memory.

[0134] In order to verify the trajectory planning performance of 3D-STG in a conventional environment, this embodiment designs two scenarios, which are marked as E1 and E2 respectively. Fig.10 As shown in the figure, the layout of obstacles in each scene has unique spatial characteristics. Scene E1 has dense obstacles, and scene E2 has a long corridor and is full of dense obstacles. The range of the simulation scene is 2000×2000×100m 3 ,The flight speed of the drone is 25km / h, the detection distance of the sensor is 20m, the maximum pitch angle is 30°, the minimum turning radius is 8m, and the safety distance from obstacles is 10m.

[0135] This example selects four algorithms, RRT*, SAS, APF, and improved ant colony algorithm IACO, as comparison methods to verify the superiority of the 3D-STG method proposed in this example. It should be noted that in the face of a maze-like environment, the classic APF will cause the track to wander back and forth in the maze, so this example introduces a tangent point to enhance its ability to escape from simple traps. The comparison experiment results are shown in Figure 2. Fig.11 and as shown in Table 2. Table 2 mainly includes cases, scenario types, number of obstacles, track length, and running time (in seconds).

[0136] Table 2 Comparative experimental results of trajectory planning under conventional environment

[0137]

[0138] Fig.11 (a) shows that SAS always generates the shortest trajectory for cases C1-C10. However, for cases C11 and C12, SAS cannot plan a feasible obstacle-avoiding trajectory for the UAV within a limited time. This is because SAS searches for track points in the space surrounded by the pyramid and the sphere based only on the track length. In addition to the track length, 3D-STG also considers the obstacle avoidance of the track, which will help guide the UAV to escape from the corridor. Overall, 3D-STG has the best planning performance in terms of track length, followed by RRT* and IACO. APF performs well in solving cases C1-C8, while in dense and cluttered environments with corridors, such as cases C9-C12, APF plans the longest track length. This is mainly because the strong repulsive force generated by the obstacle can drive the UAV away from the obstacle, but the gravitational force generated by the end point will cause the track to oscillate back and forth in environment E2.

[0139] In terms of running time, 3D-STG only needs 0.3 seconds to plan an obstacle avoidance track in all cases, which proves that the 3D-STG track planning efficiency is the highest, followed by APF and RRT*. In addition, it can be seen from Table 2 that the more complex the scene, the more efficient the 3D-STG track planning. The reason is that 3D-STG only considers obstacles on the path from the current position to the end point, rather than all obstacles in the scene, which can effectively avoid some redundant calculations. In addition, the present embodiment finds an obstacle avoidance track by continuously connecting the tangents of the obstacles, without relying on point-to-point traversal. In contrast, SAS and IACO both find feasible track points in three-dimensional space through point-to-point traversal (grid-to-grid search), and their track point search time is highly dependent on the size and number of obstacles. Therefore, for environments with dense obstacles, the running time of both algorithms is relatively long. From Fig.11(b) It can be seen that IACO takes more time in environments with sparse obstacles (such as C1 and C2). This is mainly because the obstacles in cases C1 and C2 are larger in size and occupy more grids, so IACO needs to spend more time to search the grid.

[0140] In real life, there are maze-like application scenarios, such as drone logistics delivery in narrow alleys. For this reason, it is necessary to verify the trajectory planning performance of 3D-TG in a maze-like environment. Fig.12 As shown, 8 cases are designed. Due to the selection of the reference direction (i.e., the update of the starting point and the target point), 3D-STG can help the drone escape the simple maze in the designed case. Specifically, the reference direction is continuously updated in real time, sometimes toward a track point near the end point, thereby guiding the drone out of the maze. In the case designed in this embodiment, 3D-STG can plan a feasible avoidance track for the drone in a simple maze environment within 0.25s.

[0141] In order to verify the effectiveness of the proposed 3D-DTG method, two dynamic environments, namely partially or completely unknown environments, are considered in simulation experiments.

[0142] This embodiment designs four cases (C21-C24) to compare the trajectory replanning capabilities of 3D-DTG and APF when facing sudden obstacles. Fig.13 As shown. The offline tracks are all generated by 3D-STG. Table 3 shows that 3D-DTG can replan the conflicting offline sub-tracks within 0.15 seconds to avoid sudden obstacles. When 3D-DTG and APF replan for cases C21 and C22, the lengths of the generated tracks are not much different. Similarly, the time consumed by track replanning of 3D-DTG and APF is not much different. However, when faced with sudden obstacles (corridors) in cases C23 and C24, the tracks generated by APF are more likely to wander back and forth in the corridor, which will consume more time and result in longer tracks.

[0143] Table 3 Comparative experimental results of trajectory replanning under sudden obstacle scenarios

[0144]

[0145] This embodiment designs four cases (C25-C28) to compare the trajectory planning capabilities of 3D-DTG, APF, and RRT* algorithm based on rolling time domain (RHRRT*) in completely unknown environments. The environments of cases C25 and C27 are shown in E1, and the environments of cases C26 and C28 are shown in E2. The number of obstacles in C25 and C26 is 120, while the number of obstacles in C27 and C28 is 160. The experimental results are shown in Tables 4 and Fig.14 shown.

[0146] Table 4. Comparative experimental results of trajectory planning under completely unknown scenarios

[0147]

[0148] It can be seen from Table 4 that 3D-DTG outperforms RHRRT* and APF in the set cases in terms of both track length and running time. Especially in the environment with long corridors (C26, C28), the advantage of 3D-DTG is more obvious. Compared with RHRRT* and APF, the track length generated by 3D-DTG is shortened by about 20% and 50%, respectively, and the running time is reduced by about 65% and 40%, respectively. There are two reasons why RHRRT* takes a lot of time to plan the track. The first reason is that RHRRT* needs to select a sub-target in the rolling window for track planning each time until the end point appears in the rolling window. The second reason is that RHRRT* always finds the obstacle avoidance track leading to the sub-target through random sampling in the rolling window, which is very random. From Fig.14 As can be seen from (e) and (f), the trajectories generated by APF and 3D-DTG seem to be completely consistent. However, in fact, the trajectories generated by APF are prone to oscillation around obstacles, especially when encountering a simple maze environment. The repulsive force generated by obstacles will push the track points away from the obstacles. However, when facing a long corridor, the strong repulsive force will cooperate with the attractive force of the end point, resulting in track oscillation.

[0149] This embodiment also selects Fig.15 The real scene shown in the figure is used for trajectory planning, thereby verifying the practicability of the method proposed in this embodiment. The selected area is Tianma Apartment, Hunan University, Changsha (N28°10', E112°57'). Fig.16 The planned trajectories in static scenes and completely unknown environments are given.

[0150] from Fig.16 (a) It can be seen that 3D-STG can plan a feasible obstacle avoidance trajectory for the UAV in a narrow channel in 0.108 seconds, which verifies the efficiency of 3D-STG. Fig.16 As shown in (b), in a dynamic environment, based on real-time perceived environmental information, 3D-DTG plans an obstacle avoidance track within 0.2 seconds. In summary, in real scenarios, 3D-TG can quickly generate a better obstacle avoidance track for the UAV and achieve real-time planning.

[0151] In summary, for the problem of UAV trajectory planning in urban low-altitude environments, this embodiment proposes that the 3D-TG method can obtain three obstacle avoidance sub-paths by using the tangent graph method whenever encountering an obstacle, assisting the UAV to bypass the obstacle from both sides or above the obstacle. According to the designed heuristic rules, a better sub-path is selected from the three sub-paths. The UAV will fly along the path and use the tangent graph method to avoid obstacles until the UAV successfully reaches the destination. Simulation experiments show that, compared with traditional trajectory planning methods, in conventional environments, 3D-TG can efficiently search for a feasible obstacle avoidance trajectory that is approximately optimal. In particular, 3D-TG can help UAVs escape from simple mazes within a limited time. In addition, based on real-time perceived environmental information, 3D-TG can respond to sudden obstacles in a timely manner, perform trajectory replanning, and has real-time trajectory planning capabilities.

[0152] The above description is only a preferred embodiment of the present invention, and does not limit the patent scope of the present invention. All equivalent structural changes made by using the contents of the present invention specification and drawings under the inventive concept of the present invention, or directly / indirectly applied in other related technical fields are included in the patent protection scope of the present invention.

Claims

1. A UAV trajectory planning method based on three-dimensional tangent graph method in urban low-altitude environment, characterized in that: The steps include: Whenever the UAV encounters an obstacle, the tangent graph method is used to generate three obstacle avoidance tracks that can bypass the obstacle from both sides and above; Based on the heuristic rules, an optimal obstacle avoidance track is selected, the UAV is controlled to fly along the obstacle avoidance track, and the track is adjusted by the tangent graph method during the flight to avoid obstacles; This cycle repeats until the drone can successfully reach the destination; The UAV trajectory planning method specifically includes the following steps: Step 1: Establish an obstacle model containing all obstacle coordinate information and initialize the track point set Pa , candidate track point set Ca Set with processed obstacles Ba is an empty set; Step 2: Add the drone flight starting point S to the point set Pa , add the drone flight endpoint E to the point set Ca ; Step 3: Set the point set Pa The last track point in the O , the point set Ca The last track point in the D , determine whether there are obstacles and straight paths OD intersect: If it exists, record the first intersecting obstacle and go to step 4; If it does not exist, then point D Add to Point Set Pa , and at the same time D From point set Ca After deleting, go to step 7; Step 4: start point O , Target point D As the starting point and end point of the sub-path, the tangent graph method is used to generate two horizontal sub-paths that bypass the obstacle from both sides of the first intersecting obstacle, and based on the heuristic rule, the better horizontal sub-path is selected from the two horizontal sub-paths h_path ; Step 5: start point O , Target point D As the starting point and end point of the subpath, a vertical subpath is generated from the top of the first intersecting obstacle to bypass the obstacle using the tangent graph method, and based on the heuristic rule, the vertical subpath and the horizontal subpath are connected. h_path Select one as the best subpath p ; Step 6: The best subpath p The tangent point with the first intersecting obstacle is taken as the track point, and the optimal subpath is used. p The best subpaths are p Add the track points on to the point set Ca , and add the first intersecting obstacle to the collection Ba ; Step 7: Determine whether point E is in the point set Pa middle: If so, output the point set Pa ; Otherwise, return to step 3.

2. The method for unmanned aerial vehicle trajectory planning based on three-dimensional tangent graph method in urban low-altitude environment according to claim 1 is characterized in that: In step 4, the two generated horizontal sub-paths are both located in the plane where the straight line SE is located, and the altitude of the UAV while flying along the two horizontal sub-paths continuously rises or continuously falls or remains unchanged.

3. The method for unmanned aerial vehicle trajectory planning based on three-dimensional tangent graph method in urban low-altitude environment according to claim 2 is characterized in that: In step 4, the better horizontal sub-path is selected from the two horizontal sub-paths based on the heuristic rule. h_path , specifically: The following four heuristic rules with priorities are used to select the better horizontal subpath from two horizontal subpaths: h_path : When selecting a horizontal sub-path, the starting point tangent and the target point tangent of the selected horizontal sub-path do not intersect with the latest obstacle added in the set Ba; Select a horizontal subpath whose tangent line from the starting point intersects with fewer obstacles; Select a horizontal subpath where the tangent line of the target point intersects with fewer obstacles; Select the horizontal subpath with the shorter path length.

4. The method for unmanned aerial vehicle trajectory planning based on three-dimensional tangent graph method in urban low-altitude environment according to claim 1 is characterized in that: In step 5, the horizontal projection of the vertical sub-path overlaps with the horizontal projection of the spatial straight line SE.

5. The method for unmanned aerial vehicle trajectory planning based on three-dimensional tangent graph method in urban low-altitude environment according to claim 4 is characterized in that: In step 5, based on the heuristic rule, the vertical sub-path and the horizontal sub-path h_path Select one as the best subpath p , specifically: The following three heuristic rules with priorities are used to identify the vertical subpath and the horizontal subpath. h_path Select one as the best subpath p : If the tangent line of the starting point of the vertical subpath intersects more obstacles, the horizontal subpath is selected; If the target point tangent of the vertical subpath intersects more obstacles, the horizontal subpath is selected; Select the subpath with the shorter path length.

6. The method for unmanned aerial vehicle trajectory planning based on three-dimensional tangent graph method in an urban low-altitude environment according to any one of claims 1 to 5, characterized in that: Also includes: Step 8: Get the point set output from step 7 Pa ; Step 9: Extract point set Pa Randomly select three consecutive track points and determine whether the line between the first track point and the third track point intersects with the obstacle: If so, return to step 9 until the point set is traversed Pa After all the track points in the output point set Pa As the result of the UAV trajectory planning; Otherwise, add the second track point from the point set Pa After deleting, return to step 9.

Images