Path planning method for cleaning robots in complex environments

Abstract

The present invention discloses a path planning method for a cleaning robot in a complex environment. The method pre-processes an image and then extracts and sorts contours: based on the pre-processed binary grayscale image, a contour extraction method is used to extract all contours and their hierarchical relationships. By sorting the contours, the area to be cleaned and the obstacle area are determined. Polygon fitting is performed on all contours, and the side lengths and angles of all fitted polygons are counted to determine the main path direction of the map. The fitted polygon point set is constructed as a polygon with internal holes, and the BCD decomposition method is applied to decompose it. An adjacency matrix is ​​constructed based on the decomposed units, and the connection order of the units is determined in combination with the starting point position and the adjacency matrix. Path planning is performed for each unit according to the connection order of the units, and the global optimal path is finally obtained. The present invention improves the efficiency and coverage of the full-coverage path planning of the cleaning robot.

Description

Technical Field

[0001] The present invention relates to a robot path planning technology, and in particular to a cleaning robot path planning method in a complex environment. Background Art

[0002] Path planning involves finding a suitable, collision-free path for a robot within a map. Depending on the scenario and goal, path planning can be categorized as point-to-point or full-coverage. In cleaning robot applications, full-coverage path planning primarily involves finding a path within a designated area that avoids all obstacles and covers the entire area as closely as possible. Commonly used techniques include polygon decomposition and template modeling.

[0003] Existing full-coverage path planning technologies are mostly suitable for simple environments and have a relatively simplified approach to obstacle handling. Some methods create a grid map to mark the grids where obstacles are located, while others use contour extraction to determine the area to be cleaned. A very small number of methods ignore the presence of small obstacles in the map.

[0004] However, in actual production and living environments, complex environments are more common, and small obstacles often cannot be ignored. If the above method is used to directly plan the path, the robot's coverage rate may be reduced, and it may even be easy to get stuck in an environmental dead end. Summary of the Invention

[0005] Purpose of the invention: The purpose of the present invention is to solve the deficiencies in the prior art and provide a path planning method for a cleaning robot in a complex environment, which solves the problems of low path planning coverage and high energy consumption in the prior art. The technical solution of the present invention can ensure that the cleaning robot can complete the task efficiently and with a high coverage rate in a complex environment with numerous obstacles.

[0006] Technical solution: A cleaning robot path planning method in a complex environment of the present invention comprises the following steps:

[0007] Step 1: Acquire map images and perform image preprocessing;

[0008] First, the original image is converted into a binary black and white image (white is the area to be cleaned, black is the obstacle area), and then the binary black and white image is sequentially subjected to morphological erosion and opening operations to preliminarily identify the obstacles and passable areas on the image, that is, the area to be cleaned and the obstacle area;

[0009] Here, the original image (RGB three-channel image) is first converted into a single-channel grayscale image, and then all pixel values ​​are binarized using the set pixel value threshold, that is, those greater than the threshold are set to 255 and those less than the threshold are set to 0; thus, a black and white image is obtained;

[0010] Step 2: Extract and sort the contours of the image obtained in step 1 to determine the area to be cleaned and the obstacle area. The specific process is as follows:

[0011] Step 2.1, extracting contours using a contour extraction method, and then obtaining a hierarchical relationship between all contours, wherein the hierarchical relationship includes the index of the next contour of the current contour, the index of the previous contour of the current contour, the index of the first child contour of the current contour, and the index of the parent contour of the current contour;

[0012] Step 2.2: Sort all contours from largest to smallest according to their area, and determine the index of the contour with the largest area, which is the outer frame of the robot's feasible area.

[0013] At the same time, based on the hierarchical relationship between contours, all contours whose parent contour index is the maximum contour index are found. These contours are the contours of the obstacles.

[0014] Step 2.3: For smaller obstacle contours (smaller obstacle contours are polygonal), calculate the distance between them and the adjacent obstacle contours (calculate the distance from all points of the obstacle contour to all edges of the adjacent obstacle contours, and take the minimum value to represent this distance). If this distance is smaller than the robot's size, meaning the robot cannot pass through, merge the two obstacle contours to restrict the robot's route and avoid a dead end. If this distance is larger than the robot's size (width), no action is taken.

[0015] Step 3, polygon fitting, that is, polygon fitting is performed on all the contours extracted in step 3 to form a polygon set, simplify the complex contours, reduce the number of points, and provide a basis for the subsequent BCD region decomposition method;

[0016] Step 4: Calculate the path direction. For all polygons in the polygon set obtained in step 3, count their side lengths and angles respectively. Build an angle histogram through mapping and weighting. Determine the angle with the highest frequency as the path direction of the map. The specific method is as follows:

