Planning method for inspection path of factory operation and maintenance equipment

Abstract

The invention relates to the technical field of equipment operation and maintenance, and particularly discloses a factory operation and maintenance equipment inspection path planning method, which comprises the following steps: S1, constructing a factory three-dimensional point cloud map by using a laser synchronous positioning and map construction system, and removing dynamic obstacle point cloud; s2, a three-dimensional occupied grid map is constructed according to the three-dimensional point cloud map, a current grid and continuous grids in the vertical direction of the current grid are marked as grids with the same attribute, the grid attributes comprise idle, passable and impassable grids, and all passable grids are screened out; s3, setting position information of a tour-inspection starting point and a tour-inspection ending point, and enabling the position information to correspond to the grids; and S4, according to all screened passable grids, traversing surrounding grids from an inspection starting point by using an A * algorithm until an inspection terminal point is searched, and forming a global path from the inspection starting point to the inspection terminal point. According to the method, the adaptive grid resolution is obtained through optimization calculation of the grid resolution, and path planning is more intelligent by using the A * algorithm.

Description

Technical Field

[0001] The present invention relates to the technical field of equipment operation and maintenance, and in particular to a method for planning an inspection path of factory operation and maintenance equipment. Background Art

[0002] With the continuous development of industrialization, more and more factories introduce or upgrade automated production or semi-automated production. However, the inspection of special equipment mainly still relies on manual labor at present. The increase in equipment has led to an increase in the inspection intensity. In order to solve the problem of a large workload of manual inspection in some factory areas, robots are selected for inspection. However, whether it is manual inspection or robot inspection, both face the problem of low inspection efficiency caused by the complex inspection path caused by a large number of equipment. Therefore, in order to improve the inspection efficiency, it is necessary to optimize the inspection path in order to perform inspections with the optimal inspection path so as to improve the inspection efficiency. Summary of the Invention

[0003] In order to solve the problems existing in the prior art, the present invention provides a method for planning an inspection path of factory operation and maintenance equipment.

[0004] The technical solution adopted by the present invention is as follows:

[0005] A method for planning an inspection path of factory operation and maintenance equipment includes the following steps:

[0006] S1. Use a laser simultaneous localization and mapping system to construct a three-dimensional point cloud map of the factory area, and remove the dynamic obstacle point cloud from the three-dimensional point cloud map;

[0007] S2. Calculate the resolution P grid size of a single grid adapted in the three-dimensional occupancy grid map. Construct a three-dimensional occupancy grid map according to the three-dimensional point cloud map, mark the current grid and the continuous grids in the vertical direction of the current grid as the same attribute grids, and the grid attributes include free, passable, and impassable, and filter out all passable grids;

[0008] S3. Set the inspection start point and inspection end point position information, and map them to the corresponding grids;

[0009] S4. According to all the passable grids filtered out, use the A* algorithm to traverse the surrounding grids starting from the inspection start point until the inspection end point is searched, and form a global path from the inspection start point to the inspection end point, and the global path can only pass through the grids with the attribute of passable.

[0010] Preferably, in step S2, calculating the resolution P grid size of a single grid adapted in the three-dimensional occupancy grid map includes the following steps:

[0011] Obtain the lower limit value P minand the upper limit value P max , calculate the resolution P adapted to a single grid using the following formula grid Size:

[0012] ;

[0013] where η is a weight parameter, with a value greater than 0 and less than 1; the larger η is, the smaller P grid is, the more accurate the obstacle boundary is, and the safer the path planning is; the smaller η is, the larger P grid is, the smaller the computational amount is, and the lower the memory occupancy is.

[0014] Preferably, in step S2, constructing a three-dimensional occupancy grid map based on the three-dimensional point cloud map includes the following steps:

[0015] Set the length direction, width direction, and height direction of the three-dimensional point cloud map, establish a Cartesian three-dimensional coordinate system with the lower left corner of the three-dimensional point cloud map as the origin, the x-axis of the Cartesian three-dimensional coordinate system along the length direction of the three-dimensional point cloud map, the main y-axis of the Cartesian three-dimensional coordinate system along the width direction of the three-dimensional point cloud map, and the z-axis of the Cartesian three-dimensional coordinate system along the height direction of the three-dimensional point cloud map;

[0016] Convert the coordinates of the point cloud into the serial number of the three-dimensional occupancy grid map according to the following formula:

[0017] ;

[0018] ;

[0019] ;

