Path planning algorithm for fast passing through narrow channel

Abstract

The invention provides a path planning algorithm for quickly passing through a narrow channel, which comprises the following steps of: acquiring a narrow channel identification graph according to a planning input graph, acquiring a narrow channel position from the narrow channel identification graph, automatically extracting a narrow channel center, judging whether a global planning path passes through the narrow channel, then replacing a global path point, and ensuring that the path is centered. Therefore, the machine can quickly pass through a narrow channel. According to the method, the high efficiency and safety of the robot passing through the narrow channel can be ensured, the task execution efficiency is improved, and the universality is high.

Description

Technical Field

[0001] The invention relates to the field of robot path planning, and in particular to a path planning algorithm for quickly passing through a narrow channel. Background Art

[0002] Due to multiple factors such as environmental interference and calibration errors, sensors have observation errors, so there are certain errors in the positioning and perception modules. This causes the data used by the planning module to not perfectly reflect the real world. When the robot follows the planned path in narrow areas, it often collides and cannot pass through these narrow areas. Many existing solutions are to escape or walk along the edge after problems occur. On the one hand, it is not efficient, and on the other hand, it may introduce new problems and has limited scene adaptability. Summary of the invention

[0003] In order to solve the problems of the prior art, the present invention provides a path planning algorithm for quickly passing through narrow passages, which automatically extracts the center of the narrow passage and ensures that the path is centered, so that the machine can quickly pass through the narrow passage, improves the task execution efficiency, and has strong versatility.

[0004] The present invention provides a path planning algorithm for quickly passing through a narrow passage, comprising the following steps:

[0005] 1) Obtaining a narrow road identification map according to the planning input map; the specific process is to process the planning input map through an image thinning algorithm to obtain the skeleton of the image, and while processing the image, record the number of layers of each skeleton point thinning.

[0006] 2) Obtain the narrow lane position from the narrow lane identification map; the specific method is to obtain a threshold value based on the machine size and image resolution information. The threshold formula is: Where s is the resolution, n and m are the gradients in the x and y directions, α and β are adjustment hyperparameters, and r is the machine radius. When the number of refinement levels is less than the threshold, the corresponding position is a narrow road, and these skeleton points that meet the requirements are recorded in groups.

[0007] 3) Obtain the narrow road polygon; the specific method is to traverse the grouping of skeleton points, expand them according to the number of refinement layers, and expand the number of pixels α is the adjustment hyperparameter, t is the number of refinement layers, and the expanded contour is obtained, and then polygon fitting is performed, and finally the skeleton points and their corresponding polygons are retained.

[0008] 4) Determine whether the global planning path crosses a narrow road. The specific algorithm is as follows:

[0009] 4.1) Select the global path starting point;

[0010] 4.2) Calculate the relationship between the point and the narrow channel polygon;

[0011] 4.3) Calculate whether the point is inside the narrow path polygon. If so, go to step 4.4); otherwise, go to step 4.5).

[0012] 4.4) Record the path points and bind them to the corresponding narrow path polygon index, denoted as the replacement path data set.

[0013] 4.5) Select the next point on the global path and repeat steps 4.2)-4.4) until all points on the global path are traversed.

[0014] 5) Replace the global path points. The specific algorithm is as follows:

[0015] 5.1) Select the first index of the narrow path polygon in the replacement path set.

[0016] 5.2) Remove the path points with the bound index from the global path points.

[0017] 5.3) Replace the skeleton points inside the index polygon on the global path in the front-to-back order. The point in front of the starting path point in the index is the point in front of the starting skeleton point in the index, and the point behind the ending path point in the index is the point behind the ending skeleton point in the index.

[0018] 5.4) Traverse the indexes in the replacement path data set and repeat steps 5.2)-5.3) until all replacements are completed.

[0019] 5.5) If the end point of the global path is inside the narrow path, use the end point of the global path as the point behind the ending skeleton point of the narrow path.

[0020] 6) Smooth the replaced path. Specifically, use the B-spline curve to slightly smooth the replaced global path.

[0021] 7) Modify the local planning logic. Specifically, when approaching the narrow path area, directly use the result of the global planning in the local planning, discard the local planning path, find the nearest point on the global planning path according to the current position point, follow the global planning path points, and ensure that the path is centered.

[0022] The beneficial effects of the present invention are as follows: Automatically extract the center of the narrow path, ensure that the path is centered, so that the machine can quickly pass through the narrow channel, improve the task execution efficiency, and have strong versatility. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0024] Figure 1 is the planning input graph;

[0025] Figure 2 is the narrow passage recognition graph. Detailed implementation manners

[0026] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0027] The present invention provides a path planning algorithm for quickly passing through a narrow passage, including the following steps:

[0028] 1) Obtain the narrow passage recognition graph (as shown in Figure 1 ) according to the planning input graph (as shown in Figure 2 ); The specific process is to process the planning input graph through an image thinning algorithm to obtain the skeleton of the image. While processing the image, record the number of thinning layers of each skeleton point.

[0029] 2) Obtain the narrow passage position from the narrow passage recognition graph; The specific method is to obtain a threshold according to the machine size and image resolution information. The threshold formula is where s is the resolution, n and m are the gradients in the x and y directions, α and β are adjustment hyperparameters, and r is the machine radius; Traverse the skeleton points. When the number of thinning layers is less than the threshold, the corresponding position is the narrow passage, and group and record these qualified skeleton points.

[0030] 3) Obtain the narrow passage polygon; The specific method is to traverse the groups of skeleton points and perform dilation processing according to their thinning layers. The number of dilated pixels α is an adjustment hyperparameter, t is the number of thinning layers, to obtain the dilated contour, then perform polygon fitting, and finally retain the skeleton points and their corresponding polygons.

