An automatic path planning method for underground scenes

Abstract

The present invention discloses an automatic path planning method for underground scenes, comprising obtaining a pre-constructed grid map, obtaining a proportional relationship between a world map and the grid map; extracting obstacle boundary information of the grid map, and generating an obstacle vector vector; generating a collision intensity matrix D through the pre-acquired installation size of a laser radar, the body size of a vehicle, the motion model of the vehicle, and the proportional relationship between the world map and the grid map; calculating a collision intensity map through the collision intensity matrix D and the obstacle vector vector, removing points where collisions occur, and retaining areas where no collisions occur; performing path planning on the collision intensity map through an A* algorithm, and generating an optimal global path. The present invention can effectively match the actual operating conditions of vehicles in underground tunnels, and meet actual unmanned driving requirements.

Description

Technical Field

[0001] The invention relates to an automatic path planning method for an underground scene, and belongs to the technical field of mining machinery scrapers. Background Art

[0002] With the development of autonomous driving technology, underground mine operations are gradually entering the ranks of autonomous driving. When the grid map of the construction site is known, the quality of automatic path planning directly affects the actual vehicle running speed, control difficulty and mining efficiency. Commonly used path planning algorithms, such as the A* algorithm and the greedy algorithm, can only obtain the local optimal path, but do not meet the steering and collision avoidance conditions of the actual vehicle driving.

[0003] In order to solve the problem of automatically planning paths to meet the needs of automatic driving operations of underground shovel loaders, there is no good targeted solution for the existing path planning algorithms. CN106500704A proposes a robot path planning method based on an improved genetic algorithm. This method first plans a shortest path, and then continuously repairs and iterates on this basis to obtain an optimal path. Due to the problems of safety and production tasks in the underground environment, it is impossible to repeatedly optimize and iterate a path; CN110515380A proposes a shortest path planning method based on turning weight constraints. By calculating the shortest path and performing weight penalties at the intersections, the shortest path has been repaired. Since the path planned by the shortest path is often a broken line, it cannot meet the linear control of the vehicle. At the same time, in addition to the intersections, the underground roads have small angle turns and direction adjustments, which are not considered in the patent. Summary of the invention

[0004] The purpose of the present invention is to overcome the deficiencies in the prior art and provide an automatic path planning method for underground scenes to improve the prior art.

[0005] To achieve the above object, the present invention is implemented by adopting the following technical solutions:

[0006] In a first aspect, the present invention provides an automatic path planning method for underground scenarios, comprising:

[0007] Get a pre-built raster map and get the proportional relationship between the world map and the raster map;

[0008] Extracting obstacle boundary information of the grid map and generating an obstacle vector vector;

[0009] Generate a collision intensity matrix D by using the pre-acquired installation size of the laser radar, the body size of the vehicle, the motion model of the vehicle, and the proportional relationship between the world map and the grid map;

[0010] The collision intensity map is calculated by using the collision intensity matrix D and the obstacle vector vector, and the points where collisions occur are removed, and the areas where no collisions occur are retained;

[0011] Path planning is performed on the collision intensity graph using the A* algorithm to generate an optimal global path.

[0012] Furthermore, the formula for obtaining the proportional relationship between the world map and the grid map is as follows:

[0013]

[0014] Among them: resolution: represents the proportional relationship between the world map and the grid map; origin: represents the rotation angle relationship between the image coordinate system and the actual map after the proportional relationship conversion.

[0015] Furthermore, the obstacle boundary information of the grid map is extracted to generate an obstacle vector vector, and the formula is as follows:

[0016]

[0017] Where: (x n ,y n ) represents the image coordinate information of the obstacle in the grid map.

[0018] Furthermore, the formula of the collision intensity matrix D is as follows:

[0019]

[0020] Furthermore, the collision intensity map is calculated by using the collision intensity matrix D and the obstacle vector vector, and the formula is as follows:

[0021]

[0022] Where: M represents the pixel matrix of the grid map, and the value of the pixel matrix is ​​the grayscale value under the corresponding image coordinate; D represents the collision intensity matrix; H represents the grid map with collision intensity obtained by convolution calculation of M and D.

[0023] Furthermore, the planned path is smoothed, including:

[0024] Extract a path coordinate point every two meters;

[0025] The extracted path coordinate points are fitted by a Bezier curve to obtain the fitted path points.

[0026] Furthermore, the conversion relationship between the grid map coordinate system and the world coordinate system is obtained; the fitted path points are converted into actual vehicle operation path points in the grid map through the conversion relationship between the grid map coordinate system and the world coordinate system for subsequent trajectory tracking.

[0027] In a second aspect, the present invention provides an automatic path planning device for underground scenes, comprising:

[0028] A grid map acquisition unit is used to acquire a pre-built grid map and acquire a proportional relationship between the world map and the grid map;

[0029] An obstacle vector generating unit, used for extracting obstacle boundary information of the grid map and generating an obstacle vector vector;

[0030] A collision intensity matrix generating unit, used to generate a collision intensity matrix D by using the pre-acquired installation size of the laser radar, the body size of the vehicle, the motion model of the vehicle, and the proportional relationship between the world map and the grid map;

[0031] A calculation unit, used to calculate a collision intensity map through the collision intensity matrix D and the obstacle vector vector, remove points where collisions occur, and retain areas where no collisions occur;

[0032] The optimal global path generation unit is used to perform path planning on the collision intensity map through the A* algorithm to generate an optimal global path.

[0033] In a third aspect, the present invention provides an automatic path planning device for underground scenarios, including a processor and a storage medium;

[0034] The storage medium is used to store instructions;

[0035] The processor is configured to operate according to the instructions to execute the steps of any of the aforementioned methods.

[0036] In a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of any of the aforementioned methods.

[0037] Compared with the prior art, the present invention has the following beneficial effects:

[0038] The present invention provides an automatic path planning method for underground scenes, which uses a collision intensity matrix to restrict the path planning map according to the actual size parameters and motion model of the vehicle, and processes the path generated by A* path planning by point sparseness and smooth insertion. The final planned path can effectively match the actual operating conditions of the vehicle in the underground tunnel and meet the actual unmanned driving needs. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a flow chart of an automatic path planning method for underground scenarios provided by an embodiment of the present invention;

[0040] Figure 2 is a collision grid diagram provided by an embodiment of the present invention;

[0041] Figure 3 is a schematic diagram of an optimal global path provided by an embodiment of the present invention;

[0042] Figure 4 It is a schematic diagram after smoothing provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0043] The present invention will be further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of the present invention.

[0044] Example 1

[0045] This embodiment introduces an automatic path planning method for underground scenarios, including:

[0046] Get a pre-built raster map and get the proportional relationship between the world map and the raster map;

[0047] Extracting obstacle boundary information of the grid map and generating an obstacle vector vector;

[0048] Generate a collision intensity matrix D by using the pre-acquired installation size of the laser radar, the body size of the vehicle, the motion model of the vehicle, and the proportional relationship between the world map and the grid map;

[0049] The collision intensity map is calculated by using the collision intensity matrix D and the obstacle vector vector, and the points where collisions occur are removed, and the areas where no collisions occur are retained;

[0050] Path planning is performed on the collision intensity graph using the A* algorithm to generate an optimal global path.

[0051] The application process of the automatic path planning method for underground scenarios provided in this embodiment specifically involves the following steps:

[0052] Step 1: Obtain the proportional relationship between grid pixels and actual size, and the conversion relationship between the grid map coordinate system and the world coordinate system;

[0053]

[0054] in:

[0055] Resolution: indicates the proportional relationship between the world map and the grid map, that is, the size of a pixel in the grid map corresponding to the actual scene;

[0056] origin; represents the rotation angle relationship between the image coordinate system and the actual map after the scale conversion.

[0057] Step 2: Extract obstacle boundary information from the grid map and generate obstacle vector vector;

[0058]

[0059] Where: (xn,yn) represents the image coordinate information of the obstacle in the grid map.

[0060] Step 3: Generate the collision intensity matrix D based on the installation size of the laser radar, the body size of the vehicle, the vehicle's motion model, and the resolution obtained in step 1:

[0061]

[0062] Step 4: Based on the collision strength matrix D obtained in step 3 and the obstacle vector vector obtained in step 2, a collision strength map is obtained by calculation, and the points where collisions occur are removed, leaving the areas where no collisions occur:

[0063]

[0064] Where: M represents the pixel matrix of the grid map, and the value of the pixel matrix is ​​the gray value under the corresponding image coordinate; D represents the collision intensity matrix; H represents the grid map with collision intensity obtained by convolution of M and D, that is, the collision grid map, such as Figure 2 shown.

[0065] Step 5: According to the actual loading and unloading conditions of mining operations, the path planning is carried out through the A* algorithm based on the collision intensity map, such as Figure 3 As shown;

[0066] Step 6: Path point sparseness and Bezier curve fitting; The paths planned by the A* algorithm are mostly broken lines, which are inconsistent with the actual vehicle motion control. The planned paths need to be smoothed. According to the actual motion conditions, a path coordinate point is extracted every two meters. The extracted path coordinate points are fitted with Bezier curves, such as Figure 4 shown.

[0067] Step 7: The fitted path points obtained in step 6 are converted into actual vehicle path points in the grid map through the coordinate transformation relationship obtained in step 1 for subsequent trajectory tracking.