[0020] where I x represents the grid serial number of point P in the x direction, I y represents the grid serial number of point P in the y direction, I z represents the grid serial number of point P in the z direction, P x represents the coordinate value of point P in the x direction, P y represents the coordinate value of point P in the y direction, P z represents the coordinate value of point P in the z direction.

[0021] Preferably, in step S22, the calculation of the value of the weight parameter η includes the following steps:

[0022] Obtain the floor area S of the factory area o , calculate the projected area S of the static obstacle point cloud on the xy plane static , and calculate the ratio of S static to S o as the weight parameter η:

[0023] ;

[0024] When the proportion of S static in the plant area is larger, η is larger, resulting in fewer feasible paths. To reduce misjudged collisions, the grid resolution needs to be reduced; when the proportion of S static in the plant area is smaller, η is smaller, resulting in more feasible paths. To avoid exponential growth of the number of grids and high calculation and storage costs, the grid resolution needs to be increased.

[0025] Preferably, step S2 further includes the following steps:

[0026] Use a moving sliding window to calculate 9 grids, and count the height of each grid and the grids continuously occupied in the vertical direction, i.e., in the z direction;

[0027] Regarding the current grid as the central grid, calculate the height difference between the central grid and the adjacent grids. If the height difference is greater than the set threshold, then judge the central grid as an impassable area; conversely, if the height difference is less than or equal to the set threshold, then judge the central grid as a passable area;

[0028] Traverse all grids to find all passable areas.

[0029] Preferably, step S4 specifically includes the following steps:

[0030] S41. Initialize the Open List list and the Close List list;

[0031] S42. Take out the grid with the lowest cost from the Open List and set it as the current grid;

[0032] S43. Move the current grid to the Close List list, indicating that the grid has been visited;

[0033] S44. Evaluate the adjacent grids of the current grid, including the grids in the up, down, left, right, and diagonal directions. For each adjacent grid, calculate its cost, i.e., the f value:

[0034] ;

[0035] Among them, g represents the actual cost from the starting point to the current grid, h represents the estimated cost from the current grid to the end grid, s represents the state of the grid, and s is determined by the following formula:

[0036] ;

[0037] S45. 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;

[0038] S46. Repeat steps S42 to S45 until the currently evaluated adjacent grid is the end grid, the path planning is completed, a preliminary path is obtained, and the preliminary path is traced backward;

[0039] S47. After generating the preliminary path, perform smoothing processing on the preliminary path to obtain the global path.

[0040] The beneficial effects of the present invention are as follows:

[0041] The traditional two - dimensional occupancy grid map is improved to a three - dimensional occupancy grid map, which can represent various suspended obstacles, slopes, and other irregular terrain features more comprehensively and accurately, enabling the robot to navigate effectively in a complex environment and enhancing the obstacle avoidance ability of the robot in complex terrains; through the optimized calculation of the grid resolution, an appropriate grid resolution is obtained, avoiding the obstacle expansion (occupying more grids) caused by too large a resolution, which may lead to a reduction in the feasible path or misjudgment of collisions, or avoiding the exponential growth of the number of grids (volume growth in three - dimensions) caused by too small a resolution, with high calculation and storage costs; based on the appropriate grid resolution, the A* algorithm is used to make the path planning more intelligent, and the state quantity is introduced through the grid attributes, making the path planning more reasonable and having better operability. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is a flowchart of the method of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0043] The present invention will be described in detail below with reference to the drawings and embodiments.

[0044] Embodiment:

[0045] As Figure 1 shown is a flowchart of a method for path planning of plant operation and maintenance equipment inspection, and the method includes the following steps:

[0046] S1. Use a laser simultaneous localization and mapping system to construct a three - dimensional point cloud map of the plant area, and remove the dynamic obstacle point cloud from the three - dimensional point cloud map;

[0047] Simultaneous Localization and Mapping (SLAM) is a lidar-based simultaneous localization and mapping technology that is widely used in fields such as robotics, autonomous driving, and service robots. It has the advantages of high precision and high resolution, strong anti-interference ability, real-time performance and low latency, and strong scene adaptability, and is suitable for constructing high-precision 2D / 3D grid maps or point cloud maps.

[0048] S2. Obtain the lower limit value P of the preset single grid resolution range min and the upper limit value P max , and use the following formula to calculate the resolution P adapted to a single grid grid Size:

[0049] ;

[0050] where η is a weight parameter, with a value greater than 0 and less than 1, so that the calculated P grid is still within the preset grid resolution range; the larger η is, the smaller P grid is, the more accurate the obstacle boundary is, and the safer the path planning is; the smaller η is, the larger P grid is, the smaller the calculation amount is, and the lower the memory occupancy is; therefore, an appropriate grid resolution needs to be selected.

