Cleaning robot path planning method in complex environment

Abstract

The invention discloses a cleaning robot path planning method in a complex environment, which comprises the following steps of: preprocessing an image, and then extracting and sequencing contours: extracting all contours and a hierarchical relationship thereof by adopting a contour extraction method based on a preprocessed binary gray level image, and sequencing the contours to determine an area to be cleaned and an obstacle area. Performing polygon fitting on all contours, counting side lengths and angles of all fitted polygons to determine a main path direction of the map, constructing a polygon with internal holes by using a point set of the fitted polygons, performing decomposition by applying a BCD decomposition method, constructing an adjacency matrix based on decomposed units, and combining the position of a starting point and the adjacency matrix to obtain a main path direction of the map; and determining a connection sequence of the units, performing path planning on each unit according to the connection sequence of the units, and finally obtaining a global optimal path. According to the invention, the efficiency and coverage rate of full-coverage path planning of the cleaning robot are improved.

Description

Technical Field

[0001] The 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 refers to finding a suitable, collision-free path for the robot in the map. According to different scenarios and goals, path planning can be divided into point-to-point path planning and full coverage path planning. In the application scenario of cleaning robots, full coverage path planning is mainly involved. Full coverage path planning refers to finding a path for the robot in a specified area that avoids all obstacles and covers the entire area as much as possible. Commonly used technical methods include polygon decomposition method, template model method, etc.

[0003] Most existing full coverage path planning technologies are suitable for simple environments and have a relatively simplified approach to obstacle handling. Some methods mark the grids where obstacles are located by building a grid map, while others obtain the area to be cleaned by contour extraction. Very few methods ignore the existence of small obstacles in the map.

[0004] However, in actual production and living environments, complex environmental conditions are more common, and small obstacles often cannot be ignored. If the above method is used to directly plan the path, the robot coverage rate may decrease, and it may even fall into 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 to 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, and black is the obstacle area), and then the morphological erosion and opening operations are performed on the binary black-and-white image in sequence to preliminarily confirm 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 by 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 by a contour extraction method, and then obtaining a hierarchical relationship between all contours, wherein the hierarchical relationship includes an index of a next contour of a current contour, an index of a previous contour of a current contour, an index of a first child contour of a current contour, and an index of a parent contour of a current contour;

[0012] Step 2.2, sort all contours from large to small 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, and these contours found are the contours of the obstacles;

[0014] Step 2.3: For the obstacle contour with a smaller area (the obstacle contour with a smaller area is a polygon), traverse and calculate the distance between it and the adjacent obstacle contour area (traverse the distance from all points of the obstacle contour to all edges of the adjacent obstacle contour, and take the minimum value to represent this distance); if the distance is smaller than the size of the robot, that is, the robot cannot pass through, then merge the two obstacle contours to limit the robot's route and avoid a dead end when the robot enters the area; if the distance is larger than the size (width) of the robot, no processing is performed;

[0015] Step 3, polygon fitting, that is, polygon fitting is performed on all contours extracted in step 3 to form a polygon set, simplify 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, construct an angle histogram by mapping and weighting, and 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 of [0,180). Next, all the points of the polygon are traversed, the lengths of the sides with the same direction angle are accumulated, and a histogram of the 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:

