Inspection robot global path planning method based on three-dimensional grid map

Abstract

The invention relates to an inspection robot global path planning method based on a three-dimensional grid map. The method comprises the following steps: constructing a three-dimensional point cloud map; constructing a three-dimensional occupation grid map; calculating the height difference between each grid and an adjacent grid of the three-dimensional occupied grid map, and marking the current grid and continuous grids in the vertical direction, namely the z direction, as grids with the same attribute; giving current position and target point position information of the robot, and converting the information into serial numbers to which grids belong; and traversing surrounding grids from a starting point by using an A * algorithm until a target point position is searched, and forming a global path from the starting point to the target point. According to the invention, a traditional two-dimensional grid map is replaced by a three-dimensional grid map, so that the obstacle avoidance capability of the ground mobile robot in a complex terrain is remarkably improved, the flexibility of the robot is improved, the safety of the robot in an actual application scene is enhanced, and especially in a dynamically changing environment, the obstacle avoidance capability of the robot is greatly improved. The path can be adjusted in time to avoid collision.

Description

Technical Field

[0001] The invention relates to the technical field of global path planning of wheeled robots, and in particular to a global path planning method of an inspection robot based on a three-dimensional grid map. Background Art

[0002] In the field of wheeled robots, path planning is one of the key steps to achieve autonomous navigation. In the prior art, the robot's three-dimensional global path planning is usually completed based on a two-dimensional grid map. However, when facing the problem of three-dimensional path planning, the traditional method needs to switch to a two-dimensional navigation map and plan each layer of the global path separately, which reduces the efficiency of path planning and complicates the planning process.

[0003] In view of the limitations of the traditional A* method, some studies in recent years have been devoted to improving the method to improve the performance and robustness of path planning. These improvements include introducing new path calculation mechanisms and combining machine learning and other methods to optimize decision making and path selection in the path planning process. However, most of the current improved methods still have limitations, such as long calculation time and only considering two-dimensional information during planning, which limits their widespread use in practical applications.

[0004] Therefore, it is necessary to propose a new A* algorithm based on 3D maps to overcome the limitations of traditional methods and achieve safer and more efficient autonomous navigation. Summary of the invention

[0005] In order to solve the problem that when a ground mobile robot is navigating in three dimensions, due to insufficient map information of a two-dimensional A* algorithm, global path planning has to be completed in multiple times, each planning only plans the global path of the current height, and a map switching point needs to be set at the intersection of the three-dimensional heights, the purpose of the present invention is to provide a global path planning method for an inspection robot based on a three-dimensional grid map, which replaces the two-dimensional grid map used by a traditional ground robot with a three-dimensional occupied grid map, adds the state attributes of each grid, simplifies the planning process during three-dimensional navigation, and improves the efficiency of planning the path and the smoothness of the path.

[0006] To achieve the above object, the present invention adopts the following technical solution: a global path planning method for an inspection robot based on a three-dimensional grid map, the method comprising the following steps in order:

[0007] (1) Use laser SLAM technology to build a three-dimensional point cloud map and filter out dynamic obstacle point clouds;

[0008] (2) constructing a three-dimensional occupancy grid map based on the three-dimensional point cloud map;

[0009] (3) Calculate the height difference between each grid and the adjacent grid of the three-dimensional occupancy grid map, and mark the current grid and the continuous grids in the vertical direction, i.e., the z direction, as grids with the same attribute. The grid has three attributes: free, passable, and impassable. The three attributes are respectively recorded as 1, 2, and 3 in a one-to-one correspondence, and all passable areas are found;

[0010] (4) Given the robot’s current position and target point position information, convert them into the grid’s serial number;

[0011] (5) Based on all the passable areas found, use the A* algorithm to traverse the surrounding grids from the starting point until the target point is found. A global path is formed from the starting point to the target point. All path points on the global path can only pass through grids with passable attributes.

[0012] The step (2) specifically refers to: traversing the three-dimensional point cloud map, mapping the points in the three-dimensional point cloud map to the grid, setting the length, width, height of the three-dimensional occupied grid map and the resolution of each grid, and according to the coordinates of the point cloud, using the formula for converting the Cartesian coordinate system to the grid serial number to convert the coordinates of the point cloud into the serial number of the three-dimensional occupied grid map, and marking the grid corresponding to each point cloud as occupied, and the formula for converting the Cartesian coordinate system to the grid serial number is:

[0013] I x =[(P x -G x_size / 2) / G r ]

[0014] I y =[(P y -G y_size / 2) / G r ]

