RRT UAV Obstacle Avoidance Path Planning Method Based on LiDAR Detection

Abstract

The present invention relates to a method for obstacle avoidance path planning of an RRT unmanned aerial vehicle based on lidar detection, belonging to the field of obstacle avoidance path planning of unmanned aerial vehicles. The present invention improves the problem of low efficiency caused by excessive randomness of the traditional RRT algorithm by using lidar, and proposes a sampling point generation strategy based on the lidar detection field. By detecting obstacles within the detection range of the lidar detection field, the generation strategy of random sampling points is changed. When no obstacle is detected, the search tree extends towards the end point. When an obstacle is detected, the search tree temporarily stops extending towards the end point and quickly bypasses the nearby obstacles. The present invention reduces the randomness of path planning of the traditional RRT algorithm, improves the purposefulness of the planned path, reduces the distance of the path, and improves the efficiency of path planning.

Description

Technical Field

[0001] The present invention belongs to the technical field of UAV path planning, and particularly relates to an RRT UAV obstacle avoidance path planning method based on lidar detection. Background Technique

[0002] UAVs have the characteristics of low cost, low risk, and convenient use, so they are widely used in the military and civilian fields. During the process of UAVs performing specific tasks, a path planning algorithm is required to plan a safe and efficient route for them; at the same time, a large number of obstacles will appear in the UAV operation environment, and configuring sensors for the UAV to sense the environment will also greatly improve the safety of the UAV. As a kind of sensor, lidar has the advantages of high resolution, strong anti-interference ability, good detection performance, small volume and light weight, and is very suitable for UAVs to carry for collecting three-dimensional environmental information and detecting obstacles.

[0003] UAV path planning algorithms include various types of algorithms, such as graph search-based algorithms, sampling search-based algorithms, heuristic-based algorithms, etc. Traditional path planning algorithms, such as the Rapidly-exploring Random Tree (RRT) algorithm, is a path planning algorithm based on a random sampling strategy. By performing collision detection on the sampling points in the state space, it avoids the modeling of the space and can effectively solve the path planning problems in high-dimensional spaces and complex constraints. However, due to the randomness of the algorithm sampling of the RRT algorithm, the finally generated path is often only a feasible path rather than an optimal path. Summary of the Invention

[0004] The purpose of the present invention is to optimize the generation strategy of random sampling points by setting up a lidar detection field, reduce the randomness of the RRT algorithm, and improve the efficiency of the algorithm.

[0005] To achieve the above object, the present invention proposes an RRT UAV obstacle avoidance path planning method based on lidar detection, which specifically includes the following steps:

[0006] Step 1: Based on lidar mapping technology, initialize the three-dimensional map model. The user determines the target point G on the map, and takes the position of the UAV as the starting point as the root node of the search tree , denote the th generated node on the search tree , denote the newly generated node on the search tree, realize flight environment modeling and path planning parameter initialization;

[0007] Step 2: Take the newly generated node Taking the center of the sphere, the maximum detection distance of the lidar is used to establish a spherical surface with a radius of R , represents the newly generated spherical surface, represents the th generated spherical surface, and the lidar detection field modeling is completed;

[0008] Step 3: Determine whether intersects with the obstacle. If it does not intersect, the spherical surface and the center of the sphere of the sphere are called the non-intersecting spherical surface and the center of the non-intersecting spherical surface, respectively, and the sampling point is generated at the intersection of the line connecting and G and . If intersects with the obstacle, the sampling point is randomly generated on the spherical surface that does not intersect with the non-intersecting spherical surface and the obstacle, where intersecting with the obstacle means that the spherical surface has an intersection surface or intersection point with the obstacle model in the 3D map;

[0009] Step 4: Based on the nearest point expansion principle and the collision detection principle, let the Euclidean distance between the node in the search tree and be . Connect the point on the search tree that is the closest to to . Then the search tree generates a new node . After that, perform a collision detection on the newly generated path. If the detection fails, delete and return to step S3;

[0010] Among them, if intersects with the obstacle, the sampling point is randomly generated on the spherical surface that does not intersect with and the obstacle. The steps are as follows:

[0011] a. Determine the generated spherical surface: will only be generated on the spherical surfaces in the set . The probability of generating on the spherical surface is , where is the sum of the reciprocals of all Euclidean distances in the set ;

[0012] b. Determine : After determining the generated spherical surface , the sampling point is randomly generated on the part of the spherical surface that does not intersect with the obstacle; the li Represents a node q i to the target point G distance, l n Represents the n th node to the target point G distance;

[0013] Step 5: Detect whether the distance is less than the lidar detection distance , if then it is determined that the planning is successful, otherwise return to Step 2;

[0014] Step 6: When the drone moves along the planned path, the lidar detects the surrounding environment of the drone in real time. If an unknown obstacle is detected, the drone immediately stops moving forward and returns to Step 1; where the unknown obstacle is an obstacle detected by the lidar during the mission execution of the drone and not modeled in the 3D map model.

[0015] Advantageous effects: By establishing a lidar detection field, the present invention optimizes the generation method of sampling points on the basis of the traditional RRT algorithm, improves the purposefulness of sampling point generation when there are no obstacles nearby, and at the same time ensures the randomness of sampling point generation near obstacles, improves the efficiency of path planning, and optimizes the path. Brief description of the drawings

[0016] Figure 1 is a flowchart of the improved RRT algorithm based on lidar detection of the present invention.

[0017] Figure 2 is a schematic diagram of the search tree expansion method. Detailed implementation manners

[0018] The following details the implementation manners of the present invention.

[0019] As Figure 1 shown, the present invention discloses an RRT obstacle avoidance path planning method for drones based on lidar detection, and the specific steps are as follows:

[0020] Step 1: Manually control the drone equipped with a lidar to collect flight environment information, establish a 3D environment model according to the collected environment information, the drone determines its initial position according to the real-time detection information of the lidar, and the user determines the target point G on the map. Taking the position where the drone is located as the starting point as the root node of the search tree, represents the search tree the th generated node, represents the search tree The newly generated node at the top is used to implement flight environment modeling and initialize path planning parameters.

[0021] Step 2: With as the center of the sphere, a spherical surface is established with the maximum detection distance of the lidar as the sphere radius R , indicating the newly generated spherical surface, indicating the th generated spherical surface, and the lidar detection field modeling is completed.

[0022] Step 3: Determine whether intersects with the obstacle. If it does not intersect, the spherical surface and the center of the sphere of this sphere are called the non-intersecting spherical surface and the center of the non-intersecting spherical surface respectively. The sampling point is generated at the intersection of the line connecting and G and . If intersects with the obstacle, the sampling point is randomly generated on the part of the spherical surface that does not intersect with the obstacle; where intersecting with the obstacle means that the spherical surface has an intersection surface or intersection point with the obstacle model in the three-dimensional map.

[0023] Step 4: Based on the nearest point expansion principle and the collision detection principle, assume that the Euclidean distance between the node and in the search tree is . Connect the point on the search tree T that is closest to to , then the search tree generates a new node . Then, collision detection is performed on the newly generated path. If the detection fails, and are deleted and the process returns to Step 3;

[0024] is the distance formula, indicating the distance between and is the distance formula, indicating the distance between

[0025] Among them, if intersects with the obstacle, the sampling point is randomly generated on the spherical surface that does not intersect with the non-intersecting spherical surface and the obstacle. The steps are as follows:

[0026] Step 4.1, determine the generated spherical surface: will only be generated on the spherical surface in the set , on the spherical surface The generated probability is , where is the sum of the reciprocals of all Euclidean distances in the set ;

[0027] Step 4.2, determine : After determining the generated spherical surface , the sampling points are randomly generated on the part of the spherical surface that does not intersect with the obstacle;

[0028] The said l i represents the distance from the node q i to the target point G, l n represents the distance from the n th node to the target point G;

[0029] Step 5, detect whether the distance of is less than the detection distance of the lidar . If , it is determined that the path planning is successful, otherwise return to Step 2;

[0030] Step 6, when the UAV moves along the planned path, the lidar detects the surrounding environment of the UAV in real time. If an unknown obstacle is detected, it immediately stops moving forward and returns to Step 1.