[0017] First, the distance between two adjacent points in the polygon is calculated using the Euclidean distance formula, and the direction angle of the edge between the two adjacent points is calculated using the atan2 function, and it is mapped to the range [0,180). Next, all points in the polygon are traversed, and the lengths of the edges with the same direction angle are accumulated to construct a histogram of edge length-direction angle. Finally, by analyzing the histogram, the direction angle with the highest frequency is determined as the main direction angle of the polygon (the main direction angle and the angle perpendicular to the main direction). The main direction angle provides two possible decomposition directions for polygon decomposition:

[0018] Step 5: Polygon decomposition. The polygon point set fitted in step 3 is constructed into a polygon with internal holes. The polygon with holes is then decomposed using the best unit decomposition method (BCD). For each decomposition direction obtained, the height value corresponding to the optimal scanning direction of each unit is traversed and accumulated. Finally, the decomposition solution with the smallest height value is selected as the optimal decomposition result under the optimal direction.

[0019] Step 6: Constructing the unit adjacency graph and connecting the units, that is, constructing the adjacency matrix based on the decomposed units, and combining the starting point position and the adjacency matrix to determine the unit connection order using the DFS algorithm;

[0020] Step 7: Path planning, that is, planning the path for each unit according to the unit connection sequence obtained in step 6, and finally obtaining the global optimal path.

[0021] Furthermore, the detailed method of step 1 is:

[0022] First, use OpenCV to read the PNG format image file in the specified folder and convert it from a color image to a grayscale image for subsequent processing.

[0023] Next, by setting a threshold (the threshold is set to 244 to 254), the pixels greater than the threshold are set to white, and the pixels less than the threshold are kept black, thereby highlighting the bright areas in the image and indicating the passable areas.

[0024] Subsequently, an image erosion operation is applied, and the erosion kernel is set to simulate the size of the robot and reduce the highlighted areas to avoid collisions with obstacles during path planning.

[0025] Finally, through the opening operation (which can be implemented using the cv::morphologyEx function), the opening kernel is set to remove noise points and isolated bright spots in the image, further cleaning the image and providing more accurate input for subsequent path planning.

[0026] Furthermore, the detailed method of step 3 is:

[0027] Step 1) Connect a straight line segment AB between the first and last points A and B of the expansion curve, where the straight line segment AB is the chord of the curve;

[0028] Step 2) Calculate the point C on the curve that is the farthest from the straight line AB, and calculate the distance b between point C and the straight line AB;

[0029] Step 3) Compare the distance b with a predetermined threshold. If the distance b is smaller than the threshold, the straight line AB is used as an approximation of the curve, and the curve segment is processed.

[0030] If the distance b is greater than the threshold, the curve is divided into two segments AC and BC using point C, and steps 1) to 3) are performed on the two segments respectively.

[0031] When all curves have been processed, the broken lines formed by connecting the segmentation points (ie, all points C) are sequentially connected, and the obtained broken lines are used as the approximation of the curve.

[0032] Furthermore, the detailed method of step 5 is:

[0033] For the polygon point set obtained in step 3, first extract the first polygon point set (that is, the polygon with the largest contour area) as the outer polygon, and the remaining point sets as the inner holes to construct a polygon with holes;

[0034] For the obtained polygon with holes, calculate all possible decomposition directions and evaluate the decomposition results in different directions: for each decomposition direction, traverse the height value corresponding to the optimal scanning direction of each unit and accumulate them;

[0035] Finally, the decomposition solution with the smallest height value is selected as the optimal decomposition result.

[0036] Furthermore, the specific method of step 6 is:

[0037] Based on the optimal decomposition result obtained in step 5, an adjacency matrix is ​​constructed according to the adjacency relationship between the decomposed units. Then, the position of the starting point is obtained from the original image. The unit in which the starting point is located is further confirmed based on the position of the starting point. Finally, the DFS algorithm is used to traverse all units in sequence starting from the unit where the starting point is located to obtain the connection order of the units.

[0038] Beneficial effects: The present invention merges small obstacle-dense areas that the robot cannot pass through smoothly into a large obstacle area through contour extraction, thereby avoiding the dead end problem caused by the robot entering the area due to unreasonable path planning; at the same time, by optimizing the unit decomposition method, the total length of path planning is effectively reduced, and the efficiency and coverage of the cleaning robot's full coverage path planning are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a schematic diagram of the overall process of the present invention;

[0040] Figure 2 It is a schematic diagram of the pretreatment of the present invention;

