A sampling-based path planning method considering obstacle information

Abstract

The present invention discloses a sampling-based path planning method that incorporates obstacle information. The present invention first samples state points near obstacles in space, connects the sampling points of adjacent obstacles, and then constructs an obstacle-based path map. A search-based path planning method is executed on the path map to obtain an initial path; based on the initial path, a heuristic sampling set is constructed in the surrounding segments, and random sampling is performed again. Then, an optimal path is searched for through a fast marching tree method on the secondary sampling point set. The present invention effectively solves the problem of slow running speed caused by too many sampling points in the fast marching tree method, and provides a fast and optimal heuristic sampling and search method, thereby effectively improving the overall speed and optimality of path planning.

Description

Technical Field

[0001] The present invention relates to a path planning method, and in particular to a sampling-based path planning method based on obstacle information. Background Art

[0002] In recent years, autonomous driving has gradually entered the public eye. Both traditional automobile manufacturers and new forces in intelligent driving have invested a lot of energy in the research of assisted driving and autonomous driving technologies. The key technologies of autonomous driving include perception, mapping, decision-making, planning and control. The planning module receives information from upstream perception, mapping and decision-making to plan a feasible trajectory and hand it over to the control module for execution. It is one of the core technologies of autonomous driving.

[0003] Path planning methods can be divided into three categories: the first is graph search-based methods, such as A* method, Dijkstra method, etc.; the second is sampling-based methods, such as rapid random tree (RRT) method, probability map method (PRM) and state lattice planning method (State Lattice); in recent years, with the development of deep learning, learning-based path planning methods have gradually attracted attention. Among them, graph search-based methods have a slow search speed in high-dimensional space and unstructured space due to their dependence on grid maps, while learning-based methods require a large amount of data sets and training overhead. There is no universal training set for network training in the current path planning field. Sampling-based path planning methods benefit from their fast search capabilities in high-dimensional space and are widely used in the planning of unmanned vehicles, drones, etc.

[0004] However, there are still some problems with sampling-based path planning methods. For example, the RRT method is not asymptotically optimal and can only guarantee the search of a feasible path when there is a solution. Although the RRT* method guarantees optimality as the number of sampling points increases and the number of planning batches increases, its planning speed is not ideal. The fast marching tree method (FMT*) has a strong dependence on the quality of the initial sampling points. When the number of sampling points is insufficient, it is difficult to search for a satisfactory path, and too many sampling points will seriously limit the search speed. Summary of the invention

[0005] In view of this, in order to solve the problems existing in the background technology, the purpose of the present invention is to solve the dependence and uncertainty of the fast marching tree method on the quality of the initial sampling points, and propose a sampling-based path planning method that takes into account obstacle information. The first sampling is performed based on the obstacle to obtain the first batch of sampling sets. Based on the first batch of sampling sets, a path map is established and an initial path is searched. The initial path is used to establish a heuristic sampling set to obtain a second batch of sampling sets, so as to achieve a controllable number and range of sampling points, thereby searching for the optimal path more quickly and accurately.

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

[0007] Step 1: In a space where the obstacle position, starting point, and target point are known, random sampling is performed near the boundaries of each obstacle in the space to obtain sampling points at the boundaries of each obstacle and record them as the first batch of sampling sets;

[0008] Step 2: Use the K nearest neighbor method to connect the sampling points at the boundaries of different obstacles in the first batch of sampling sets to obtain the path map of the current space;

[0009] Step 3: According to the starting point and target point of the current space, the search-based path planning method is used to plan the path map of the current space to obtain the initial path;

[0010] Step 4: After the initial path is segmented, heuristic sampling areas are constructed in segments around the initial path to obtain multiple heuristic sampling areas, and the ranges of two adjacent heuristic sampling areas overlap in front and back.

[0011] Step 5: Randomly sample in each heuristic sampling area and form the second batch of sampling sets from all sampling points;

[0012] Step 6: According to the second batch of sampling sets, the fast marching tree method is used to search for sampling points on the basis of the initial path. When the target point is found, the sampling points selected in each step of the fast marching tree method are connected to obtain the target path; otherwise, the fast marching tree algorithm is re-executed until the target path is obtained.

[0013] In step 1, the area near the obstacle boundary is specifically a boundary ring composed of the space between the boundary of each obstacle and 0.5 m from the current obstacle boundary, and the boundary ring does not include the space where the obstacle is located.

