Point cloud-based edge path planning method

Abstract

This invention provides a point cloud-based edge-path planning method. The process includes acquiring real-time sensor data and fusing it into point cloud data based on the machine's current coordinates; cropping and downsampling the point cloud; searching for a continuous point cloud along the edge direction as the basis for edge-path planning; calculating the point cloud projection and obtaining the edges of the projected point cloud; identifying the corner points of the point cloud, slicing the point cloud at the corner points, and then translating the sliced ​​point cloud according to the edge distance and edge direction; pruning and interpolating the translated point cloud segments; and finally, smoothing and optimizing the stitched point cloud to obtain a basic obstacle-based edge path. This invention dynamically generates paths based on sensor data and the robot's current coordinate system, is sensitive to dynamic obstacles, can adjust and generate new paths, and has improved accuracy compared to map-based path planning. It also allows for arbitrary adjustment of the edge distance and direction.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mobile robot planning control, and particularly relates to a point cloud-based edge path planning method. BACKGROUND

[0002] With the development of mobile robot technology, related technologies have been applied and developed in many fields such as military, industry and civil use, and the iteration of technology in practical feedback has further promoted the maturity of mobile robot technology.

[0003] At present, in the civil market, the household intelligent cleaning robot is the main application product of mobile robot technology, and the development momentum is rapid. Relatively, the product development of outdoor mobile cleaning robots is slow, mainly because the outdoor scene is more complex, and more sensor data fusion and more complex logic judgment are needed. As a cleaning robot, it is necessary to ensure the efficiency and effect of cleaning. In a static environment, it needs to be able to clean the road surface under the curb or guardrail on the roadside. In most cases, the path planning of the mobile robot is planned according to the static map or dynamic map. Since the map will lose accuracy due to the resolution in the generation process, the map will also be updated during the movement of the robot, so the edge path planned when cleaning the edge of the obstacle may cause the cleaning effect of the robot on the edge area to be poor. SUMMARY

[0004] The present application provides a point cloud-based edge path planning method to solve the problems of the prior art, which dynamically generates a path based on sensor data and the current coordinate system of the robot, is sensitive to dynamic obstacles, can adjust to generate a new path, and has improved path accuracy compared to map-based path planning, and can randomly adjust the distance and direction of the edge.

[0005] The present application provides a point cloud-based edge path planning method, comprising the following steps:

[0006] 1) obtaining real-time sensor data;

[0007] 2) fusing and converting the real-time sensor data into point cloud data based on the current coordinates of the robot;

[0008] 3) cropping and down-sampling the point cloud;

[0009] 4) reversely searching a continuous piece of point cloud from the edge direction as the basis for edge path planning;

[0010] 5) projecting the point cloud;

[0011] 6) obtaining the edge of the projected point cloud;

[0012] 7) Identify the corner points of the point cloud, slice the point cloud at the corner points, and then translate the sliced point cloud according to the edge distance and edge direction;

[0013] 8) Pruning and interpolation of the translated point cloud segment;

[0014] 9) Smooth optimization of the spliced point cloud to obtain the basic obstacle edge path.

[0015] Further improvement, in the process of acquiring real-time sensor data in step 1), after receiving sensor data with different ids, search for other sensor data frames with similar timestamps within a certain time difference before and after the timestamp of the sensor data frame with the least data.

[0016] Further improvement, in step 2), according to the installation position of the sensor on the machine, the point cloud is converted to the machine coordinate system by rotation-translation:

[0017] X'=R·X+T

[0018] X' is the rotated point cloud, R is the rotation matrix, X is the original point cloud, and T is the translation matrix. The formula is expanded as:

[0019]

[0020] Where R can also be represented as:

[0021] R=R z ·R y ·R x

[0022]

[0023]

[0024]

[0025] Further improvement, in the process of cutting and downsampling in step 3), according to the required edge sensing distance and edge direction, the point cloud is cut and downsampled, which includes:

[0026] 3.1) According to the machine shape, cut off the noise point cloud reflected by the machine itself;

[0027] 3.2) According to the edge path distance dist to be predicted, cut off the point cloud outside the radius dist;

[0028] Further improvement, in the process of cutting and downsampling, set the voxel size, establish the voxel filter to filter out redundant point clouds, and keep the first point cloud falling within the voxel.

