A robot fast path planning method based on sparse visibility graph

Abstract

The present invention discloses a robot fast path planning method based on sparse visible graph, which is characterized by comprising: for the point cloud information collected by laser radar, point cloud downsampling is performed through PCL point cloud library to obtain sparse point cloud; the obtained sparse point cloud is stored using a hollow grid structure, and the sparse point cloud is plane-projected to obtain a binary image, and the image is blurred to obtain a blurred image; contour points are extracted from the blurred image to obtain contour feature points of obstacles, and the contour feature points of obstacles are filtered; a visible graph is constructed through the contour feature points of the filtered obstacles; a bidirectional BFS search is used to find the optimal path, and obstacles are eliminated during the path planning process. The algorithm of the present invention runs fast and has good real-time performance; it shortens the time required for map construction and path planning, and at the same time reduces the space waste caused by the robot exploring in an unknown environment, and has the advantages of strong resistance to interference from complex environments.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robots, relates to robot map construction and robot path planning technology, and specifically relates to a robot fast path planning method based on a sparse visibility graph. Background Art

[0002] In recent years, with the continuous development of the times, the mobile robot industry has been rising and gradually integrated into our lives. Guide service robots can be seen everywhere in shopping malls, food delivery robots in hotels, medical robots in hospitals, industrial robots in factories, etc. In the research on robots, especially mobile robots, in order to enable robots to better avoid obstacles on the road and reach the designated location during the action, the path planning algorithm is indispensable. The current path planning algorithm has the following main disadvantages: first, it is impossible to plan the path in an unknown environment; second, the current path planning mostly uses the grid map. In a relatively large area of ​​the scene, the efficiency of calculating the path using the grid map is low, and it is difficult to achieve good real-time performance.

[0003] Therefore, a new technical solution is needed to solve these problems. Summary of the invention

[0004] Purpose of the invention: In order to overcome the deficiencies in the prior art, a robot fast path planning method based on sparse visibility graph is provided, which has many advantages such as strong real-time performance, less exploration space, and strong resistance to environmental interference.

[0005] Technical solution: To achieve the above object, the present invention provides a robot fast path planning method based on sparse visibility graph, comprising the following steps:

[0006] S1: For the point cloud information S collected by the laser radar, the point cloud is downsampled through the PCL point cloud library to obtain a sparse point cloud S′;

[0007] S2: The obtained filtered sparse point cloud S′ is used as a hole grid structure Grid local Store and project the sparse point cloud S′ onto the plane to obtain a binary image Image origin , and convert the binary image Image origin Blur the image to get the blurred image Image blur ;

[0008] S3: Blur the image Image blur Extract contour points to obtain the contour feature points of obstacles Where Z+ is a positive integer, and the contour feature points of the obstacle are filtered;

[0009] S4: Construct a visible graph using the filtered obstacle contour feature points

[0010] S5: In the visible graph The bidirectional BFS (Breadth First Search) search is used to find the optimal path. The bidirectional BFS search opens up two search directions, one from the robot node to the end node, and the other from the end node to the robot node. In the process of path planning, based on the constructed visual graph Perform dynamic obstacle detection and maintain a visual map of the final The detected obstacles are removed and the robot planning path is finally obtained.

[0011] Furthermore, the hollow grid structure Grid in step S2 local A group of small grids local The Grid local Arranged in a hollow shape.

[0012] Furthermore, the contour feature points of the obstacle in step S3 The filtering method is:

[0013] For the kth obstacle contour There are n vertices in If n is greater than the threshold η (according to engineering experience, the threshold η can be selected as 18), the vertices of the obstacle need to be filtered. For the i-th vertex, the Euclidean distance between it and the i+1-th vertex is calculated. The formula is as follows:

[0014]

[0015] Calculate the kth obstacle contour based on the distance dist of each adjacent vertex The longest edge distance dist max , the formula is as follows:

[0016] dist max =max(dist (i，i+1) )|(0<i<n)