[0014] In step 3, the search-based path planning method is the A* method.

[0015] In step 4, according to the sampling points of the first sampling set, the initial path is segmented according to the number of sampling points to obtain each local path, the number of sampling points of each local path is 3, and the sampling points between two adjacent local paths overlap by at least 2; a corresponding heuristic sampling area is constructed for each local path, the heuristic sampling area is an ellipse, the first sampling point and the last sampling point are used as the two foci of the ellipse, and the total path length of the current local path is used as the focal length.

[0016] In step 6, the formula for determining the search radius R of the fast marching tree method is as follows:

[0017]

[0018] Among them, r n represents the search radius on the nth sampling set, σ is the search radius control coefficient, d is the dimension of the planning problem, μ(X free ) is the Lomborg measure of the traversable area, ζ d is the volume / area of ​​the unit sphere / circle in d-dimensional space, and n is the number of sampling points in the corresponding heuristic sampling area in the second batch of samples.

[0019] The beneficial effects of the present invention are:

[0020] (1) The present invention combines the ideas of single-batch sampling method and multi-batch sampling method, and samples the configuration space in two batches, thereby overcoming the disadvantage of single-batch sampling that is overly dependent on the quality of the initial sampling point set, and avoiding the waste of time and computing space caused by repeated multi-batch sampling, thereby improving the calculation speed of path search;

[0021] (2) The present invention uses a graph search method to establish an initial solution with the help of an initial path map constructed with the first set of sampling points, thereby providing an effective heuristic strategy for sampling-based path planning methods. Different from traditional heuristic strategies, the initial solution guided by the graph search method has better convergence, that is, the sampling method can be used to quickly search for the optimal solution. At the same time, this method can be extended to other sampling-based path planning methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 It is a system flow chart of the present invention.

[0023] Figure 2 It is a comparison chart of the experimental results of the present invention and the existing method on the optimal path.

[0024] Figure 3 This is a comparison chart of experimental results on the calculation time between the present invention and the existing method. DETAILED DESCRIPTION

[0025] The implementation process of the present invention is described in detail below with reference to the accompanying drawings in the embodiments of the present invention.

[0026] like Figure 1 As shown, the present invention comprises the following steps:

[0027] Step 1: In the space where the obstacle position, starting point and target point are known, random sampling is performed near the boundaries of each obstacle in the space to obtain the sampling points of each obstacle boundary and record them as the first batch of sampling sets; in the specific implementation, the space is a 300×300㎡ simulation area. The boundary near the obstacle boundary is specifically a boundary ring composed of the space between the boundary of each obstacle and 0.5m away from the current obstacle boundary (excluding the space where the obstacle is located).

[0028] Step 2: Use the K nearest neighbor method to connect the sampling points at the boundaries of different obstacles in the first batch of sampling sets to obtain the path map of the current space; in the specific implementation, K in the K nearest neighbor method is 5.

[0029] Step 3: According to the starting point and target point of the current space, the search-based path planning algorithm is used to plan the path map of the current space to obtain the initial path;

[0030] In step 3, the search-based path planning algorithm is the A* algorithm. Step 4: After the initial path is segmented, heuristic sampling areas are constructed around the initial path in sequence to obtain multiple heuristic sampling areas, and the ranges of two adjacent heuristic sampling areas overlap in front and back; the heuristic function of the A* method is:

[0031] F(n)=H(n)+G(n)

[0032] Among them, F(n) represents the cost function of each sampling point, G(n) represents the actual cost value from the initial point to the sampling point, and H(n) represents the heuristic estimated cost value from the sampling point to the target point.

[0033] Step 4: After the initial path is segmented, heuristic sampling areas are constructed in segments around the initial path to obtain multiple heuristic sampling areas, and the ranges of two adjacent heuristic sampling areas overlap in front and back.

[0034] In step 4, based on the sampling points of the first sampling set, the initial path is segmented according to the number of sampling points to obtain each local path. The number of sampling points of each local path is 3, and the sampling points between two adjacent local paths overlap by at least 2. For each local path, a corresponding heuristic sampling area is constructed. The heuristic sampling area is an ellipse, and the first sampling point and the last sampling point are used as the two foci of the ellipse, and the total path length of the current local path is used as the focal length.

[0035] The heuristic sampling region is defined as:

[0036] S i ={x∈X free |‖xx start ‖2+‖xx goal ‖2 <c i}