[0015] I z =[(P z -G z_size / 2) / G r ]

[0016] In the formula, I x Indicates the grid number of point P in the x direction, P x represents the coordinate value of point P in the x direction, G r Indicates the resolution of the grid; G x_size Indicates the size of the three-dimensional grid map in the x direction, [] means rounding up; I y Indicates the grid number of point P in the y direction; P y Represents the coordinate value of point P in the y direction; I z Indicates the grid number of point P in the z direction; P z Represents the coordinate value of point P in the z direction.

[0017] The step (3) specifically comprises the following steps in order:

[0018] (3a) Use a moving sliding window to calculate the 9 grids and count the heights of each grid and the vertical direction, i.e., the heights of the grids that are continuously occupied in the z direction;

[0019] (3b) Taking the current grid as the central grid, the height difference between the central grid and the adjacent grid is calculated. If the height difference is greater than a set threshold, the central grid is judged to be an inaccessible area. Otherwise, if the height difference is less than or equal to the set threshold, the central grid is judged to be a accessible area.

[0020] (3c) Traverse all grids and find all passable areas.

[0021] The step (5) specifically comprises the following steps in order:

[0022] (5a) Initialize the Open List and Close List;

[0023] (5b) Take the grid with the lowest cost from the Open List and set it as the current grid;

[0024] (5c) Move the current grid to the CloseList list, indicating that the grid has been visited;

[0025] (5d) Evaluate the adjacent grids of the current grid, including the grids in the upper, lower, left, right and diagonal directions. For each adjacent grid, calculate its cost, i.e., the f value:

[0026] f=g+h+s

[0027] In the formula, g represents the actual cost from the starting point to the current grid, h represents the estimated cost from the current grid to the target grid, and s represents the state of the grid;

[0028]

[0029] (5e) Add the evaluated adjacent grid to the Open List. If the currently evaluated adjacent grid is already in the Open List, compare the f value of the adjacent grid already in the Open List with the f value of the currently evaluated adjacent grid, and select the adjacent grid with the smaller f value to add to the Open List.

[0030] (5f) Repeat steps (5b) to (5e) until the adjacent grid currently being evaluated is the target grid, the path planning is completed, the preliminary path is obtained, and the preliminary path is traced back;

[0031] (5g) After the preliminary path is generated, the preliminary path is smoothed to obtain the global path.

[0032] It can be seen from the above technical scheme that the beneficial effects of the present invention are: first, the present invention replaces the traditional two-dimensional grid map with a three-dimensional occupied grid map, which significantly improves the obstacle avoidance ability of the ground mobile robot in complex terrain. The three-dimensional map can more accurately represent various suspended obstacles, slopes and other irregular terrain features, enabling the robot to effectively navigate in highly complex environments, which not only improves the flexibility of the robot, but also enhances its safety in practical application scenarios, especially in dynamically changing environments, and can adjust the path in time to avoid collisions; second, the state attributes of each grid are increased, and the heuristic function of the standard A* algorithm is modified to make path planning more intelligent. By introducing the state quantity s, the passing performance and Kinematic characteristics, which means that the planned path is not only feasible in theory, but also has higher operability in the actual execution process. This state-based consideration can effectively reduce the robot's energy consumption and time consumption in complex environments, and improve the overall task completion efficiency; third, when the ground mobile robot is planning across floors, there is no need to switch to a two-dimensional map, which simplifies the planning process in the three-dimensional navigation process. In traditional methods, frequent map switching often leads to information loss and additional computing burden, while the present invention uses a unified three-dimensional occupancy grid map to provide continuous environmental information, which not only improves the efficiency of planning paths, but also enables the robot to respond to environmental changes more quickly and accurately when performing tasks, thereby improving its adaptability in multi-story buildings or complex facilities. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 A schematic diagram of searching for a traversable area in the present invention;

[0034] Figure 2 This is a schematic diagram of the three-dimensional A* global path planning in the present invention;

[0035] Figure 3 The present invention is a flow chart of the method. DETAILED DESCRIPTION

[0036] like Figure 3 As shown, a global path planning method for an inspection robot based on a three-dimensional grid map comprises the following steps in sequence:

[0037] (1) Use laser SLAM technology to build a three-dimensional point cloud map and filter out dynamic obstacle point clouds;

[0038] (2) constructing a three-dimensional occupancy grid map based on the three-dimensional point cloud map;