[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 scheme 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 according to the decomposed units, and combining the starting point position and the adjacency matrix, using the DFS algorithm to determine the unit connection order;

[0020] Step 7: Path planning, that is, performing path planning 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, the 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, and 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 the point C and the straight line AB;

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

[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 connected in sequence, and the obtained broken lines are used as the approximation of the curves.

[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 (i.e., the fitted 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, all possible decomposition directions are calculated, and the decomposition results under different directions are evaluated: for each decomposition direction, the height value corresponding to the optimal scanning direction of each unit is traversed and accumulated;

[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, 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.

[0038] Beneficial effects: The present invention merges small obstacle-dense areas that the robot cannot pass 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 It is a schematic diagram of contour extraction and sorting of the present invention;

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

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

[0044] Figure 6 It is a schematic diagram of polygon decomposition of the present invention;

[0045] Figure 7 It 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] Fig. 9 For example, Figure 8 Schematic diagram of the preliminary processing of the house scene;

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

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

[0050] Fig.12 The diagram is a 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 comprises 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, and black is the obstacle area), and then the morphological erosion and opening operations are performed on the binary black-and-white image in sequence to preliminarily confirm the obstacles and passage 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, those greater than the threshold are set to 255 and those 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 by a contour extraction method, and then obtaining a hierarchical relationship between all contours, wherein the hierarchical relationship includes an index of a next contour of a current contour, an index of a previous contour of a current contour, an index of a first child contour of a current contour, and an index of a parent contour of a current contour;

[0057] Step 2.2, sort all contours from large to small 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, and these contours found are the contours of the obstacles;

[0059] Step 2.3: For obstacle contours with smaller areas (obstacle contours with smaller areas are polygons), traverse and 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 the distance is smaller than the size of the robot, 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 the area; if the distance is larger than the size (width) of the robot, no processing is performed.

[0060] Step 3, polygon fitting, that is, polygon fitting is performed on all contours extracted in step 3 to form a polygon set, simplify 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, the path direction is calculated. For all polygons in the polygon set obtained in step 3, their side lengths and angles are counted respectively. An angle histogram is constructed by mapping and weighting, and the angle with the highest frequency is determined 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 mapped to the range of [0,180) at the same time; 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, and then the polygon with holes is 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 scheme 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: Unit adjacency graph construction and unit connection, that is, construct an adjacency matrix based on the decomposed units, and combine the starting point position and the adjacency matrix to determine the unit connection order using the DFS algorithm, such as Figure 7 As shown; 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, 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, performing path planning 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, the 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, and 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 the point C and the straight line AB;

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

[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 connected in sequence, and the obtained broken lines are used as the approximation of the curves.

[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 fitted polygon of the contour with the largest area) 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, 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] Fig. 9 The left image in is the initial input image. Fig. 9 The middle image in is the image after binarization in step 1. Fig. 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 Fig.10 As shown in the figure, the blue outline is the internal obstacle outline, and the red outline is the outline with the largest area; the image result after executing steps 4 and 5 is shown as follows Fig.11 As shown; the image results after step 6 and step 7 are shown as follows Fig.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 confirm the obstacles and passage 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 by a contour extraction method, and then obtaining a hierarchical relationship between all contours, wherein the hierarchical relationship includes an index of a next contour of a current contour, an index of a previous contour of a current contour, an index of a first child contour of a current contour, and an index of a parent contour of a current contour; Step 2.2: Sort all contours from large to small 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, and these contours found are the contours of the obstacles; Step 2.3: For obstacle contours with a smaller area, traverse and calculate the distance between it and the adjacent obstacle contour area; if the distance is smaller than the size of the robot, 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 the area; if the distance is larger than the size of the robot, no processing is performed; Step 3, polygon fitting, that is, polygon fitting is performed 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, construct an angle histogram by mapping and weighting, and 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, 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). Next, all points in the polygon are traversed, the lengths of the edges with the same direction angle are accumulated, and a histogram of edge 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. 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 scheme 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 according to the decomposed units, and combining the starting point position and the adjacency matrix, using the DFS algorithm to determine the unit connection order; Step 7: Path planning, that is, performing path planning 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 area in the image, indicating the passable area; Then, the image erosion operation is applied, and the erosion kernel is set to simulate the size of the robot, reduce the highlight area, and 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 value. If the distance b is smaller than the threshold value, the straight line segment AB is used as an approximation of the curve, and the processing of the curve segment is completed; 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 obtained 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, firstly 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, all possible decomposition directions are calculated, and the decomposition results under different directions are evaluated: for each decomposition direction, 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.

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: 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, 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.

Images