A UAV trajectory generation method based on point cloud nearest neighbor query

Abstract

The present invention relates to a method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query, which is applicable to rotor UAVs equipped with direct or indirect depth sensors for directly generating trajectories based on raw point clouds. Compared with traditional trajectory generation methods that consume a large amount of computing resources for online map maintenance, the present invention adopts a mapless planner that can directly abstract un-fused sensor data. At the same time, in order to maintain the original historical information, a finite memory data structure with a reliable proximity query algorithm is adopted, and a free space skeleton is extracted based on a sampling scheme, and high-quality trajectories are generated within the final flight corridor. The planner proposed by the present invention is different from other mapless planners, which can effectively extract and utilize environmental information, significantly improving the consistency and success rate of online replanning.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle (UAV) flight control, and specifically refers to a UAV trajectory generation method based on point cloud nearest neighbor query, which is applicable to rotor UAVs equipped with direct or indirect depth sensors for directly generating trajectories based on the original point cloud. Background Art

[0002] When a UAV performs a flight mission in a complex environment, it generally relies on depth sensors such as depth cameras and lidar to obtain the relative position information of obstacles, and builds a rasterized map based on this information to further plan an executable trajectory. However, online maintaining the fused rasterized map requires a large amount of memory and computational overhead, and in the case of poor positioning, a large amount of computing power is also required to maintain the consistency of the map to ensure the quality of the planning. Therefore, traditional environmental representation methods directly project sensor data into a three-dimensional space point cloud and use spatial segmentation such as building KD trees, R trees, etc. for management to facilitate nearest neighbor query of the point cloud. However, such a lightweight environmental representation form lacks an abstract expression of environmental information and is difficult to directly use for trajectory planning.

[0003] In the prior art [1] (Ryll Markus, et al. "Efficient trajectory planning for high speed flight in unknown environments", 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019), the states of the directly discretely sampled trajectories are used, and then the nearest neighbor query is used for safety detection and dynamic detection, and finally the executable trajectories are selected. Due to the limitations of the sampling resolution and the trajectory state representation ability, the trajectories generated by such methods of sampling the trajectories first and then detecting are far from optimal, unable to cope with relatively complex environments, and because there is no abstraction of the environmental information at all, the short-sightedness of the local planning is more obvious. In the prior art [2] (Gao Fei, et al. "Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments", Journal of Field Robotics, 2019, 36(4):710-733), by online maintaining a random search tree, and generating a flight corridor composed of a string of balls by this tree, and finally solving QCQP (quadratic constraint quadratic programming) in the flight corridor to optimize the trajectory expressed by the Bezier curve (refer to Z Wang, et al. "Alternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight", IEEE Robotics and Automation Letters, 2020, 5(3):4836-4843). This method can cope with relatively complex environments, but the back-end optimization requires a large overhead, and the time allocation of the trajectory can only be given in advance and cannot be optimized. At the same time, the random search tree is continuously pruned online according to the update of the point cloud, which has high requirements for positioning and cannot cope with the environment of dynamic obstacles.

[0004] In summary, the existing methods commonly have problems such as high requirements for computing power, inability to guarantee the quality of trajectory planning, and poor stability when applied to UAV trajectory generation. Summary of the Invention

[0005] In view of the problem that online maintenance of maps during the trajectory planning process consumes computing resources, and trajectory planning usually requires environmental abstraction through a well-fused map, the present invention proposes a method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query. The planner proposed by this method is different from traditional mapless planners, and can effectively extract and utilize environmental information, improving the consistency and success rate of online replanning.

[0006] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query, comprising the following steps:

[0008] Step 1: Store the point cloud data using a KD-tree data structure;

[0009] Step 2: When the UAV first passes through an unknown area, generate sampling points based on the RRT* method and construct a random sampling tree, where the vertices are the centers of spherical safety regions and the edges are the connecting safety regions between these regions;

[0010] Step 3: Sort all sampling points according to the distance to the starting position, and add them to the path search tree in the order from near to far;

[0011] Step 4: Score each leaf node of the path search tree according to the growth direction of the branches, and then prune the entire path search tree according to the scores;