[0039] (3) Calculate the height difference between each grid and the adjacent grid of the three-dimensional occupancy grid map, and mark the current grid and the continuous grids in the vertical direction, i.e., the z direction, as grids with the same attribute. The grid has three attributes: free, passable, and impassable. The three attributes are respectively recorded as 1, 2, and 3 in a one-to-one correspondence, and all passable areas are found;

[0040] (4) Given the robot’s current position and target point position information, convert them into the grid’s serial number;

[0041] (5) Based on all the passable areas found, use the A* algorithm to traverse the surrounding grids from the starting point until the target point is found. A global path is formed from the starting point to the target point. All path points on the global path can only pass through grids with passable attributes.

[0042] The step (2) specifically refers to: traversing the three-dimensional point cloud map, mapping the points in the three-dimensional point cloud map to the grid, setting the length, width, height of the three-dimensional occupied grid map and the resolution of each grid, and according to the coordinates of the point cloud, using the formula for converting the Cartesian coordinate system to the grid serial number to convert the coordinates of the point cloud into the serial number of the three-dimensional occupied grid map, and marking the grid corresponding to each point cloud as occupied, and the formula for converting the Cartesian coordinate system to the grid serial number is:

[0043] I x =[(P x -G x_size / 2) / G r ]

[0044] I y =[(P y -G y_size / 2) / G r ]

[0045] I z =[(P z -G z_size / 2) / G r ]

[0046] In the formula, I x Indicates the grid number of point P in the x direction, P x represents the coordinate value of point P in the x direction, G r Indicates the resolution of the grid; G x_size Indicates the size of the three-dimensional grid map in the x direction, [] means rounding up; I y Indicates the grid number of point P in the y direction; P y Represents the coordinate value of point P in the y direction; I z Indicates the grid number of point P in the z direction; P z Represents the coordinate value of point P in the z direction.

[0047] like Figure 1 As shown, the step (3) specifically includes the following steps in order:

[0048] (3a) Use a moving sliding window to calculate the 9 grids and count the heights of each grid and the vertical direction, i.e., the heights of the grids that are continuously occupied in the z direction;

[0049] (3b) Taking the current grid as the central grid, the height difference between the central grid and the adjacent grid is calculated. If the height difference is greater than a set threshold, the central grid is judged to be an inaccessible area. Otherwise, if the height difference is less than or equal to the set threshold, the central grid is judged to be a accessible area.

[0050] (3c) Traverse all grids and find all passable areas.

[0051] like Figure 2 As shown, in Figure 2 In the figure, the pentagram is the starting point, the triangle is the target point, and the path from the starting point to the target point is the global path. The step (5) specifically includes the following steps in order:

[0052] (5a) Initialize the Open List and Close List;

[0053] (5b) Take the grid with the lowest cost from the Open List and set it as the current grid;

[0054] (5c) Move the current grid to the CloseList list, indicating that the grid has been visited;

[0055] (5d) Evaluate the adjacent grids of the current grid, including the grids in the upper, lower, left, right and diagonal directions. For each adjacent grid, calculate its cost, i.e., the f value:

[0056] f=g+h+s

[0057] In the formula, g represents the actual cost from the starting point to the current grid, h represents the estimated cost from the current grid to the target grid, and s represents the state of the grid;

[0058]

[0059] (5e) Add the evaluated adjacent grid to the Open List. If the currently evaluated adjacent grid is already in the Open List, compare the f value of the adjacent grid already in the Open List with the f value of the currently evaluated adjacent grid, and select the adjacent grid with the smaller f value to add to the Open List.

[0060] (5f) Repeat steps (5b) to (5e) until the adjacent grid currently being evaluated is the target grid, the path planning is completed, the preliminary path is obtained, and the preliminary path is traced back;

[0061] (5g) After the preliminary path is generated, the preliminary path is smoothed. The purpose of smoothing is to reduce turns and unnecessary grid jumps and optimize the preliminary path. After smoothing, the global path is obtained.

[0062] In summary, the present invention replaces the traditional two-dimensional grid map with a three-dimensional occupied grid map, which significantly improves the obstacle avoidance ability of the ground mobile robot in complex terrain. The three-dimensional map can more accurately represent various suspended obstacles, slopes and other irregular terrain features, so that the robot can effectively navigate in a highly complex environment, which not only improves the flexibility of the robot, but also enhances its safety in practical application scenarios, especially in a dynamically changing environment, and can adjust the path in time to avoid collisions; the state attributes of each grid are increased, and the heuristic function of the standard A* algorithm is modified to make the path planning more intelligent. By introducing the state quantity s, the passing performance and kinematic characteristics of the ground robot can be fully considered during global planning, which means that the planned path is not only theoretically feasible, but also has higher operability in the actual execution process. This state-based consideration can effectively reduce the energy consumption and time consumption of the robot in a complex environment, and improve the overall task completion efficiency; the present invention has achieved a significant technical breakthrough in the navigation system of the ground mobile robot. Through the introduction of three-dimensional maps, state attributes and a simplified planning process, the intelligence level and operation efficiency of the robot are comprehensively improved, providing a solid foundation for the future development of intelligent robot technology.