[0029] Further improvement, in step 5), the orthogonal projection [x', y'] of the three-dimensional point [x, y, z] on the x-y plane is calculated by the following formula:

[0030]

[0031] In the formula, [t x t y ] T = [0 0], s = 1.

[0032] Further improvement, in step 6), the edge of the point cloud is obtained according to the edge coefficient and the search angle, which specifically includes:

[0033] 6.1) Construct a kd-tree;

[0034] 6.2) Calculate the neighborhood radius of the point cloud;

[0035] 6.3) The points at the non-edge are distributed around the point, and the points at the edge are gathered in a certain direction. The edge coefficient F is used to describe the degree of gathering of the point distribution in the neighborhood of the point. The edge coefficient is defined as:

[0036]

[0037] 6.4) When the vector is changed to the vector , a smaller search angle is set to limit the traversal points and the search direction vector, and the angle between the traversal point and the machine position (origin) is less than 90°, so that the edge point cloud close to the machine side is obtained.

[0038] Further improvement, in step 7), the front and rear windows are slid in a fixed window, the angle between the front and rear window edge point clouds is compared to determine whether it is a corner point, all corner points are found by traversing the two-dimensional point cloud, the point cloud slice is cut at the corner point, the straight line L is fitted by the point cloud segment, and the perpendicular V of the straight line L is calculated. The point cloud segment is translated along the perpendicular V to the edge direction by a set distance.

[0039] Further improvement, in the pruning and interpolation process of step 8), part of the translated point cloud exists between the obstacle and the edge path, resulting in poor or being separated far away from the adjacent point cloud;

[0040] For the intersection part, the kd-tree (k-dimensional tree) is used to quickly search for the two closest points in the two intersecting point cloud segments; through these two points, the two point cloud segments can be divided into two, a total of four point cloud segments; then the vector cross product is used to discard the point cloud inside the edge, that is,

[0041] For the separated part which is too far away, linear Bezier curve interpolation method is adopted, and the formula is as follows:

[0042] B(t)=p0+(p1-p0)*t[0≤t≤1]。

[0043] Further improvement, the path optimization process in step 9) adopts a moving least square method to fit a smooth path, and the specific steps are as follows:

[0044] 9.1) rasterize the fitting area;

[0045] 9.2) traverse the point cloud in the grid:

[0046] Determine the size of the influence area of the grid point cloud p;

[0047] Determine the nodes contained in the influence area of p;

[0048] Calculate the shape function O k (x).

[0049] The present application has the beneficial effects that:

[0050] 1. The method does not require a map, and the path is generated based on sensor data and the current coordinate system of the robot.

[0051] 2. The edge-following path is dynamically generated and is sensitive to dynamic obstacles, and can adjust to generate a new path.

[0052] 3. Compared with the path based on map planning, the edge-following path is based on the point cloud cutting and translation of obstacles, and is dynamically adjusted during movement, so the accuracy is improved;

[0053] 4. The dynamically generated path is more flexible, and the distance and direction of edge-following can be adjusted at will. BRIEF DESCRIPTION OF DRAWINGS

[0054] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0055] Figure 1 A schematic diagram of the edge-following path planning based on point cloud;

[0056] Figure 2 A schematic diagram of the edge-following path planning system based on point cloud;

[0057] Figure 3 A flowchart of the edge-following path planning based on point cloud;

[0058] Figure 4 Schematic diagram for synchronously acquiring sensor data. DETAILED DESCRIPTION

[0059] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work are within the protection scope of the present application.

[0060] The principle of the present application is shown in Figure 1 , an example of the left side of the machine along the edge, and the edge distance is 0.5. The solid line segment is the obstacle, and the dot on the solid line segment is the converted point cloud of the sensor, wherein the triangular point is the identified obstacle corner point; the dashed line segment is the edge path, and the quadrilateral point on the dashed line segment is the path point obtained according to the obstacle shearing translation.

[0061] Referring to Figure 2 and Figure 3 , the specific process of the present application is as follows:

[0062] 1. Synchronously acquiring sensor data:

[0063] A sensor data buffer band is established, and the sensor data is not output immediately after being received. When sensor data of different ids are all received, the time stamps of other sensor data frames with similar time stamps are searched within a time difference before and after the data frame with the least time stamp.