[0031] The processes from Step 1 to Step 6 are as shown in the schematic diagram of the search tree expansion method Figure 2 . The search tree extends straight towards the end point without encountering obstacles. When the lidar detection field detects an obstacle, the sampling points are randomly generated on the spherical surface. The search tree has a tendency to bypass the obstacle. When the search tree bypasses the obstacle, the search tree continues to extend straight towards the end point. Among them, the solid black triangles and circles represent obstacles, the hollow circles represent the lidar detection field, S represents the starting point of path planning, and G represents the end point of path planning.

[0032] The present invention optimizes the generation method of sampling points on the basis of the traditional RRT algorithm by establishing a lidar detection field, improves the purposefulness of sampling point generation when there are no obstacles nearby, and at the same time ensures the randomness of sampling point generation near obstacles, improves the efficiency of path planning, and optimizes the path.

[0033] The above is only the preferred implementation manner of the present invention. Of course, it cannot be used to limit the scope of the rights of the present invention. It should be noted that for those skilled in the art of this technology, without creative labor, the modification or equivalent replacement of the technical solution of the present invention does not depart from the protection scope of the technical solution of the present invention.

Claims

1. A method for obstacle avoidance path planning of an RRT UAV based on lidar detection, characterized in that, Specifically, it includes the following steps: S1: Initialize a 3D map model based on lidar mapping technology. The user determines the target point G on the map, with the position of the drone as the starting point as the root node of the search tree and denote the nth generated node on the search tree and denote the latest generated node on the search tree, realizing flight environment modeling and path planning parameter initialization; S2: Taking the latest generated node as the center of the sphere, a spherical surface is established with the maximum detection distance of the lidar as the sphere radius R , denotes the latest generated spherical surface, denotes the th generated spherical surface, and the lidar detection field modeling is completed; S3: Determine whether it intersects with an obstacle. If not, the spherical surface and the center of the sphere of the sphere are respectively called the non-intersecting spherical surface and the center of the non-intersecting spherical surface, and the sampling point is generated at the intersection point of the line connecting and G and . If intersects with an obstacle, the sampling point is randomly generated on the spherical surface that does not intersect with and the obstacle; S4: Based on the nearest point expansion principle and the collision detection principle, assume the search tree with a node and has a Euclidean distance of . Connect the point on the search tree that is closest to to . Then the search tree generates a new node . After that, perform a collision detection on the newly generated path. If the detection fails, delete and and then return to step S3; S5: Detection Check if the distance is less than the detection distance of the lidar If it is determined that the planning is successful; otherwise, return to step S2 S6: When the drone moves along the planned path, the lidar detects the surrounding environment of the drone in real time. If an unknown obstacle is detected, it immediately stops moving forward and returns to step S1.

2. The RRT unmanned aerial vehicle obstacle avoidance path planning method based on lidar detection according to claim 1, wherein, In the said step S3 Intersecting with an obstacle means The spherical surface has an intersection surface or intersection point with the obstacle model in the three-dimensional map.

3. The RRT UAV obstacle avoidance path planning method based on lidar detection according to claim 1, wherein, If in the step S3 intersects with an obstacle, the sampling point is randomly generated on the sphere that does not intersect with the non-intersecting sphere and the obstacle, specifically: Let all the spheres that do not contain non-intersecting spheres be the set , and let the Euclidean distances from the centers of all the spheres in the set to the end point be the set , where the sampling points are generated in two steps: Step 1, determine the generation of the spherical surface: It will only be generated on the spherical surface in the set , and the generation probability on the spherical surface is , where is the sum of the reciprocals of all Euclidean distances in the set ; Step 2, determine : After determining to generate a spherical surface , sample points are randomly generated on the part of the spherical surface that does not intersect with obstacles; The said l i represents the node q i to the distance of the target point G, l n represents the n distance from the nth node to the target point G.

4. The RRT UAV obstacle avoidance path planning method based on lidar detection according to claim 1, wherein The unknown obstacle in step S6 is specifically: during the mission execution of the drone, if the lidar detects an obstacle that is not modeled in the 3D map model, then this obstacle is regarded as an unknown obstacle.

Images