[0037] Among them, S i represents the i-th heuristic sampling area, x represents the state point in the map, X free represents the traversable area in the current space, x start and x goal are the starting point and end point of the local path, c iIt represents the cost value from the starting point (i.e. the first sampling point) to the end point (i.e. the third sampling point) of the current local path, that is, the path distance, which is also the total path length.

[0038] Step 5: Randomly sample in each heuristic sampling area and form the second batch of sampling sets from all sampling points; the formula of sampling density ρ of each heuristic sampling area in sampling satisfies:

[0039] n=ρ*S

[0040] Among them, S is the area covered by the current heuristic sampling area, and n is the number of sampling points in the current heuristic sampling area.

[0041] Step 6: According to the second batch of sampling sets, the fast marching tree algorithm is used to search for sampling points on the basis of the initial path. When the target point is found, the sampling points selected in each step of the fast marching tree algorithm are connected to obtain the target path; otherwise, the fast marching tree algorithm is re-executed until the target path is obtained.

[0042] In step 6, the formula for determining the search radius R of the fast marching tree algorithm is as follows:

[0043]

[0044] Among them, r n represents the search radius on the nth sampling set, σ is the search radius control coefficient, which is a positive constant, d is the dimension of the planning problem, μ(X free ) is the Lomborg measure of the traversable area, ζ d is the volume / area of ​​the unit sphere / circle in d-dimensional space, and n is the number of sampling points in the corresponding heuristic sampling area in the second batch of samples.

[0045] In the specific implementation, a 300m×300m two-dimensional map was designed for the experiment. For each experimental result, the average value of ten experiments was selected as the final result. The evaluation criteria for the planning results were search time, path length, and optimality, where optimality was defined as the ratio of path length to the theoretical optimal path length. The experimental results are shown in Table 1. The effects of each method vary with the sampling density. Figure 2 and Figure 3 shown.

[0046] Table 1 Comparison of experimental results of the present invention and the existing method

[0047]

Claims

1. A sampling-based path planning method with added obstacle information, characterized in that: The following steps are involved: Step 1: In a space where the obstacle position, starting point, and target point are known, random sampling is performed near the boundaries of each obstacle in the space to obtain sampling points at the boundaries of each obstacle and record them as the first batch of sampling sets; Step 2: Use the K nearest neighbor method to connect the sampling points at the boundaries of different obstacles in the first batch of sampling sets to obtain the path map of the current space; Step 3: According to the starting point and target point of the current space, the search-based path planning method is used to plan the path map of the current space to obtain the initial path; Step 4: After the initial path is segmented, heuristic sampling areas are constructed in segments around the initial path to obtain multiple heuristic sampling areas, and the ranges of two adjacent heuristic sampling areas overlap in front and back. Step 5: Randomly sample in each heuristic sampling area and form the second batch of sampling sets from all sampling points; Step 6: According to the second batch of sampling sets, the fast marching tree method is used to search for sampling points on the basis of the initial path. When the target point is found, the sampling points selected in each step of the fast marching tree method are connected to obtain the target path; otherwise, the fast marching tree algorithm is re-executed until the target path is obtained; In step 4, according to the sampling points of the first sampling set, the initial path is segmented according to the number of sampling points to obtain each local path, the number of sampling points of each local path is 3, and the sampling points between two adjacent local paths overlap by at least 2; a corresponding heuristic sampling area is constructed for each local path, the heuristic sampling area is an ellipse, the first sampling point and the last sampling point are used as the two foci of the ellipse, and the total path length of the current local path is used as the focal length.

2. A sampling-based path planning method with added obstacle information according to claim 1, characterized in that: In step 1, the area near the obstacle boundary is specifically a boundary ring composed of the space between the boundary of each obstacle and 0.5 m from the current obstacle boundary, and the boundary ring does not include the space where the obstacle is located.

3. The sampling-based path planning method with added obstacle information according to claim 1, characterized in that: In step 3, the search-based path planning method is the A* method.

4. The sampling-based path planning method with added obstacle information according to claim 1, characterized in that: In step 6, the formula for determining the search radius R of the fast marching tree method is as follows: Among them, r n represents the search radius on the nth sampling set, σ is the search radius control coefficient, d is the dimension of the planning problem, μ(X free ) is the Lomborg measure of the traversable area, ζ d is the volume / area of ​​the unit sphere / circle in d-dimensional space, and n is the number of sampling points in the corresponding heuristic sampling area in the second batch of samples.

Images