[0017] For the kth obstacle contour For each vertex in the i-th vertex, calculate the distance dist between the two consecutive vertices i-1 and i+1 before and after i. (i-1，i) and dist (i，i+1) , if dist (i-1，i) and dist (i，i+1) All are less than 0.1×dist max, then the vertex i is erased and the edge edge connected to vertex i is deleted (i-1，i) and edge (i，i+1) ;

[0018] If all dist (i-1，i) and dist (i+1，i) All greater than or equal to 0.1×dist max , then the obstacle contour feature points are completed Filtering.

[0019] Furthermore, in step S4, The construction method is:

[0020] A1: Obstacle contour points As vertices in the visible graph, adjacent vertices of the same obstacle constitute the edges of the obstacle in the visible graph;

[0021] A2: Visible graph From local map and global map It consists of two parts. After step A1, the obstacle contour points On local map is recorded as

[0022] A3: Pass Merge to global map form For local maps The kth obstacle contour vertex in is connected to The u-th vertex in is associated to complete a visible graph Updates.

[0023] Furthermore, in step S2, the point cloud is divided into obstacle point clouds S′ by the reflectivity in the sparse point cloud information S′. obs and non-obstacle point cloud S′ free .

[0024] Furthermore, in step S5, the visual image finally maintained The process of removing the detected obstacles is as follows:

[0025] B1: Initialize a grid of size R×R of the lidar detection radius voxel , the grid resolution is 0.5m×0.5m;

[0026] B2: S′ of the current frame obs Perform Euclidean distance matching with the next frame of sparse point cloud S′, and replace the unmatched S′ obs The point is marked as S′obs (RAY), the S′ obs (RAY) is expanded to form a point cloud that wraps the dynamic obstacles, thereby improving the robustness of eliminating dynamic obstacles. The expanded point cloud is marked as S′ obs (INFLATE);

[0027] B3: On the global map Eliminate S′ obs (INFLATE) corresponds to the obstacle contour point.

[0028] Furthermore, the specific process of the expansion operation in step B2 is:

[0029] Input point cloud S′ obs (RAY), traverse S′ obs Each point cloud P in (RAY) obs (RAY), get the point cloud P obs (RAY) corresponding Sub voxel Coordinate (x, y), expand it to nine neighborhoods, namely (x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1), and each Sub voxel Coordinates recorded in S′ obs (INFLATE).

[0030] The present invention converts the point cloud data information collected by the laser radar into the obstacle vertices and edge information in the visible graph through the hollow grid structure, and then improves the efficiency of the visible graph search by filtering the obstacle vertices, and finally uses the bidirectional BFS search for path planning. After the path planning method of the sparse visible graph is adopted, the time required for map construction and path planning is shortened, and the space waste caused by the robot exploring in an unknown environment is reduced, and it has the advantages of strong resistance to interference from complex environments.

[0031] The maintenance cost in the present invention specifically refers to the computer resource overhead and the speed of updating the visual map. The method adopted by the present invention reduces the resource overhead of computer operation and improves the update speed of the visual map, thereby reducing the maintenance cost of the visual map.

[0032] The path planning in the present invention involves the establishment of a map and a path search algorithm, and the path planning based on the visible graph uses the obstacle points and edges to search. Because the sparse method of establishing the visible graph in the present invention reduces the processing amount of real-time data, and also reduces the redundant obstacle points and edges, when searching for path planning, only fewer vertices and edges need to be traversed to find the path, thus shortening the time required for path planning.

[0033] In the present invention, since the search algorithm adopts a bidirectional BFS algorithm, the traversal speed of the algorithm itself is faster than that of a unidirectional BFS algorithm, and it can well prevent the unidirectional algorithm from entering a dead end. Due to this feature, the benefit brought in physical space is that the waste of exploration space is reduced.

[0034] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0035] 1. The present invention provides a method for extracting obstacle vertices and edge information from the point cloud information of the laser radar, and combines the algorithm for filtering redundant obstacle vertices and edges and eliminating dynamic obstacles, effectively reducing the maintenance cost of the visible graph in terms of computer resources and operating speed.