[0012] Step 5: As new sampling points are generated, expand and rewire the path search tree to generate a series of connected spherical safety regions connecting the starting point and the ending point, that is, a safe flight corridor;

[0013] Step 6: When the UAV revisits an area that has already been passed, adopt a resampling strategy under prior guidance to generate sampling points to improve the consistency of the flight corridors generated in different replannings;

[0014] Step 7: According to the safe flight corridor, adopt the dynamic interpolation method to generate the final trajectory.

[0015] Further, in step 2, find the nearest point in the point cloud map through fast nearest neighbor search in the KD-tree, calculate the safety radius of any sampling point, and define a spherical safety region at the nearest point.

[0016] Further, the path search tree is a forward generation tree; when generating the path search tree, first sort the batch-sampled nodes according to the distance to the starting position, and then add them to the path search tree in the order from near to far.

[0017] Further, the specific manner of step 4 is as follows:

[0018] Step 4a: Score the leaf nodes using the obstacle position and the branch growth direction, where the obstacle position is obtained through nearest neighbor search;

[0019] Step 4b: Perform a post-order traversal of the entire path search tree and set the score of each node to the sum of the scores of its child nodes;

[0020] Step 4c: Perform a level-order traversal. For each branch, select the highest score among its sibling branches as the criterion, and prune the branches that are lower than this criterion by a certain proportion. The value range of the proportion is 30% - 50%.

[0021] Furthermore, in step 6, the method of generating sampling points using the prior-guided resampling strategy is as follows: Generate corresponding spherical safety regions according to the sampling results of each frame, and then estimate the feasible region distribution through the spherical safety regions of consecutive multiple frames to form a series of overlapping three-dimensional Gaussian distributions in space, and generate sampling points based on the three-dimensional Gaussian distributions.

[0022] Furthermore, for the dynamic interpolation method adopted in step 7, the position of the inserted point is selected at the edge of the intersection part of the spheres, and the one with the farthest distance from the existing trajectory is selected; by continuously finding the most suitable position in the safe flight corridor as the waypoint and iteratively optimizing the trajectory until the trajectory is restricted within the entire safe flight corridor.

[0023] The present invention has the following advantages compared with the prior art:

[0024] (1) Through the dynamic resampling strategy, the sampling area of the previous frame is queried with a higher probability. Based on the basic assumptions that the environmental changes during the UAV flight process have continuous characteristics and the UAV sensor readings do not mutate, the success rate of obtaining effective samples can be significantly improved. In addition, this method is a prior distribution-guided sampling, which indirectly improves the overlap rate with the previous planned trajectory, thereby improving the consistency with the historical planned trajectory.

[0025] (2) The scoring and pruning of the path search tree make full use of the obstacle position information obtained by the nearest neighbor query, obtain the degree to which the growth direction of the branch hits the obstacle, avoid local traps, and at the same time provide the skeleton of the passable area for multi-topology planning.

[0026] (3) In a complex environment, the number of node spheres for generating the safe flight corridor is generally about 10. This method only needs to insert 1 - 2 points to cover most scenarios through the dynamic interpolation strategy.

[0027] (4) Compared with the traditional trajectory generation method that consumes a large amount of computing resources for online maintenance of the map, the present invention adopts a mapless planner that can directly abstract the un-fused sensor data.

[0028] (5) To maintain the original historical information, the present invention adopts a finite-memory data structure with a reliable proximity query algorithm, extracts the free-space skeleton based on a sampling scheme, and generates high-quality trajectories within the final flight corridor.

[0029] (6) The planner proposed by the present invention is different from other mapless planners, can effectively extract and utilize environmental information, and significantly improves the consistency and success rate of online replanning. Description of the Drawings

[0030] Figure 1 It is the overall flowchart of the method of the embodiment of the present invention.

[0031] Figure 2 It is the schematic diagram of the safety radius of the sampling points obtained by the nearest neighbor query method of the point cloud in the embodiment of the present invention.

[0032] Figure 3 It is the schematic diagram of the dynamic resampling process of generating sampling points by using the resampling strategy under prior guidance in the embodiment of the present invention.

