Inspection robot dynamic obstacle avoidance method based on 3D point cloud

Abstract

The invention relates to the technical field of robots, and discloses an inspection robot dynamic obstacle avoidance method based on 3D point cloud, and the method comprises the steps: inputting the current frame point cloud of a robot into a dynamic obstacle avoidance module in real time, and carrying out the dynamic obstacle judgment; if it is judged that a dynamic obstacle exists, the current frame point cloud is projected into an obstacle image, contour extraction is conducted on the obstacle image, an optimal visual edge is calculated, and a corresponding course angle is input into a path tracking module; and after receiving the course angle issued by the dynamic obstacle avoidance module, the path tracking module stops the current movement of the robot, rotates the original point of the robot to a specified course angle, provides a linear velocity for the robot, and continues path tracking when the robot detects that no obstacle exists in front. According to the invention, the environmental sanitation robot can sense and identify surrounding dynamic obstacles in real time while tracking the pre-recorded path, and can automatically adjust the cleaning path to avoid the obstacles.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a dynamic obstacle avoidance method for an inspection robot based on 3D point cloud. Background Art

[0002] In the field of closed park cleaning, sanitation robots, as an automated cleaning device, have gradually been widely used. Existing closed park cleaning sanitation robots mainly rely on pre-recorded trajectories to operate. This operation mode requires detailed recording and planning of the park's cleaning paths before the robots are put into use. During the actual cleaning process, the robots will move along these pre-recorded paths and perform cleaning operations at the same time.

[0003] However, this cleaning method based on pre-recorded trajectories has significant limitations. In particular, existing sanitation robots are often unable to deal with dynamic obstacles. When the robot is cleaning along a predetermined path, if it encounters obstacles such as pedestrians, temporarily parked vehicles, or other dynamic obstacles, the robot will usually choose to stop moving and wait for the obstacles to disappear before continuing to work. This processing method not only leads to the interruption of the cleaning operation, but also greatly reduces the robot's work efficiency. Due to the lack of dynamic obstacle avoidance capabilities, the robot seems to be unable to cope with the complex and changing campus environment and it is difficult to meet the needs of efficient and continuous cleaning. Summary of the invention

[0004] In order to solve the above technical problems, the present invention provides a dynamic obstacle avoidance method for an inspection robot based on 3D point cloud.

[0005] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0006] A dynamic obstacle avoidance method for an inspection robot based on 3D point cloud, wherein the adopted robot system includes a multi-sensor fusion slam module, a local map module, a path tracking module, and a dynamic obstacle avoidance module;

[0007] The multi-sensor fusion slam module is used to provide positioning information to the robot and build an environmental map; the local map module is used to provide a dynamic local map centered on the robot for dynamic obstacle avoidance of the robot;

[0008] When the robot tracks the path through the path tracking module, there is no need to maintain the local cost map in real time; the robot's current frame point cloud is input into the dynamic obstacle avoidance module in real time to determine the dynamic obstacle; if it is determined that there is a dynamic obstacle, the current frame point cloud is projected into an obstacle image, and the obstacle image is contour extracted, the optimal visible edge is calculated, and the corresponding heading angle is input into the path tracking module;

[0009] After receiving the heading angle issued by the dynamic obstacle avoidance module, the path tracking module stops the current movement of the robot, rotates the robot origin to the specified heading angle, and then gives the robot a linear speed. When the robot checks that there are no obstacles in front, it continues to track the path.

[0010] Furthermore, the inputting of the robot's current frame point cloud into the dynamic obstacle avoidance module to perform dynamic obstacle judgment specifically includes:

[0011] According to the distance between the current frame point cloud and the robot and the height of the current frame point cloud relative to the robot, it is determined whether there are obstacles in the robot's forward direction.

[0012] Furthermore, the projecting of the current frame point cloud into an obstacle image and performing contour extraction on the obstacle image specifically includes:

[0013] By mapping the x and y in the coordinates of the current frame point cloud to the index in the image coordinate system, the current frame point cloud is reconstructed into the data format of the image; then OpenCV is used to extract the contour of the obstacle image, and only the outermost contour is obtained. The obtained contour consists of a set of two-dimensional coordinate points.

[0014] Furthermore, the calculating of the optimal visible edge and inputting the corresponding heading angle into the path tracking module specifically includes:

[0015] Take the robot as the starting point start and the i-th contour point contour_i as the end point, connect the starting point and the end point respectively, and then check the relationship between the connecting line and the obstacle contour in turn; first, remove the connecting line with the number of intersections with the contour greater than 1, because this connecting line will lead the robot to collide with the obstacle; among the remaining connecting lines, select the connecting line with the smallest atan angle between the contour point and the robot position as the optimal visible edge, and the atan angle corresponding to the optimal visible edge is used as the heading angle and input into the path tracking module.

[0016] Compared with the prior art, the beneficial technical effects of the present invention are:

[0017] In view of the problem that the sanitation robot in the prior art lacks the ability to avoid dynamic obstacles while following a path, the present invention proposes a new technical solution. This solution enables the sanitation robot to sense and identify dynamic obstacles around it in real time while following a pre-recorded path. By integrating an advanced sensor system and an intelligent obstacle avoidance algorithm, the robot can respond quickly to obstacles and autonomously adjust the cleaning path to avoid them.

[0018] This technological innovation not only significantly improves the adaptability and flexibility of sanitation robots in dynamic environments, but also greatly improves the efficiency of cleaning operations. The robot no longer interrupts its operation to wait for obstacles to disappear, but can complete cleaning tasks continuously and efficiently. In addition, the implementation of the dynamic obstacle avoidance function also enhances the safety and reliability of the robot, providing a more stable and efficient solution for automated cleaning operations in closed parks. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 A logic flow chart of the robot system used in the present invention;