[0036] 2. The present invention reduces the time required for path planning by reducing the point cloud information of the laser radar to generate a sparse visible graph. At the same time, it adopts a bidirectional BFS search algorithm combined with a sparse visible graph to reduce the space waste caused by the robot's exploration in an unknown environment. It has the advantages of strong resistance to interference from complex environments, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is a flowchart of the method of the present invention;

[0038] Figure 2 is a schematic diagram of a hollow grid structure in the present invention;

[0039] Figure 3 is a schematic diagram of a fuzzy binary image in the present invention;

[0040] Figure 4 is a schematic diagram of optimizing obstacle contour points in the present invention;

[0041] Figure 5 It is a record diagram of obstacle vertices in the visible graph in a specific embodiment of the present invention;

[0042] Figure 6 It is a schematic diagram of the structure of the visible image composition in a specific embodiment of the present invention;

[0043] Figure 7 This is a rendering of the robot performing path planning in a specific embodiment of the present invention;

[0044] Figure 8 It is a time-consuming comparison diagram of processing radar data to generate a visual graph in a specific embodiment of the present invention;

[0045] Fig. 9 It is a time-consuming comparison diagram of the path search algorithm in a specific embodiment of the present invention. DETAILED DESCRIPTION

[0046] The present invention is further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. After reading the present invention, various equivalent forms of modifications to the present invention by those skilled in the art all fall within the scope defined by the claims attached to this application.

[0047] The present invention provides a robot fast path planning method based on sparse visibility graph, such as Figure 1 As shown, it includes the following steps:

[0048] S1: For the point cloud information S collected by the laser radar, the PCL point cloud library is used to downsample the point cloud to obtain a sparse point cloud S′. Figure 2 As shown in (a);

[0049] S2: The obtained filtered sparse point cloud S′ is used as a hole grid structure Grid local Store, and divide the point cloud into obstacle point cloud S′ by the reflectivity in the sparse point cloud information S′ obs and non-obstacle point cloud S′ free , and the sparse point cloud S′ is projected onto the plane to obtain a binary image Image origin , and convert the binary image Image origin Blur the image to get the blurred image Image blur ;