[0033] Figure 4 It is the schematic diagram of the implementation method of scoring and pruning the path search tree in the embodiment of the present invention.

[0034] Figure 5 It is the schematic diagram of the flight path obtained in the embodiment of the present invention. Detailed Embodiment

[0035] The technical solutions and effects of the present invention are further described in detail below with reference to the drawings.

[0036] A method for generating an unmanned aerial vehicle (UAV) trajectory based on nearest neighbor query of point cloud. The method first obtains a path search tree through a sampling method, and then obtains a flight corridor composed of a series of spheres, and then optimizes the trajectory in the corridor; secondly, abstracts the un-fused sensor data through a mapless planner, and adopts a finite-memory data structure with a reliable proximity query algorithm; finally, extracts the free-space skeleton based on a sampling scheme, and generates high-quality trajectories within the final flight corridor through an intelligent waypoint selection strategy.

[0037] Referring to FIG. 1, the method specifically includes the following steps:

[0038] Step 1: Store the point cloud data by using a KD-tree data structure.

[0039] Step 2: When the UAV first passes through an unknown area, generate sampling points based on the RRT* method and construct a random sampling tree; Refer to Figure 2, the generated sampling points find the nearest points in the point cloud map through fast nearest neighbor search in the KD tree, and calculate the safety radius of the sampling points accordingly. A spherical safety area is defined at this point, where the vertex is the center of the spherical safety area and the edges are the connecting safety areas between these areas.

[0040] Step 3: Sort all the sampling points according to their distances to the starting position, and add them to the tree in the order from near to far, generating a path search tree. This is mainly to avoid the time-consuming reconnection mechanism of RRT*, so that it can be quickly established without having to modify the previously generated tree online.

[0041] Step 4: As Figure 4 shown, score and prune the generated path search tree, score each leaf node according to the growth direction of the branches, where Figure 4 α in (b) represents the angle between the numerical direction and the direction of the sampling point towards the obstacle. The smaller α is, the greater the possibility of colliding with the obstacle and the smaller the score. Then prune the whole tree accordingly. Specifically, first score the leaf nodes, and then perform a post-order traversal of the whole tree. The score of each node is the sum of the scores of its child nodes, just as Figure 4 the score of each node in (c) is the sum of the scores of all its nodes. Then perform a level-order traversal. For each branch, select the highest score among its sibling branches as the criterion, and those below this criterion by a certain proportion are cut off. Through the above steps, most of the useless nodes can be quickly cut off, leaving the branches that truly represent the skeleton of the feasible area of the environment. Figure 4 (d) is the skeleton branch of the feasible area that is retained, where the scores of the sibling nodes are normalized, and the numbers represent the likelihood of selecting the leaf node.

[0042] Step 5: As new sampling points are generated, the tree is expanded and rewired to generate a series of connected spherical safety areas connecting the starting point and the ending point, that is, the safe flight corridor, as Figure 3 shown.

[0043] Step 6: Refer to Figure 3 , to further improve the consistency of the safe flight corridors generated in different replannings, a resampling strategy under prior guidance is adopted to generate sampling points. This scheme estimates the distribution of the feasible area through the sampling results of two consecutive frames or even multiple frames, forming a series of overlapping three-dimensional Gaussian distributions in space, and gradually transitioning the uniform sampling of the flight space to a sampling process with certain prior information. Among them, FoV represents the field of view of the sensor at a certain moment, S represents the position of the UAV, and Traj represents the UAV trajectory.

[0044] Step 7: Referring to FIG. 5, after generating a flight corridor composed of a series of spheres, the dynamic interpolation method is used to find the most suitable position in the flight corridor as the waypoint. The interpolation point position is selected at the edge of the intersection of the spheres, and the one farthest from the existing trajectory is selected. Continuously find the most suitable position in the flight corridor as the waypoint, and iteratively optimize the trajectory until the trajectory is restricted within the entire flight corridor to ensure the safety of the flight path. The trajectory generation at the backend uses the alternating optimization method, which can optimize the energy and time consumption of the trajectory simultaneously and also meet the dynamic constraints.