[0051] Obtain the floor area S of the factory area o , calculate the projected area S of the static obstacle point cloud on the xy plane static , and calculate the ratio of S static to S o as the weight parameter η:

[0052] ;

[0053] When the proportion of S static in the factory area is larger, η is larger, resulting in a reduction in the feasible path. To reduce misjudged collisions, the grid resolution needs to be reduced; when the proportion of S static in the factory area is smaller, η is smaller, resulting in an increase in the feasible path. To avoid an exponential increase in the number of grids, high calculation and storage costs, the grid resolution needs to be increased.

[0054] Set the length direction, width direction, and height direction of the three-dimensional point cloud map, and establish a Cartesian three-dimensional coordinate system with the lower left corner of the three-dimensional point cloud map as the origin. The x-axis of the Cartesian three-dimensional coordinate system is along the length direction of the three-dimensional point cloud map, the main y-axis of the Cartesian three-dimensional coordinate system is along the width direction of the three-dimensional point cloud map, and the z-axis of the Cartesian three-dimensional coordinate system is along the height direction of the three-dimensional point cloud map;

[0055] Convert the point cloud to the serial number of the 3D occupancy grid map according to the coordinates of the point cloud. Planning the path using the grid serial number can greatly reduce the computational workload compared to the specific coordinate values and improve the path planning efficiency. The calculation formula is as follows:

[0056] ;

[0057] ;

[0058] ;

[0059] Among them, I x represents the grid serial number of point P in the x direction, I y represents the grid serial number of point P in the y direction, I z represents the grid serial number of point P in the z direction, P x represents the coordinate value of point P in the x direction, P y represents the coordinate value of point P in the y direction, P z represents the coordinate value of point P in the z direction.

[0060] Mark the current grid and the continuous grids in the vertical direction with the current grid as grids of the same attribute. The grid attributes include free, passable, and impassable. The three attributes are respectively recorded as 1, 2, and 3, and all passable grids are screened out;

[0061] Use a moving sliding window to calculate 9 grids, and count the height of each grid and the grids continuously occupied in the vertical direction, that is, in the z direction;

[0062] Taking the current grid as the central grid, calculate the height difference between the central grid and the adjacent grids. If the height difference is greater than the set threshold, then judge that the central grid is an impassable area. On the contrary, if the height difference is less than or equal to the set threshold, then judge that the central grid is a passable area;

[0063] Traverse all grids to find all passable areas.

[0064] S3. Set the inspection start point and inspection end point position information, and map them to the corresponding grids.

[0065] S4. According to all the passable grids screened out, use the A* algorithm to traverse the surrounding grids starting from the inspection start point until the inspection end point is searched. A global path is formed from the inspection start point to the inspection end point, and the global path can only pass through grids with the attribute of passable. Specifically as follows:

[0066] S41. Initialize the Open List list and the Close List list;

[0067] S42. Take out the grid with the lowest cost from the Open List and set it as the current grid;

[0068] S43. Move the current grid to the Close List, indicating that the grid has been visited;

[0069] S44. Evaluate the adjacent grids of the current grid, including the grids in the up, down, left, right, and diagonal directions. For each adjacent grid, calculate its cost, that is, the f value:

[0070] ;

[0071] Among them, g represents the actual cost from the starting point to the current grid, h represents the estimated cost from the current grid to the end grid, s represents the state of the grid, and s is determined by the following formula:

[0072] ;

[0073] S45. Add the evaluated adjacent grids 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;

[0074] S46. Repeat steps S42 to S45 until the currently evaluated adjacent grid is the end grid, the path planning is completed, a preliminary path is obtained, and the preliminary path is traced back in reverse;

[0075] S47. After generating the preliminary path, smooth the preliminary path to obtain the global path.

[0076] If there are multiple devices that need to be inspected, first plan the path from the starting point to the first inspection device, then plan the path from the first inspection device to the second inspection device, then plan the path from the second inspection device to the third inspection device, and so on, until the last inspection device, and finally generate the complete path.

[0077] The above embodiments only represent the specific implementation manners of the present invention. The description is relatively specific and detailed, but it should not be construed as a limitation on the scope of the patent of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several deformations and improvements can still be made, and these all belong to the protection scope of the present invention.

Claims