[0050] It should be noted here that the conventional grid structure is Figure 2 As shown in (b), the present invention designs a hollow grid structure Grid local For storage, the Grid local A group of small grids local The Grid local It is arranged in a hollow shape, such as Figure 2 (c) and Figure 2 (d) For a grid of a certain size, such as 30m×30m, the resolution of the grid (i.e. Sub local The size of the grid, such as 0.5m×0.5m, will affect the processing speed of the point cloud data S′, so a hollow grid structure Grid local Sub local Suppress the point cloud data within the image, thereby improving the efficiency of processing point cloud data;

[0051] Via Grid local After that, in Grid local The point cloud information in is projected into a binary image Image origin , where the edge contour of the obstacle is white and other positions are black;

[0052] The blurred image Image obtained in this step blur Specific as Figure 3 As shown;

[0053] S3: Blur the image Image blur Extract contour points to obtain the contour feature points of obstacles Where Z+ is a positive integer, and the contour feature points of the obstacle are filtered. The specific filtering method is as follows:

[0054] For the kth obstacle contour There are n vertices in If n is greater than the threshold η (according to engineering experience, the threshold η can be selected as 18), the vertices of the obstacle need to be filtered. For the i-th vertex, the Euclidean distance between it and the i+1-th vertex is calculated. The formula is as follows:

[0055]

[0056] Calculate the kth obstacle contour based on the distance dist of each adjacent vertex The longest edge distance dist max , the formula is as follows:

[0057] dist max =max(dist (i，i+1) )|(0<i<n)

[0058] For the kth obstacle contour For each vertex in the i-th vertex, calculate the distance dist between the two consecutive vertices i-1 and i+1 before and after i. (i-1，i) and dist (i，i+1) , if dist (i-1，i) and dist (i，i+1) All are less than 0.1×dist max , then the vertex i is erased and the edge edge connected to vertex i is deleted (i-1，i) and edge (i，i+1) ;

[0059] If all dist (i-1，i) and dist (i+1，i) All greater than or equal to 0.1×dist max , then the obstacle contour feature points are completed Filtering;

[0060] In this step, Figure 4 As shown in (a), this figure is the unoptimized obstacle contour points, such as Figure 4As shown in (b), the modified image is the effect after the obstacle contour points are filtered by the present invention. In the example, the effect obtained without using the method of the present invention is as follows Figure 5 As shown in (a), the effect of using the present invention is as follows Figure 5 As shown in (b), it can be seen that the obstacle contour points obtained by the method of the present invention are fewer, and the visual There are also fewer vertices to maintain.

[0061] S4: Construct a visible graph using the filtered obstacle contour feature points The specific construction method is:

[0062] A1: Obstacle contour points As vertices in the visible graph, adjacent vertices of the same obstacle constitute the edges of the obstacle in the visible graph;

[0063] A2: Visible graph From local map and global map It consists of two parts, as follows: Figure 6 As shown, the blue map is the global map The red area on the blue map is a local map The light yellow map is a schematic diagram of the enlarged local map; after step A1, the obstacle contour points On local map is recorded as

[0064] A3: Pass Taiwan merged to global map form For local maps The kth obstacle contour vertex in is connected to The u-th vertex in is associated to complete a visible graph Updates;

[0065] S5: In the visible graph The bidirectional BFS (Breadth First Search) search is used to find the optimal path. The bidirectional BFS search opens up two search directions, one from the robot node to the end node, and the other from the end node to the robot node. In the process of path planning, based on the constructed visual graph Perform dynamic obstacle detection and maintain a visual map of the final The detected obstacles are removed and the robot planning path is finally obtained.

[0066] In the process of path planning in step S5 above, the point cloud information of dynamic obstacles will be fed back to the visual map. In the visible The process of removing dynamic obstacles in the scene is as follows:

[0067] B1: Initialize a grid of size R×R of the lidar detection radius voxel , the grid resolution is 0.5m×0.5m;

[0068] B2: S′ of the current frame obs Perform Euclidean distance matching with the next frame of sparse point cloud S′, and replace the unmatched S′ obs The point is marked as S′ obs (RAY), the S′ obs (RAY) is expanded to form a point cloud that wraps the dynamic obstacles, thereby improving the robustness of eliminating dynamic obstacles. The expanded point cloud is marked as S′ obs (INFLATE);

[0069] The specific process of the expansion operation is:

[0070] Input point cloud S′ obd (RAY), traverse S′ obs Each point cloud P in (RAY) obs (RAY), get the point cloud P obs (RAY) corresponding Sub voxel Coordinate (x, y), expand it to nine neighborhoods, namely (x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1), and each Sub voxel Coordinates recorded in S′ obs (INFLATE);

[0071] B3: On the global map Eliminate S′ obs (INFLATE) corresponds to the obstacle contour point.

[0072] In order to verify the actual effect of the method of the present invention, the method of the present invention and the existing method are compared by example application in this embodiment, and the following results are obtained: Figure 7 The effect comparison diagram shown in the figure shows that the path planning effect of the existing method is as follows Figure 7 As shown in (a), the robot starts from the red point and reaches the green point, where the blue space area is the space explored by the robot. Figure 7(b) is a path diagram obtained by the method of the present invention. It can be seen that compared with the existing method, Figure 7 The path obtained in (b) is shorter and better, and the robot wastes less space exploration, which verifies the effectiveness of the method of the present invention.

[0073] Figure 8 The method adopted by the present invention can reduce the time consumption of processing radar data to generate a visible graph.

[0074] Fig. 9 This is the decision time consumed by adopting different schemes in robot path planning. It can be seen that the combination of the sparse visibility graph generated by the present invention and the bidirectional BFS search effectively reduces the decision time in robot path planning.

Claims

1. A robot fast path planning method based on sparse visibility graph, characterized in that: The steps include: S1: For the point cloud information S collected by the laser radar, the point cloud is downsampled through the PCL point cloud library to obtain a sparse point cloud S′; S2: The obtained filtered sparse point cloud S′ is used as a hole grid structure Grid local Store and project the sparse point cloud S′ onto the plane to obtain a binary image Image origin , and convert the binary image Image origin Blur the image to get the blurred image Image blur ; S3: Blur the image Image blur Extract contour points to obtain the contour feature points of obstacles Where Z+ is a positive integer, and the contour feature points of the obstacle are filtered; S4: Construct a visible graph using the filtered obstacle contour feature points S5: In the visible graph Bidirectional BFS search is used to find the optimal path. Bidirectional BFS search opens up two search directions, one from the robot node to the end node, and the other from the end node to the robot node. In the process of path planning, based on the constructed visual graph Perform dynamic obstacle detection and maintain a visual map of the final The detected obstacles are removed and the robot planning path is finally obtained; The contour feature points of the obstacle in step S3 The filtering method is: For the kth obstacle contour There are n vertices in If n is greater than the threshold η, the vertices of the obstacle need to be filtered. For the i-th vertex, the Euclidean distance between it and the i+1-th vertex is calculated. The formula is as follows: Calculate the kth obstacle contour based on the distance dist of each adjacent vertex The longest edge distance dist max , the formula is as follows: dist max =max(dist (i,i+1) )|(0<i<n) For the kth obstacle contour For each vertex in the i-th vertex, calculate the distance dist between the two consecutive vertices i-1 and i+1 before and after i. (i-1，i) and dist (i，i+1) , if dist (i-1，i) and dist (i,i+1) All are less than 0.1×dist max , then the vertex i is erased and the edge edge connected to vertex i is deleted (i-1，i) and edge (i,i+1) ; If all dist (i-1，i) and dist (i+1,i) All greater than or equal to 0.1×dist max , then the obstacle contour feature points are completed Filtering; In step S4, the visible The construction method is: A1: Obstacle contour points As vertices in the visible graph, adjacent vertices of the same obstacle constitute the edges of the obstacle in the visible graph; A2: Visible graph From local map and global map It consists of two parts. After step A1, the obstacle contour points On local map is recorded as A3: Pass Merge to global map form For local maps The kth obstacle contour vertex in is connected to The u-th vertex in is associated to complete a visible graph Updates; In step S2, the point cloud is divided into obstacle point clouds S′ by the reflectivity in the sparse point cloud information S′. obs and non-obstacle point cloud S′ free ; In the step S5, the visual map of the final maintenance The process of removing the detected obstacles is as follows: B1: Initialize a grid of size R×R of the lidar detection radius voxel ; B2: S′ of the current frame obs Perform Euclidean distance matching with the next frame of sparse point cloud S′, and replace the unmatched S′ obs The point is marked as S′ obs (RAY), the S′ obs (RAY) is expanded to form a point cloud that wraps the dynamic obstacle. The expanded point cloud is marked as S′o b s(INFLATE); B3: On the global map Eliminate S′ obs (INFLATE) corresponds to the obstacle contour point.

2. A robot fast path planning method based on sparse visibility graph according to claim 1, characterized in that: The hollow grid structure Grid in step S2 local A group of small grids local The Grid local Arranged in a hollow shape.

3. The method for rapid robot path planning based on sparse visibility graph according to claim 1, characterized in that: The specific process of the expansion operation in step B2 is: Input point cloud S′ obs (RAY), traverse S′ obs Each point cloud P in (RAY) obs (RAY), get the point cloud P obs (RAY) corresponding Sub voxel Coordinate (x, y), expand it to nine neighborhoods, namely (x-1, y-1), (x-1, y), (x-1, y+1), (x, y-1), (x, y), (x, y+1), (x+1, y-1), (x+1, y), (x+1, y+1), and each Sub voxel Coordinates recorded in S′ obs (INFLATE).

Images