[0064] As shown in Figure 4 , pointcloud1_4, pointcloud2_3, and pointcloud3_2 are considered as time-synchronized data frames.

[0065] 2. Converting the point cloud to the machine coordinate system:

[0066] According to the installation position of the sensor on the machine, the point cloud is converted to the machine coordinate system through rotation-translation.

[0067] X' = R·X + T

[0068] X' is the rotated point cloud, R is the rotation matrix, X is the original point cloud, and T is the translation matrix. The formula is expanded as:

[0069]

[0070] Wherein R can also be expressed as:

[0071] R = R z ·R y• R x

[0072]

[0073]

[0074]

[0075] 3. Point cloud clipping, down-sampling:

[0076] According to the machine shape, clip the noise point cloud which is blocked by the machine itself;

[0077] According to the edge path distance dist to be predicted, clip the point cloud outside the radius dist;

[0078] Set the voxel size, establish the voxel filter to filter out the redundant point cloud, in order to keep the authenticity of the data and speed up the filtering speed, keep the first point cloud falling in the voxel, do not do other operations.

[0079] 4. Point cloud projection:

[0080] Find the orthogonal projection [x', y'] of the three-dimensional point [x, y, z] in the x-y plane, which has the following formula:

[0081]

[0082] Since the point cloud is converted to the machine coordinate system, and projected along the z-axis of the machine, [t x t y ] T =

[00] , s = 1.

[0083] 5. Get the edge of the projected point cloud: get the point cloud edge according to the edge coefficient and the search view angle.

[0084] 5.1) First, construct a kd-tree to realize fast search;

[0085] 5.2) Calculate the point cloud neighborhood radius;

[0086] 5.3) The points at the non-edge place, the points in its neighborhood are distributed around the point; the points at the edge place, the points in its neighborhood are gathered in a certain direction. Here, the edge coefficient F is used to describe the gathering degree of the point distribution in the point neighborhood. The edge coefficient is defined as:

[0087]

[0088] 5.4) When the vector to the vector The angle between the two vectors is called the search angle. Setting a smaller search angle can avoid the edge search falling into a local dead loop. In addition, by limiting the traversal point and the search direction vector, and the angle between the traversal point and the machine position (origin) is less than 90°, the edge point cloud close to the machine side can be obtained.

[0089] 6. Corner point recognition:

[0090] The basic idea of corner point detection is to slide forward and backward in a fixed window, compare the angle between the edge point clouds of the front and back windows to determine whether it is a corner point. The window size is called the corner point recognition radius;

[0091] All corner points are found by traversing the 2D point cloud, and the point cloud is sliced at the corner points.

[0092] 7. Geometric operation:

[0093] The point cloud segment obtained from step 6 is considered to be a relatively flat point cloud segment within the set corner point recognition radius. The point cloud segment is fitted to obtain a straight line L, and the perpendicular V of the straight line L is calculated. The point cloud segment is translated in the direction of the edge by a set distance along the perpendicular V.

[0094] 8. Pruning and interpolation:

[0095] The point cloud segment after step 7 translation may have overlapping parts or parts separated too far apart.

[0096] For overlapping parts, kd-tree is used to quickly search for the two closest points in the two overlapping point cloud segments. Through these two points, the two point cloud segments can be divided into two segments, a total of four segments. Vector cross product is used to distinguish which of the four point cloud segments exist inside the edge, and the point cloud inside the edge is discarded;

[0097] For parts that are too far apart, linear Bezier curve interpolation is used, with the formula as follows:

[0098] B(t) = p0 + (p1 - p0) * t [0 ≤ t ≤ 1].

[0099] 9. Path optimization:

[0100] Path optimization uses the moving least squares method to fit a smooth path, with the following steps:

[0101] 9.1) Grid the fitting area;

[0102] 9.2) Traverse the point cloud in the grid:

[0103] Determine the size of the influence area of the grid point cloud p;

[0104] Determine the nodes contained in the influence area of p;

[0105] Computing the shape function O k (x).