Claims

1. A global path planning method for an inspection robot based on a three-dimensional grid map, characterized in that: The method comprises the following steps in the following order: (1) Use laser SLAM technology to build a three-dimensional point cloud map and filter out dynamic obstacle point clouds; (2) constructing a three-dimensional occupancy grid map based on the three-dimensional point cloud map; (3) Calculate the height difference between each grid and the adjacent grid of the three-dimensional occupancy grid map, and mark the current grid and the continuous grids in the vertical direction, i.e., the z direction, as grids with the same attribute. The grid has three attributes: free, passable, and impassable. The three attributes are respectively recorded as 1, 2, and 3 in a one-to-one correspondence, and all passable areas are found; (4) Given the robot’s current position and target point position information, convert them into the grid’s serial number; (5) Based on all the passable areas found, use the A* algorithm to traverse the surrounding grids from the starting point until the target point is found. A global path is formed from the starting point to the target point. All path points on the global path can only pass through grids with passable attributes.

2. The global path planning method for an inspection robot based on a three-dimensional grid map according to claim 1, characterized in that: The step (2) specifically refers to: traversing the three-dimensional point cloud map, mapping the points in the three-dimensional point cloud map to the grid, setting the length, width, height of the three-dimensional occupied grid map and the resolution of each grid, and according to the coordinates of the point cloud, using the formula for converting the Cartesian coordinate system to the grid serial number to convert the coordinates of the point cloud into the serial number of the three-dimensional occupied grid map, and marking the grid corresponding to each point cloud as occupied, and the formula for converting the Cartesian coordinate system to the grid serial number is: I x =[(P x -G x_size / 2) / G r ] I y =[(P y -G y_size / 2) / G r ] I z =[(P z -G z_size / 2) / G r ] In the formula, I x Indicates the grid number of point P in the x direction, P x represents the coordinate value of point P in the x direction, G r Indicates the resolution of the grid; G x_size Indicates the size of the three-dimensional grid map in the x direction, [] means rounding up; I y Indicates the grid number of point P in the y direction; P y Represents the coordinate value of point P in the y direction; I z Indicates the grid number of point P in the z direction; P z Represents the coordinate value of point P in the z direction.

3. The global path planning method for an inspection robot based on a three-dimensional grid map according to claim 1, characterized in that: The step (3) specifically comprises the following steps in order: (3a) Use a moving sliding window to calculate the 9 grids and count the heights of each grid and the vertical direction, i.e., the heights of the grids that are continuously occupied in the z direction; (3b) Taking the current grid as the central grid, the height difference between the central grid and the adjacent grid is calculated. If the height difference is greater than a set threshold, the central grid is judged to be an inaccessible area. Otherwise, if the height difference is less than or equal to the set threshold, the central grid is judged to be a accessible area. (3c) Traverse all grids and find all passable areas.

4. The global path planning method for an inspection robot based on a three-dimensional grid map according to claim 1, characterized in that: The step (5) specifically comprises the following steps in order: (5a) Initialize the Open List and Close List; (5b) Take the grid with the lowest cost from the Open List and set it as the current grid; (5c) Move the current grid to the CloseList list, indicating that the grid has been visited; (5d) Evaluate the adjacent grids of the current grid, including the grids in the upper, lower, left, right and diagonal directions. For each adjacent grid, calculate its cost, i.e., the f value: f=g+h+s In the formula, g represents the actual cost from the starting point to the current grid, h represents the estimated cost from the current grid to the target grid, and s represents the state of the grid; (5e) Add the evaluated adjacent grid to the Open List. If the currently evaluated adjacent grid is already in the Open List, compare the f value of the adjacent grid already in the Open List with the f value of the currently evaluated adjacent grid, and select the adjacent grid with the smaller f value to add to the Open List. (5f) Repeat steps (5b) to (5e) until the adjacent grid currently being evaluated is the target grid, the path planning is completed, the preliminary path is obtained, and the preliminary path is traced back; (5g) After the preliminary path is generated, the preliminary path is smoothed to obtain the global path.

Images