[0041] Figure 3 Schematic diagram of contour extraction and sorting of the present invention;

[0042] Figure 4 This is a schematic diagram of polygon fitting of the present invention;

[0043] Figure 5 This is a schematic diagram of the path direction calculation of the present invention;

[0044] Figure 6 A schematic diagram of polygon decomposition according to the present invention;

[0045] Figure 7 This is a schematic diagram of unit connection of the present invention;

[0046] Figure 8 This is a schematic diagram of a housing scene in the embodiment;

[0047] Figure 9 For example, Figure 8 Schematic diagram of the preliminary processing of the house scene;

[0048] Figure 10 This is a schematic diagram obtained after polygon fitting and path direction calculation in the embodiment;

[0049] Figure 11 This is a schematic diagram of the unit adjacency graph construction and unit connection in the embodiment;

[0050] Figure 12 Schematic diagram of the final path obtained in the embodiment. DETAILED DESCRIPTION

[0051] The technical solution of the present invention is described in detail below, but the protection scope of the present invention is not limited to the embodiments.

[0052] like Figure 1 As shown, the cleaning robot path planning method in a complex environment of the present invention includes the following steps:

[0053] Step 1: Acquire map images and perform image preprocessing;

[0054] First, the original image is converted into a binary black and white image (white is the area to be cleaned, black is the obstacle area), and then the binary black and white image is sequentially subjected to morphological erosion and opening operations to preliminarily identify the obstacles and pass areas on the image, that is, the area to be cleaned and the obstacle area; here, the original image (RGB three-channel image) is first converted into a single-channel grayscale image, and then all pixel values ​​are binarized by the set pixel value threshold, that is, the values ​​greater than the threshold are set to 255 and the values ​​less than the threshold are set to 0; a black and white image is obtained; as shown in FIG. Figure 2 shown.

[0055] Step 2: Figure 3 As shown, the image obtained in step 1 is subjected to contour extraction and sorting to determine the area to be cleaned and the obstacle area. The specific process is as follows:

[0056] Step 2.1, extracting contours using a contour extraction method, and then obtaining a hierarchical relationship between all contours, wherein the hierarchical relationship includes the index of the next contour of the current contour, the index of the previous contour of the current contour, the index of the first child contour of the current contour, and the index of the parent contour of the current contour;

[0057] Step 2.2: Sort all contours from largest to smallest according to their area, and determine the index of the contour with the largest area, which is the outer frame of the robot's feasible area.

[0058] At the same time, based on the hierarchical relationship between contours, all contours whose parent contour index is the maximum contour index are found. These contours are the contours of the obstacles.

[0059] Step 2.3: For smaller obstacle contours (smaller obstacle contours are polygonal), calculate the distance between them and the adjacent obstacle contour areas (traverse the distances from all points of the obstacle contour to all edges of the adjacent obstacle contours, and take the minimum value to represent this distance). If this distance is smaller than the robot's size, that is, the robot cannot pass through, then merge the two obstacle contours to restrict the robot's route and avoid a dead end when the robot enters this area. If the distance is larger than the robot's size (width), no processing is performed.

[0060] Step 3, polygon fitting, that is, polygon fitting is performed on all the contours extracted in step 3 to form a polygon set, simplify the complex contours, reduce the number of points, and provide a basis for the subsequent BCD region decomposition method, such as Figure 4 shown.

[0061] Step 4: Figure 5 As shown, calculate the path direction. For all polygons in the polygon set obtained in step 3, count their side lengths and angles respectively, construct an angle histogram through mapping and weighting, and determine the angle with the highest frequency as the path direction of the map. The specific method is as follows:

[0062] First, the distance between two adjacent points in the polygon is calculated using the Euclidean distance formula, and the direction angle of the edge between the two adjacent points is calculated using the atan2 function, and it is mapped to the range of [0,180). Then, all the points of the polygon are traversed, the lengths of the sides with the same direction angle are accumulated, and a histogram of side length-direction angle is constructed. Finally, by analyzing the histogram, the direction angle with the highest frequency is determined as the main direction angle of the polygon (the main direction angle and the angle perpendicular to the main direction). The main direction angle provides two possible decomposition directions for polygon decomposition.

[0063] Step 5: polygon decomposition. The polygon point set fitted in step 3 is constructed into a polygon with internal holes. The polygon with holes is then decomposed using the best unit decomposition method (BCD). For each decomposition direction obtained, the height value corresponding to the optimal scanning direction of each unit is traversed and accumulated. Finally, the decomposition solution with the smallest height value is selected as the optimal decomposition result under the optimal direction. The decomposition process is as follows: Figure 6 shown.