[0031] 4) Determine whether the global planning path passes through the narrow passage. The specific algorithm is as follows:

[0032] 4.1) Select the starting point of the global path;

[0033] 4.2) Calculate the relationship between the point and the narrow passage polygon;

[0034] 4.3) Calculate whether the point is inside the narrow passage polygon. If so, go to step 4.4); otherwise, go to step 4.5);

[0035] 4.4) Record the path points and bind them to the corresponding narrow passage polygon indices, denoted as the replacement path dataset;

[0036] 4.5) Select the next point on the global path, and repeat steps 4.2)-4.4) until all global paths are traversed;

[0037] 5) Replace the global path points. The specific algorithm is as follows:

[0038] 5.1) Select the first index of the narrow passage polygon in the replacement path set;

[0039] 5.2) Remove the path points with the bound indices from the global path points;

[0040] 5.3) Replace the skeleton points within the index polygon with the front-to-back order onto the global path. The point before the starting path point within the index is the point before the starting skeleton point within the index, and the point after the ending path point within the index is the point after the ending skeleton point within the index;

[0041] 5.4) Traverse the indices of the replacement path dataset, and repeat steps 5.2)-5.3) until all replacements are completed;

[0042] 5.5) If the end point of the global path is within the narrow passage, then use the end point of the global path as the point after the ending skeleton point of the narrow passage;

[0043] 6) Smooth the replaced path; specifically, use a B-spline curve to slightly smooth the replaced global path.

[0044] 7) Modify the local planning logic; specifically, when approaching the narrow passage area, directly use the result of the global planning in the local planning, discard the local planning path, find the nearest point on the global planning path according to the current position point, follow the global planning path points, and ensure that the path is centered.

[0045] Each embodiment in this specification is described in a progressive manner. For the same or similar parts among the embodiments, reference can be made to each other. The key point of each embodiment is to illustrate the differences from other embodiments. In particular, for the device embodiments, the above description is only the preferred embodiment of the present invention. Since it is basically similar to the method embodiments, the description is relatively simple. For the relevant parts, reference can be made to the partial description of the method embodiments. The above is only the specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. For any person skilled in the art within the technical scope disclosed by the present invention, for those of ordinary skill in the art in this technical field, any changes or substitutions that can be easily thought of without departing from the principle of the present invention should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A path planning algorithm for quickly passing through narrow passages, characterized in that The following steps are involved: 1) Obtain a narrow road identification map based on the planning input map; 2) Obtaining the narrow lane position from the narrow lane identification map; 3) Obtain narrow road polygon; 4) Determine whether the global planning path crosses a narrow road. The specific algorithm is as follows: 4.1) Select the global path starting point; 4.2) Calculate the relationship between the point and the narrow channel polygon; 4.3) Calculate whether the point is inside the narrow road polygon. If yes, go to step 4.4), otherwise go to step 4.5); 4.4) Record the path points and bind them to the corresponding narrow road polygon indexes, which are recorded as the replacement path dataset; 4.5) Select the next point on the global path and repeat steps 4.2)-4.4) until the entire global path is traversed; 5) Replace the global path point. The specific algorithm is as follows: 5.1) Select the first index of the narrow road polygon in the replacement path set; 5.2) Remove the path point of the binding index from the global path point; 5.3) Replace the skeleton points in the index polygon to the global path in front-to-back order, with the front point of the starting path point in the index being the front point of the starting skeleton point in the index, and the back point of the last path point in the index being the back point of the last skeleton point in the index; 5.4) Traverse the replacement path dataset index and repeat steps 5.2)-5.3) until all replacements are completed; 5.5) If the end point of the global path is within the narrow passage, the end point of the global path is used as the rear point of the skeleton point at the end of the narrow passage; 6) Smoothing the replaced path; 7) Modify local planning logic.

2. The path planning algorithm for quickly passing through narrow passages according to claim 1, characterized in that: The specific process of obtaining the narrow road identification map according to the planning input map in step 1) is to process the planning input map through an image thinning algorithm to obtain the skeleton of the image, and while processing the image, record the number of layers of each skeleton point thinning.

3. The path planning algorithm for quickly passing through narrow passages according to claim 1, characterized in that: The specific method of obtaining the narrow road position from the narrow road identification map in step 2) is to obtain a threshold value according to the machine size and image resolution information. The threshold value formula is: Where s is the resolution, n and m are the gradients in the x and y directions, α and β are adjustment hyperparameters, and r is the machine radius. When the number of refinement levels is less than the threshold, the corresponding position is a narrow road, and these skeleton points that meet the requirements are recorded in groups.

4. The path planning algorithm for quickly passing through narrow passages according to claim 3, characterized in that: Step 3) The specific method of obtaining the narrow road polygon is to traverse the grouping of skeleton points, perform expansion processing according to the number of refinement layers, and expand the number of pixels. α is the adjustment hyperparameter, t is the number of refinement layers, and the expanded contour is obtained, and then polygon fitting is performed, and finally the skeleton points and their corresponding polygons are retained.

5. The path planning algorithm for quickly passing through narrow passages according to claim 1, characterized in that: Step 6) The specific method of the smoothing process is to use a B-spline curve to slightly smooth the replaced global path.

6. The path planning algorithm for fast passage through narrow passages according to claim 1, characterized in that: The specific method of modifying the local planning logic in step 7) is that when entering a narrow road area, the local planning directly uses the result of the global planning, abandons the local planning path, finds the nearest point on the global planning path according to the current position point, follows the global planning path point, and ensures that the path is centered.

Images