[0068] Example 2

[0069] This embodiment provides an automatic path planning device for underground scenarios, including:

[0070] A grid map acquisition unit is used to acquire a pre-built grid map and acquire a proportional relationship between the world map and the grid map;

[0071] An obstacle vector generating unit, used for extracting obstacle boundary information of the grid map and generating an obstacle vector vector;

[0072] A collision intensity matrix generating unit, used to generate a collision intensity matrix D by using the pre-acquired installation size of the laser radar, the body size of the vehicle, the motion model of the vehicle, and the proportional relationship between the world map and the grid map;

[0073] A calculation unit, used to calculate a collision intensity map through the collision intensity matrix D and the obstacle vector vector, remove points where collisions occur, and retain areas where no collisions occur;

[0074] The optimal global path generation unit is used to perform path planning on the collision intensity map through the A* algorithm to generate an optimal global path.

[0075] Example 3

[0076] This embodiment provides an automatic path planning device for underground scenarios, including a processor and a storage medium;

[0077] The storage medium is used to store instructions;

[0078] The processor is used to operate according to the instructions to execute the steps of the method according to any one of Embodiment 1.

[0079] Example 4

[0080] This embodiment provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the steps of any method described in Embodiment 1 are implemented.

[0081] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. An automatic path planning method for underground scenes, characterized in that: include: Get a pre-built raster map and get the proportional relationship between the world map and the raster map; Extracting obstacle boundary information of the grid map and generating an obstacle vector vector; The collision intensity matrix D is generated by the pre-acquired installation size of the laser radar, the body size of the vehicle, the motion model of the vehicle, and the proportional relationship between the world map and the grid map; the formula of the collision intensity matrix D is as follows: ； The collision intensity map is calculated by using the collision intensity matrix D and the obstacle vector vector, and the points where collisions occur are removed to retain areas where no collisions occur. The calculation formula is as follows: ； Where: M represents the pixel matrix of the grid map, and the value of the pixel matrix is ​​the gray value under the corresponding image coordinate; D represents the collision intensity matrix; H represents the grid map with collision intensity obtained by convolution calculation of M and D; Path planning is performed on the collision intensity graph using the A* algorithm to generate an optimal global path.

2. The automatic path planning method for underground scenarios according to claim 1, characterized in that: The formula for obtaining the proportional relationship between the world map and the grid map is as follows: ； in: : Indicates the proportional relationship between the world map and the grid map; ; Represents the rotation angle relationship between the image coordinate system and the actual map after the scale conversion.

3. The automatic path planning method for underground scenarios according to claim 1 is characterized by: The obstacle boundary information of the grid map is extracted to generate the obstacle vector vector. The formula is as follows: ； in: Represents the image coordinate information of the obstacle in the grid map.

4. The automatic path planning method for underground scenarios according to claim 1, characterized in that: Smoothing the planned path includes: Extract a path coordinate point every two meters; The extracted path coordinate points are fitted by a Bezier curve to obtain the fitted path points.

5. The automatic path planning method for underground scenarios according to claim 4 is characterized in that: Obtaining the conversion relationship between the grid map coordinate system and the world coordinate system; converting the planned path points in the grid map into actual vehicle operation path points through the conversion relationship between the grid map coordinate system and the world coordinate system for subsequent trajectory tracking.

6. An automatic path planning device for underground scenes, characterized in that: include: A grid map acquisition unit is used to acquire a pre-built grid map and acquire a proportional relationship between the world map and the grid map; An obstacle vector generating unit, used for extracting obstacle boundary information of the grid map and generating an obstacle vector vector; The collision intensity matrix generating unit is used to generate a collision intensity matrix D by using the pre-acquired installation size of the laser radar, the body size of the vehicle, the motion model of the vehicle, and the proportional relationship between the world map and the grid map; the formula of the collision intensity matrix D is as follows: ； The calculation unit is used to calculate the collision intensity map through the collision intensity matrix D and the obstacle vector vector, remove the points where collisions occur, and retain the areas where no collisions occur; the calculation formula is as follows: ； Where: M represents the pixel matrix of the grid map, and the value of the pixel matrix is ​​the gray value under the corresponding image coordinate; D represents the collision intensity matrix; H represents the grid map with collision intensity obtained by convolution calculation of M and D; The optimal global path generation unit is used to perform path planning on the collision intensity map through the A* algorithm to generate an optimal global path.

7. An automatic path planning device for underground scenes, characterized in that: including processor and storage medium; The storage medium is used to store instructions; The processor is used to operate according to the instructions to execute the steps of the method according to any one of claims 1 to 5.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the method according to any one of claims 1 to 5 are implemented.

Images