[0064] Step 6: Construct the unit adjacency graph and connect the units. That is, construct the adjacency matrix based on the decomposed units, and use the DFS algorithm to determine the unit connection order based on the starting point position and the adjacency matrix. Figure 7 As shown in the figure; Based on the optimal decomposition result obtained in step 5, an adjacency matrix is ​​constructed according to the adjacency relationship between the decomposed units. Then, the position of the starting point is obtained from the original image, and the unit in which the starting point is located is further confirmed according to the position of the starting point. Finally, the DFS algorithm is used to traverse all units in sequence starting from the unit where the starting point is located to obtain the connection order of the units.

[0065] Step 7: Path planning, that is, planning the path for each unit according to the unit connection sequence obtained in step 6, and finally obtaining the global optimal path.

[0066] The detailed method of step 1 of this embodiment is:

[0067] First, use OpenCV to read the PNG format image file in the specified folder and convert it from a color image to a grayscale image for subsequent processing.

[0068] Next, by setting a threshold (the threshold is set to 244 to 254), the pixels greater than the threshold are set to white, and the pixels less than the threshold are kept black, thereby highlighting the bright areas in the image and indicating the passable areas.

[0069] Subsequently, an image erosion operation is applied, and the erosion kernel is set to simulate the size of the robot and reduce the highlighted areas to avoid collisions with obstacles during path planning.

[0070] Finally, through the opening operation (which can be implemented using the cv::morphologyEx function), the opening kernel is set to remove noise points and isolated bright spots in the image, further cleaning the image and providing more accurate input for subsequent path planning.

[0071] The detailed method of step 3 of this embodiment is:

[0072] Step 1) Connect a straight line segment AB between the first and last points A and B of the expansion curve, where the straight line segment AB is the chord of the curve;

[0073] Step 2) Calculate the point C on the curve that is the farthest from the straight line AB, and calculate the distance b between point C and the straight line AB;

[0074] Step 3) Compare the distance b with a predetermined threshold. If the distance b is smaller than the threshold, the straight line AB is used as an approximation of the curve, and the curve segment is processed.

[0075] If the distance b is greater than the threshold, the curve is divided into two segments AC and BC using point C, and steps 1) to 3) are performed on the two segments respectively.

[0076] When all curves have been processed, the broken lines formed by connecting the segmentation points (ie, all points C) are sequentially connected, and the obtained broken lines are used as the approximation of the curve.

[0077] The detailed method of step 5 of this embodiment is:

[0078] For the polygon point set obtained in step 3, first extract the first polygon point set (that is, the polygon with the largest contour) as the outer polygon, and the remaining point sets as the inner holes to construct a polygon with holes; for the obtained polygon with holes, calculate all possible decomposition directions, and evaluate the decomposition results under different directions: for each decomposition direction, traverse the height value corresponding to the optimal scanning direction of each unit and accumulate them; finally, select the decomposition scheme with the smallest height value as the optimal decomposition result.

[0079] In order to verify the technical effect of the present invention, the technical solution of the present invention is applied to a specific complex environment, such as Figure 8 As shown, the initial input image example of this embodiment is a png or jpg picture obtained after laser point cloud scanning; Figure 8 The house scene in the video has many corners, broken lines and obstacles, and the environment is relatively complex.

[0080] Figure 9 The left image in is the initial input image. Figure 9 The middle image in the figure is the image after binarization in step 1. Figure 9 The right picture in the figure shows the image after the image erosion and opening operations in step 1 are performed.

[0081] After steps 3 and 4 are completed, the resulting image is as follows: Figure 10 As shown in the figure, the blue outline is the outline of the internal obstacle, and the red outline is the outline with the largest area; the image result after executing steps 4 and 5 is shown as follows Figure 11 As shown; the image results after step 6 and step 7 are shown as follows Figure 12 shown.

Claims