[0045] In summary, the present invention extracts the free space skeleton through a sampling-based scheme and generates high-quality trajectories within the final safe flight corridor through the selection strategy of intelligent waypoints. Different from other mapless planners, the present invention can effectively extract and utilize environmental information. Compared with traditional mapless methods, the consistency and success rate of online replanning in the present invention have been significantly improved, and it is applicable to rotor UAVs equipped with direct or indirect depth sensors to directly generate trajectories based on the original point cloud.

[0046] The above description is only a specific example of the present invention and does not constitute any limitation to the present invention. Obviously, for professionals in the field, after understanding the content and principle of the present invention, various modifications and changes in form and details may be made without departing from the principle and structure of the present invention. However, these modifications and changes based on the idea of the present invention are still within the protection scope of the claims of the present invention. For example:

[0047] 1. For the implementation of the path search tree, the path search tree of any sampling method can be an alternative to the present invention. Such as RRT (rapidly-exploring random tree), RRT*, FMT (fast marching tree), etc.

[0048] 2. For the scoring and pruning of the tree, any method that utilizes the judgment of the position of the nearest neighbor query obstacle and the growth direction of its own branches can be an alternative to the present invention.

[0049] 3. For the dynamic interpolation method, the trajectory optimization method after each interpolation point can be replaced by other optimization methods. Such as minimum snap.

Claims

1. A method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query, characterized in that, it includes the following steps: Step 1: Store the point cloud data using the KD-tree data structure; Step 2: When the UAV first passes through an unknown area, generate sampling points based on the RRT* method and construct a random sampling tree, where the vertices are the centers of spherical safety regions and the edges are the connecting safety regions between these regions; among them, the nearest points in the point cloud map are found through fast nearest neighbor search in the KD-tree, the safety radius of any sampling point is calculated, and a spherical safety region is defined at the nearest point; Step 3: Sort all sampling points according to the distance to the starting position and add them to the path search tree in the order from near to far; Step 4: Score each leaf node of the path search tree according to the growth direction of the branches, and then prune the entire path search tree according to the scores; Step 5: As new sampling points are generated, expand and rewire the path search tree to generate a series of connected spherical safety regions connecting the starting point and the ending point, that is, a safe flight corridor; Step 6: When the UAV revisits an area that has been passed through, adopt a resampling strategy under prior guidance to generate sampling points to improve the consistency of the flight corridors generated in different replannings; Step 7: According to the safe flight corridor, adopt the dynamic interpolation method to generate the final trajectory.

2. The method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query according to claim 1, characterized in that, the path search tree is a forward generation tree; when generating the path search tree, first sort the batch-sampled nodes according to the distance to the starting position, and then add them to the path search tree in the order from near to far.

3. The method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query according to claim 1, characterized in that, the specific method of Step 4 is: Step 4a: Score the leaf nodes using the obstacle position and the branch growth direction, where the obstacle position is obtained through nearest neighbor search; Step 4b: Perform a post-order traversal of the entire path search tree and set the score of each node to the sum of the scores of its child nodes; Step 4c: Perform a level-order traversal. For each branch, select the highest score among its sibling branches as the standard, and prune the branches that are lower than this standard by a certain proportion, and the value range of the proportion is 30% - 50%.

4. The method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query according to claim 1, characterized in that, in Step 6, the method of generating sampling points using the resampling strategy under prior guidance is to generate corresponding spherical safety regions according to the sampling results of each frame, and then estimate the feasible region distribution through consecutive multi-frame spherical safety regions to form a series of overlapping three-dimensional Gaussian distributions in space, and generate sampling points based on the three-dimensional Gaussian distribution.

5. The method for generating an unmanned aerial vehicle (UAV) trajectory based on point cloud nearest neighbor query according to claim 1, characterized in that, In the dynamic interpolation point method adopted in step 7, the position of the interpolation point is selected at the edge of the intersection of the balls, and the one with the farthest distance from the existing trajectory is selected; by continuously finding the most suitable position in the safe flight corridor as the waypoint and iteratively optimizing the trajectory until the trajectory is restricted within the entire safe flight corridor.

Images