1. A method for planning inspection paths for factory operation and maintenance equipment, characterized in that: The steps include: S1. Use the laser synchronous positioning and mapping system to build a three-dimensional point cloud map of the factory area, and remove the dynamic obstacle point cloud from the three-dimensional point cloud map; S2. Calculate the resolution P of a single grid in the three-dimensional occupancy grid map grid Size, build a 3D occupancy grid map based on the 3D point cloud map, mark the current grid and the continuous grids in the vertical direction with the current grid as grids with the same attributes. The grid attributes include free, passable and impassable, and filter out all passable grids; S3, setting the inspection starting point and inspection end point location information, and corresponding them to the corresponding grid; S4. Based on all the traversable grids screened out, use the A* algorithm to traverse the surrounding grids from the inspection starting point until the inspection end point is found. A global path is formed from the inspection starting point to the inspection end point, and the global path can only pass through grids with traversable attributes.

2. The method according to claim 1, characterized in that In step S2, the resolution P of a single grid in the three-dimensional occupancy grid map is calculated. grid The size includes the following steps: Get the preset lower limit value P of the single grid resolution range min and the upper limit P max , use the following formula to calculate the resolution P of a single grid adaptation grid size: ； Among them, η is greater than 0 and less than 1; the larger η is, the higher P grid The smaller η is, the more accurate the obstacle boundary is and the safer the path planning is. The smaller η is, the greater the error rate is. grid The larger it is, the smaller the amount of computation and the lower the memory usage.

3. The method according to claim 1, characterized in that In step S2, constructing a three-dimensional occupancy grid map according to the three-dimensional point cloud map includes the following steps: Set the length direction, width direction and height direction of the 3D point cloud map, and establish a Cartesian 3D coordinate system with the lower left corner of the 3D point cloud map as the origin. The x-axis of the Cartesian 3D coordinate system is along the length direction of the 3D point cloud map, the main y-axis of the Cartesian 3D coordinate system is along the width direction of the 3D point cloud map, and the z-axis of the Cartesian 3D coordinate system is along the height direction of the 3D point cloud map; According to the coordinates of the point cloud, it is converted into the serial number of the three-dimensional occupancy grid map. The calculation formula is as follows: ； ； ； Among them, I x Indicates the grid number of point P in the x direction, I y Indicates the grid number of point P in the y direction, I z Indicates the grid number of point P in the z direction, P x Represents the coordinate value of point P in the x direction, P y Represents the coordinate value of point P in the y direction, P z Represents the coordinate value of point P in the z direction.

4. The method according to claim 3, characterized in that In step S22, the calculation of the value of n includes the following steps: Get the factory area S o , calculate the projection area S of the static obstacle point cloud on the xy plane static , calculate the value of η: ； When S static The larger the proportion of the plant area, the larger η is, resulting in fewer feasible paths. In order to reduce misjudgment of collisions, the grid resolution needs to be reduced. When S static The smaller the proportion within the plant area, the smaller η is, resulting in an increase in feasible paths. To avoid the exponential growth of the number of grids and the high computational and storage costs, the grid resolution needs to be increased.

5. The method according to claim 4, characterized in that The step S2 further comprises the following steps: Use a moving sliding window to calculate 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; The current grid is regarded as the central grid, and the height difference between the central grid and the adjacent grid is calculated. If the height difference is greater than the set threshold, the central grid is judged as an inaccessible area. Otherwise, if the height difference is less than or equal to the set threshold, the central grid is judged as a passable area. Traverse all grids and find all passable areas.

6. The method according to claim 5, characterized in that The step S4 specifically includes the following steps: S41, initialize the Open List and the Close List; S42, taking out the grid with the lowest cost from the Open List and setting it as the current grid; S43, moving the current grid to the Close List, indicating that the grid has been visited; S44, evaluate the adjacent grids of the current grid, including the grids in the upper, lower, left, right and diagonal directions, and for each adjacent grid, calculate its cost, i.e., f value: ； Among them, g represents the actual cost from the starting point to the current grid, h represents the estimated cost from the current grid to the end grid, and s represents the state of the grid, which is determined by the following formula: ； S45, adding the evaluated adjacent grid to the Open List, if the currently evaluated adjacent grid is already in the Open List, comparing the f value of the adjacent grid already in the Open List with the f value of the currently evaluated adjacent grid, and selecting the adjacent grid with the smallest f value to add to the Open List; S46, repeating steps S42 to S45 until the adjacent grid currently evaluated is the end grid, the path planning is completed, a preliminary path is obtained, and the preliminary path is traced back; S47: After the preliminary path is generated, the preliminary path is smoothed to obtain a global path.

Images