1. A cleaning robot path planning method in a complex environment, characterized in that: The following steps are involved: Step 1: Acquire map images and perform image preprocessing; First, the original image is converted into a binary black and white image, and then the morphological erosion and opening operations are performed on the binary black and white image in sequence to preliminarily identify the obstacles and passable areas on the image; Step 2: Extract and sort the contours of the image obtained in step 1 to determine the area to be cleaned and the obstacle area. The specific process is as follows: Step 2.1, extracting contours using a contour extraction method, and then obtaining a hierarchical relationship between all contours, wherein the hierarchical relationship includes the index of the next contour of the current contour, the index of the previous contour of the current contour, the index of the first child contour of the current contour, and the index of the parent contour of the current contour; Step 2.2: Sort all contours from largest to smallest according to their area, and determine the index of the contour with the largest area. This contour with the largest area is the outer frame of the robot's feasible area. At the same time, based on the hierarchical relationship between contours, all contours whose parent contour index is the maximum contour index are found. These contours are the contours of the obstacles. Step 2.3: For smaller obstacle contours, calculate the distance between them and adjacent obstacle contours. If the distance is smaller than the robot's size, meaning the robot cannot pass through, merge the two obstacle contours to restrict the robot's path and avoid a dead end. If the distance is larger than the robot's size, no action is taken. Step 3: polygon fitting, i.e. performing polygon fitting on all the contours extracted in step 3 to form a polygon set; Step 4: Calculate the path direction. For all polygons in the polygon set obtained in step 3, count their side lengths and angles respectively. Build an angle histogram through mapping and weighting. Determine the angle with the highest frequency as the path direction of the map. The specific method is as follows: First, the distance between two adjacent points in the polygon is calculated using the Euclidean distance formula. The direction angle between the two adjacent points is calculated using the atan2 function and mapped to the range [0, 180). Next, all points in the polygon are traversed, and the lengths of the sides with the same direction angle are accumulated to construct a histogram of side lengths and direction angles. Finally, by analyzing the histogram, the direction angle with the highest frequency is determined as the main direction angle of the polygon. Step 5: Polygon decomposition. The polygon point set fitted in step 3 is constructed into a polygon with internal holes. The polygon with holes is then decomposed using the best unit decomposition method (BCD). For each decomposition direction obtained, the height value corresponding to the optimal scanning direction of each unit is traversed and accumulated. Finally, the decomposition solution with the smallest height value is selected as the optimal decomposition result under the optimal direction. Step 6: Constructing the unit adjacency graph and connecting the units, that is, constructing the adjacency matrix based on the decomposed units, and combining the starting point position and the adjacency matrix to determine the unit connection order using the DFS algorithm; Step 7: Path planning, that is, planning the path for each unit according to the unit connection sequence obtained in step 6, and finally obtaining the global optimal path.

2. The cleaning robot path planning method in a complex environment according to claim 1, characterized in that: The detailed method of step 1 is: First, use OpenCV to read the PNG format image file in the specified folder and convert it from a color image to a grayscale image; Next, by setting a threshold, pixels greater than the threshold are set to white, and pixels less than the threshold are set to black, thereby highlighting the bright areas in the image and indicating the passable areas; Then, an image erosion operation is applied, and the erosion kernel is set to simulate the size of the robot, shrinking the highlighted area to avoid collisions with obstacles during path planning; Finally, through the opening operation, the opening kernel is set to remove noise points and isolated bright spots in the image, further cleaning the image and providing more accurate input for subsequent path planning.

3. The cleaning robot path planning method in a complex environment according to claim 1, characterized in that: The detailed method of step 3 is: Step 1) Connect a straight line segment AB between the first and last points A and B of the expansion curve, and the straight line segment AB is the chord of the curve; Step 2) Calculate the point C on the curve that is the farthest from the straight line AB, and calculate the distance b between point C and the straight line AB; Step 3) Compare the distance b with a predetermined threshold. If the distance b is smaller than the threshold, the straight line AB is used as an approximation of the curve, and the curve segment is processed. If the distance b is greater than the threshold, the curve is divided into two segments AC and BC using point C, and steps 1) to 3) are performed on the two segments respectively. When all curves have been processed, the broken lines formed by each segmentation point are connected in sequence, and the resulting broken lines are used as the approximation of the curve.

4. The cleaning robot path planning method in a complex environment according to claim 1, characterized in that: The detailed method of step 5 is as follows: for the polygon point set obtained in step 3, first extract the first polygon point set as the outer polygon, and the remaining point sets as the inner holes to construct a polygon with holes; For the obtained polygon with holes, calculate all possible decomposition directions and evaluate the decomposition results in different directions: for each decomposition direction, traverse the height value corresponding to the optimal scanning direction of each unit and accumulate them; Finally, the decomposition solution with the smallest height value is selected as the optimal decomposition result.

5. The cleaning robot path planning method in a complex environment according to claim 1, characterized in that: The specific method of step 6 is as follows: based on the optimal decomposition result obtained in step 5, an adjacency matrix is ​​constructed according to the adjacency relationship between the decomposed units, and then the position of the starting point is obtained from the original image. The unit in which the starting point is located is further confirmed according to the position of the starting point. Finally, the DFS algorithm is used to traverse all units in sequence starting from the unit where the starting point is located to obtain the connection order of the units.

Images