[0020] Figure 2 A schematic diagram of calculating the optimal visible edge according to the present invention. DETAILED DESCRIPTION

[0021] A preferred embodiment of the present invention is described in detail below with reference to the accompanying drawings.

[0022] The robot system includes a multi-sensor fusion slam module, a local map module, a path tracking module, and a dynamic obstacle avoidance module. The process of the dynamic obstacle avoidance module includes dynamic obstacle judgment, point cloud projection image, contour extraction, and calculation of the optimal visible edge. The logic is as follows Figure 1 shown.

[0023] The function of the multi-sensor fusion slam module is to provide the robot with positioning information and build an environmental map; the local map module is used to provide a dynamic map of a certain size centered on the robot for dynamic obstacle avoidance of the robot.

[0024] In the process of tracking the path, the robot does not need to maintain the local cost map in real time, because this will reduce the efficiency of the algorithm. In the present invention, firstly, according to the distance between the current frame point cloud and the robot and the height of the current frame point cloud relative to the robot, it is judged whether there is an obstacle in the direction of the robot's advance. If it is judged that there is an obstacle in the direction of the robot's advance, dynamic obstacle avoidance processing is performed. First, the 3D current frame point cloud is reconstructed into the data format of the image, where only the X and Y in the current frame point cloud coordinates are mapped to the index in the image system; then OpenCV is used to extract the contour of the image mapped by the point cloud, and only the outermost contour is obtained. The obtained contour is composed of a set of two-dimensional coordinate points.

[0025] Take the robot as the starting point start and the i-th contour point contour_i as the end point, connect the starting point and the end point with a straight line, and then check the relationship between the obtained connecting lines and the obstacle contour in turn; first remove the connecting lines with more than 1 intersection points with the contour, because this connecting line will lead the robot to collide with the obstacle; among the remaining connecting lines, select the connecting line with the smallest atan angle between the contour point and the robot position as the optimal visible edge, such as Figure 2 As shown by the red line, this connection line can satisfy the minimum change of the robot's current heading when avoiding obstacles. In addition, in order to improve safety and consider sensor noise, the minimum atan angle should be appropriately increased. Finally, the minimum atan angle is passed to the tracking module for execution. After receiving the heading angle released by the dynamic obstacle avoidance module, the tracking module will stop the current movement of the robot, then rotate the robot origin to the specified heading angle, and then give the robot a linear speed. When the robot checks that there is no obstacle in front, it continues to use the tracking algorithm to track the previous path.

[0026] It is obvious to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential features of the present invention. Therefore, from any point of view, the embodiments should be regarded as exemplary and non-limiting, and the scope of the present invention is defined by the appended claims rather than the above description, and it is intended that all changes falling within the meaning and scope of the equivalent elements of the claims are included in the present invention, and any reference numerals in the claims should not be regarded as limiting the claims involved.

[0027] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A dynamic obstacle avoidance method for an inspection robot based on 3D point cloud, characterized in that: The robot system used includes a multi-sensor fusion slam module, a local map module, a path tracking module, and a dynamic obstacle avoidance module; The multi-sensor fusion slam module is used to provide positioning information to the robot and build an environmental map; the local map module is used to provide a dynamic local map centered on the robot for dynamic obstacle avoidance of the robot; When the robot tracks the path through the path tracking module, there is no need to maintain the local cost map in real time; the robot's current frame point cloud is input into the dynamic obstacle avoidance module in real time to determine the dynamic obstacle; if it is determined that there is a dynamic obstacle, the current frame point cloud is projected into an obstacle image, and the obstacle image is contour extracted, the optimal visible edge is calculated, and the corresponding heading angle is input into the path tracking module; After receiving the heading angle issued by the dynamic obstacle avoidance module, the path tracking module stops the current movement of the robot, rotates the robot origin to the specified heading angle, and then gives the robot a linear speed. When the robot checks that there are no obstacles in front, it continues to track the path.

2. The dynamic obstacle avoidance method for an inspection robot based on 3D point cloud according to claim 1, characterized in that: The point cloud of the current frame of the robot is input into the dynamic obstacle avoidance module in real time to judge the dynamic obstacles, specifically including: According to the distance between the current frame point cloud and the robot and the height of the current frame point cloud relative to the robot, it is determined whether there are obstacles in the robot's forward direction.

3. The dynamic obstacle avoidance method for an inspection robot based on 3D point cloud according to claim 1, characterized in that: The projecting of the current frame point cloud into an obstacle image and performing contour extraction on the obstacle image specifically includes: By mapping the x and y in the coordinates of the current frame point cloud to the index in the image coordinate system, the current frame point cloud is reconstructed into the data format of the image; then OpenCV is used to extract the contour of the obstacle image, and only the outermost contour is obtained. The obtained contour consists of a set of two-dimensional coordinate points.

4. The dynamic obstacle avoidance method for inspection robots based on 3D point cloud according to claim 1, characterized in that: The calculating of the optimal visible edge and inputting the corresponding heading angle into the path tracking module specifically includes: Take the robot as the starting point start and the i-th contour point contour_i as the end point, connect the starting point and the end point respectively, and then check the relationship between the connecting line and the obstacle contour in turn; first, remove the connecting line with the number of intersections with the contour greater than 1, because this connecting line will lead the robot to collide with the obstacle; among the remaining connecting lines, select the connecting line with the smallest atan angle between the contour point and the robot position as the optimal visible edge, and the atan angle corresponding to the optimal visible edge is used as the heading angle and input into the path tracking module.

Images