[0106] Each of the embodiments in the specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other. Each of the embodiments focuses on the difference from other embodiments. In particular, for the device embodiments, the above description is only the preferred embodiment of the present application, and since it is basically similar to the method embodiments, it is described more simply, and the relevant parts can be referred to the part of the method embodiments. The above description is only the specific embodiment of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art can easily think of changes or replacements within the technical range disclosed by the present application, and the changes or replacements within the technical range disclosed by the present application should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A point cloud based border path planning method, characterized in that The method comprises the following steps: 1) obtaining real-time sensor data; 2) converting the fused real-time sensor data into point cloud data based on the current coordinates of the machine; 3) cropping and down-sampling the point cloud; 4) reversely searching a continuous piece of point cloud along the edge direction as the basis for edge path planning; 5) projecting the point cloud; 6) obtaining the edge of the projected point cloud; 7) identifying the corner points of the point cloud, slicing the point cloud at the corner points, and then translating the sliced point cloud along the edge direction according to the edge distance and the edge direction; 8) pruning and interpolating the translated point cloud segments; 9) smoothing and optimizing the spliced point cloud to obtain a basic edge path of the obstacle.

2. The point cloud based follow-the-edge path planning method of claim 1, wherein: In step 1), when the sensor data of different IDs are all received, the time stamp of the sensor data frame is searched within a time difference before and after the time stamp to find other sensor data frames with similar time stamps.

3. The point cloud based follow-the-edge path planning method of claim 1, wherein: In step 2), the point cloud is converted to the machine coordinate system by rotation and translation according to the installation position of the sensor on the machine. ； is the rotated point cloud, is the rotation matrix, is the original point cloud, is the translation matrix, the formula expands to: ； wherein is represented by: ； ； ； 。 4. The point cloud based frontier path planning method of claim 1, wherein: In step 3), the point cloud is cropped and down-sampled along the edge direction according to the required edge sensing distance, which specifically includes: 3.1) according to the machine shape, cutting off the noise point cloud reflected by the machine itself; 3.2) according to the edge path distance to be predicted, cutting off the point cloud outside the radius.

5. The point cloud based follow-the-edge path planning method according to claim 1 or 4, characterized in that: In the cropping and down-sampling process, the voxel size is set to filter out redundant point clouds and retain the first point cloud falling within the voxel.

6. The point cloud based frontier path planning method of claim 1, wherein: In step 5), the three-dimensional point is found The orthogonal projection onto the x-y plane using the following formula: ； In the formulae, is , take 1.

7. The point cloud based frontier path planning method of claim 1, wherein: In step 6), the point cloud edge is obtained according to the edge coefficient and the search angle, which specifically includes: 6.1) constructing a kd-tree; 6.2) calculating the neighborhood radius of the point cloud; 6.3) the points at the non-edge position have points distributed around them in their neighborhood; the points at the edge position have points gathered in a certain direction in their neighborhood; the edge coefficient F is used to describe the degree of gathering of the point distribution in the neighborhood, and the edge coefficient is defined as: ； 6.4) When from vector to vector , set a smaller search view angle, limit the traversal point and the search direction to the vector, and the angle between the traversal point and the machine position (origin) is less than 90°, get the edge point cloud close to the machine side.

8. The point cloud based frontier path planning method of claim 1, wherein: In step 7), the window is slid forward and backward within a fixed window, and the angle between the edge point clouds of the front and rear windows is compared to determine whether it is a corner point. All corner points are found by traversing the two-dimensional point cloud, the point cloud is sliced at the corner points, a straight line L is fitted from the point cloud segment, and the vertical line V of the straight line L is calculated. The point cloud segment is translated along the vertical line V in the edge direction by a certain distance.

9. The point cloud based frontier path planning method of claim 1, wherein: In step 8), after translation, part of the point cloud exists between the obstacle and the edge path, resulting in a large difference or being separated far away from the adjacent point cloud. For the crossing part, the two closest points in the two crossing point cloud segments are quickly searched using a kd-tree; the two point clouds are divided into two segments through these two points, a total of four segments; the points on the inside of the edge are discarded using vector cross product; For the part that is too far apart, a linear Bezier curve interpolation method is used, and the formula is as follows: 。 10. The point cloud based frontier path planning method of claim 1, wherein: In step 9), the moving least squares method is used to fit a smooth path, and the specific steps are as follows: 9.1) rasterizing the fitting area; 9.2) traversing the point cloud in the grid: determining the size of the influence area of the grid point cloud p; determining nodes contained in a p-impact area; Shape function .

Images