CN115451989A - Underground vehicle path planning method, device and equipment - Google Patents

Abstract

The invention discloses an underground vehicle path planning method, which comprises the following steps: acquiring point cloud data in an effective range, and converting the point cloud data into a binary image; extracting a skeleton of the binary image by adopting a thinning algorithm; obtaining alternative paths through the skeleton, and selecting a proper alternative path as a path line; and smoothing the route line to obtain the planned route of the vehicle. The path planning method is simple and efficient, and can be used for path planning under the condition of straight going and turning.

Description

Underground vehicle path planning method, device and equipment

Technical Field

The invention relates to the technical field of unmanned vehicle driving, in particular to a method, a device and equipment for planning paths of underground unmanned vehicles.

Background

The unmanned driving of vehicles is a future development trend, particularly for underground mines with severe environments, and the requirement for realizing unmanned driving is more urgent. Early automated navigation of underground mines was primarily aided by drawing lines on the floor or lights on the roof. The current vehicle navigation methods are mainly divided into two categories: absolute navigation and reactive navigation. Absolute navigation requires the acquisition of real-time coordinates of a vehicle relying on satellite positioning, is more used in indoor and outdoor mobile robots, and is impractical for weak signal strengths underground. Reactive navigation is therefore mainly used in underground mines. The reactive navigation vehicle does not need to know the position in the whole driving map, only needs to know the relative position in the current visible environment, and assists the vehicle navigation according to the relative position. The classic wall-following technique for reactive navigation is proposed by scholars engaged in the research of the mine field for the special structure of the underground roadway corridor type. Due to the corridor-like special structure of the underground roadway, the learners take the central line of the roadway as a planned path. Larsson proposes a Hough transform-based method based on the thought, two parallel straight lines are detected in laser radar data, and the central lines of the two parallel straight lines are taken as a locally planned path of reactive navigation. However, the method is only applicable to the local path planning of the vehicle in a straight running mode, and no suitable method is available for the path planning of vehicle turning positions.

Disclosure of Invention

In view of this, embodiments of the present invention provide a method, an apparatus, and a device for underground vehicle path planning, which can simply and efficiently implement underground vehicle path planning, including path planning at a turn.

The technical scheme of the embodiment of the invention is realized as follows:

in a first aspect, the present invention provides a method for planning a route of an underground vehicle, including: acquiring point cloud data in an effective range, and converting the point cloud data into a binary image; extracting a skeleton of the binary image by adopting a thinning algorithm; obtaining alternative paths through the skeleton, and selecting a proper alternative path as a path line; and smoothing the route line to obtain the planned route of the vehicle.

Preferably, acquiring point cloud data within the effective range includes: the method comprises the steps of acquiring real-time point cloud data of a sensor, determining an effective range of the data according to parameters of the sensor and an actual working environment, and extracting the point cloud data in the effective range.

Preferably, the point cloud data is converted into a binary map, comprising: sequentially connecting two adjacent points in the point cloud data to form an envelope line; gridding the area where the envelope line is located; the grid color inside the envelope is defined as black and takes a value of 0, the grid color outside the envelope is defined as white and takes a value of 1, and the black and white grids form a binary image.

Preferably, in gridding the region where the envelope is located, the precision of the grid is defined as: acquiring the width value of a roadway where a vehicle runs, wherein the accuracy value of the grid is 1/6-1/4 of the width of the roadway when the vehicle runs straight; when the vehicle turns, the accuracy of the grid is 1/20-1/10 of the width of the roadway.

Preferably, the alternative path is obtained through a skeleton, comprising: acquiring a grid point which is closest to the origin and has a value of 0 as a search starting point of a search tree, sequentially searching along a set direction and judging whether the value of the grid point is 0, if so, adding the grid point into the search tree, and if so, stopping searching; searching from leaf nodes of the search tree to father nodes layer by layer until a search starting point is found; the alternative path is generated by connecting the search paths with the polyline.

Preferably, the setting direction is set to right, upper right, lower right, upper, lower left, upper left, lower left.

Preferably, selecting a suitable alternative path as the path line includes: acquiring the vehicle running direction, the running mode and the minimum path length, and if the alternative path length is greater than the minimum path length, performing the following judgment: calculating the angle of the alternative path; calculating a first angle formed by the alternative path angle and the vehicle driving direction; if the current driving mode is straight, selecting the alternative path with the minimum first angle absolute value as a corresponding path line; if the current driving mode is left turning, selecting the alternative path with the maximum first angle value as a corresponding path line; and if the current driving mode is right turning, selecting the alternative path with the minimum first angle value as the corresponding path line.

Preferably, the angle of the alternative path is calculated, including: and connecting the starting point and the end point of the alternative path to form a connecting line, and acquiring the angle of the connecting line, namely the angle of the alternative path.

In a second aspect, the present invention also discloses an underground vehicle path planning device, including: the data acquisition module is used for acquiring real-time point cloud data of the sensor; the data processing module is used for converting the point cloud data into a binary image and extracting a skeleton of the binary image by adopting a thinning algorithm; obtaining alternative paths through the skeleton, and selecting a proper alternative path as a path line; and the path planning module is used for smoothing the path line to obtain a planned path of the vehicle.

In a third aspect, the present invention also discloses an underground vehicle path planning device, which includes: a memory for storing a computer program; a processor for implementing the method of underground vehicle path planning of an embodiment of the present invention when executing the computer program stored in the memory.

According to the scheme of the embodiment of the invention, auxiliary equipment is not required to be laid, so that the economy is good; only the data of the laser radar is processed, the running speed is high, the efficiency is high, and the response time of a vehicle control program can be greatly reduced. In addition, the scheme of the embodiment of the invention can be used for path planning not only under the condition of straight going, but also under the condition of turning.

Drawings

The accompanying drawings, which are included to provide a further understanding of the invention, illustrate embodiments of the invention and together with the description serve to explain the invention and do not constitute a limitation of the invention. In the drawings:

FIG. 1 is a schematic flow chart of a method for planning a path of an underground vehicle according to an embodiment of the present invention;

FIG. 2 is a schematic view of a frame of lidar point cloud data collected in a downhole roadway in accordance with an embodiment of the present invention;

FIG. 3 is a schematic diagram of point cloud data within a valid range according to an embodiment of the present invention;

FIG. 4 is a schematic diagram of an envelope according to an embodiment of the present invention;

FIG. 5 is a diagram illustrating a binary image according to an embodiment of the present invention;

FIG. 6 is a skeleton diagram of a binary image according to an embodiment of the present invention;

FIG. 7 is a schematic diagram of an alternative path in an embodiment of the invention;

FIG. 8 is a diagram illustrating a path planning result in a forward direction according to an embodiment of the present invention;

FIG. 9 is a schematic diagram illustrating a path planning result in a left turn direction according to an embodiment of the present invention;

FIG. 10 is a diagram illustrating a path planning result in a right turn direction according to an embodiment of the present invention;

fig. 11 is a schematic structural diagram of an underground vehicle path planning device according to an embodiment of the present invention.

Detailed Description

The technical solution of the present invention is further described in detail with reference to the drawings and specific embodiments. It should be understood that the examples provided herein are merely illustrative of the present invention and are not intended to limit the present invention. In addition, the following embodiments are provided as partial embodiments for implementing the present invention, not all embodiments for implementing the present invention, and the technical solutions described in the embodiments of the present invention may be implemented in any combination without conflict.

Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used in the description of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention.

In the mining process, a plurality of large-scale mechanical equipment such as drilling machines, tunneling machines, scraper machines and the like are needed, and the realization of intelligent automatic driving of underground mining mechanical equipment has great significance for improving the mining efficiency and the safety. Because underground mines are difficult to receive signals of ground communication base stations, and the cost for laying base stations underground is high, underground vehicles generally adopt reactive navigation. The underground mine has complex environment, and is particularly suitable for the turning of a roadway, and the method, the device and the equipment for planning the underground vehicle path can realize the path planning of the underground vehicle, particularly the path planning of the turning, so that the automatic driving of the underground vehicle is safer and more efficient.

Referring to fig. 1, a method for planning a path of an underground vehicle according to an embodiment of the present invention includes the following steps:

step 101: acquiring point cloud data in an effective range, and converting the point cloud data into a binary image;

step 102: extracting a skeleton of the binary image by adopting a thinning algorithm;

step 103: obtaining alternative paths through the skeleton, and selecting a proper alternative path as a path line;

step 104: and smoothing the route line to obtain the planned route of the vehicle.

In another embodiment of the present invention, the acquiring of the point cloud data in the effective range in step 101 includes acquiring raw point cloud data and preprocessing the raw point cloud data, that is: the method comprises the steps of obtaining real-time point cloud data of a sensor, determining an effective range of the data according to parameters of the sensor and an actual working environment, and extracting the point cloud data in the effective range. Referring to fig. 2 and 3, in an embodiment, the sensor is a lidar, fig. 2 is a frame of lidar data collected in a downhole tunnel, the right triangle in the figure represents the radar, and 541 data points are totally collected. The method for determining the effective range according to the parameters of the radar sensor and the actual environment comprises the following steps: firstly, the parameters of the radar, such as the working area of the Sick radar is 0.5m-50m, the scanning range when the reflectivity is 10% is 18m, the reflectivity of the underground roadway, the smoothness of two walls, the rock property and the like are combined, and the parameters are set to be 20m in the example; second, in practical environments, such as when the downhole radar is mounted in the center of a forklift truck, where the forklift truck width is 2.7m, the distance that the radar scans into the roadway must be greater than 1.35m, so the range of valid radar data points must be greater than 1.35m. In conclusion, the effective range of the radar can be set to be 1.35m-20m. After the effective range of the data is determined, the point cloud data outside the effective range is removed, the rest data are stored for next processing, the data processing result in the effective range is shown in fig. 3, and total 481 points of the data of the radar data in the effective range exist.

In step 101, converting the point cloud data into a binary image, including: connecting two adjacent points in the point cloud data in the effective range in sequence to form an envelope line, in a certain embodiment, connecting two adjacent points in fig. 3 in sequence to form a closed envelope line, in a certain embodiment, calculating a minimum envelope matrix of the envelope line, as shown in fig. 4, preferably, expanding the minimum envelope matrix outwards by 2m to leave certain redundancy, and obtaining a range Xlim = [ -4.15,21.79], ylim = [ -9.02,11.34] of the graph; then, gridding is performed on the region where the envelope is located, in a certain embodiment, the grid precision is set to be 0.5m, the coordinate origin (0, 0) of the local coordinate system of the laser radar is selected as the grid center point coordinate, the grid color inside the envelope is defined as black, the value of the grid color is 0, the grid color outside the envelope is defined as white, the value of the grid color is 1, and the grid of black and white forms a binary image, as shown in fig. 5.

In an embodiment, in the process of converting the point cloud data into the binary map, when the region where the envelope is located is gridded, the accuracy of the grid may be defined as follows: acquiring the width value of a roadway where a vehicle runs, wherein when the vehicle runs straight, the precision value of the grid is 1/6-1/4 of the width of the roadway, preferably 1/6; when the vehicle turns, the accuracy of the grid is 1/20-1/10 of the roadway width, preferably 1/15.

In step 102, a thinning algorithm is used to extract the skeleton of the binary image, and thinning the binary image is to strip the binary image layer by layer, remove some points from the original image, but keep the original shape until the skeleton of the image is obtained. The skeleton obtaining method of the binary image has various methods, and can be applied to obtaining the binary image skeleton of the invention, including but not limited to an image skeleton extracting method based on fire simulation, a skeleton extracting method based on a distance field and an image skeleton extracting method based on a maximum disc. In one embodiment, a Zhang-suen refinement algorithm is used, and one iteration of the Zhang-suen refinement algorithm consists of the following steps: (1) marking boundary points to be deleted; (2) deleting the marked points; (3) continuing to mark the remaining boundary points to be deleted; and (4) deleting the marked points. This basic process is applied iteratively until there are no more points to be deleted, at which point the algorithm terminates, generating a skeleton for the region. And refining the binary image of the figure 5 by adopting a Zhang-suen refining algorithm to extract a skeleton, so as to obtain the skeleton of the binary image shown in the figure 6.

In step 103, obtaining an alternative path through the skeleton includes: setting coordinate origin points (0, 0) of a local coordinate system of the laser radar, acquiring grid points which are closest to the origin points and have values of 0 as search starting points of a search tree, sequentially searching and judging whether the values of the grid points are 0 or not along a set direction (the density of direction lines and the density of the direction lines can be determined according to actual needs, and if the density of the direction lines is set at intervals of 30 degrees, adding the grid points into the search tree if the values of the grid points are 0, and stopping searching if the values of the grid points are 1; searching from leaf nodes of the search tree to father nodes layer by layer until a search starting point is found; and generating an alternative path by connecting the search paths by a broken line under a Cartesian coordinate system taking the position of the scanner as an origin. In one embodiment, the skeleton of the binary map in fig. 6 is processed as follows: selecting a pixel point with the value of 0 closest to the origin of the coordinate as a search starting point, searching pixel points with the value of 0 which are not searched from the search starting point along the directions of right, upper right, lower right, upper, lower, upper left, lower left and left, and establishing a search tree for storing the pixel points with the value of 0. After the search tree is generated, searching from leaf nodes of the search tree to a parent node layer by layer until a root node of the search tree is found, and ending, connecting the search paths by using a broken line to generate alternative paths shown in fig. 7, wherein 6 alternative paths are obtained in total in fig. 7.

In step 103, selecting a suitable alternative path as a path line includes: acquiring a vehicle running direction, a vehicle running mode and a minimum path length, wherein in one embodiment, the vehicle is required to input the vehicle running mode (straight running, left turning and right turning) and a path length range in the process of advancing, acquiring the path length according to the path length range, acquiring the current running direction of the vehicle, judging whether the alternative path length is greater than the minimum path length, and if so, judging as follows (directly abandoning the alternative path with the alternative path length less than the minimum path length):

calculating the angle of the alternative path; calculating a first angle formed by the alternative path angle and the vehicle driving direction; if the current driving mode is straight, selecting the alternative path with the minimum first angle absolute value as a corresponding path line; if the current driving mode is left turning, selecting the alternative path with the maximum first angle value as a corresponding path line; and if the current driving mode is the right turn, selecting the alternative path with the minimum first angle value as the corresponding path line.

In one embodiment, the following method is used for calculating the angle of the alternative path: and connecting the starting point and the end point of the alternative path to form a connecting line, and acquiring the angle of the connecting line, namely the angle of the alternative path.

In one embodiment, the lengths and angles of the 6 candidate paths in fig. 7 are calculated, and a first angle formed by the candidate path angle and the vehicle driving direction is calculated, where the minimum length of the set path is 6m, the candidate paths with the lengths smaller than 6m are eliminated, and the following processing is performed on the remaining candidate paths:

if the current running mode is straight running, selecting the path with the minimum absolute value of the first angle, wherein the length of the finally selected path is 16.00m, the angle is-1.72 degrees, and the smoothed planned path is shown in the attached figure 8;

if the current driving mode is left turn, selecting the path with the maximum first angle value, wherein the length of the selected path is 6.15m, the angle is 68.18 degrees, and the smoothed planned path is shown in the attached figure 9;

if the current driving mode is right turning, the path with the minimum first angle is selected, the length of the selected path is 13.29m, the angle is-25.24 degrees, and the smoothed planned path is shown in the attached figure 10.

In order to implement the method of the embodiment of the present invention, the present invention further provides an underground vehicle path planning apparatus, referring to fig. 11, the apparatus includes: the data acquisition module is used for acquiring real-time point cloud data of the sensor; the data processing module is used for converting the point cloud data into a binary image and extracting a skeleton of the binary image by adopting a thinning algorithm; obtaining alternative paths through the skeleton, and selecting a proper alternative path as a path line; and the path planning module is used for smoothing the path line to obtain a planned path of the vehicle.

In some embodiments, the data acquisition module is configured to acquire real-time point cloud raw data of the sensor, pre-process the point cloud raw data, and remove data that does not meet requirements, where removing data that does not meet requirements is not a necessary step, and in some embodiments, this step may not be present.

In some embodiments, the data processing module may perform the following data processing: sequentially connecting two adjacent points in point cloud data of a data acquisition module in sequence to form an envelope line, sequentially connecting two adjacent points as shown in fig. 3 to form a closed envelope line in one embodiment, calculating a minimum envelope matrix of the envelope line in one embodiment, and expanding the minimum envelope matrix outwards by 2m to leave certain redundancy as shown in fig. 4 to obtain a range Xlim = [ -4.15,21.79], ylim = [ -9.02,11.34] of the graph; then gridding the area where the envelope is located, in a certain embodiment, the grid precision is set to be 0.5m, the coordinate origin (0, 0) of a local coordinate system of the laser radar is selected as the coordinate of the grid center point, the grid color inside the envelope is defined as black, the value is 0, the grid color outside the envelope is defined as white, the value is 1, and the black and white grids form a binary image as shown in fig. 5; and then extracting a framework of the binary image through a thinning algorithm, generating alternative paths by utilizing the framework, screening the paths according to set or acquired conditions, and selecting a proper path line.

In some embodiments, the path planning module smoothes the path lines of the data processing module, where the path lines are broken lines, and the method for smoothing the broken lines includes, but is not limited to, conventional fitting and interpolation.

The embodiment of the invention also provides underground vehicle path planning equipment, which comprises a memory, a route planning module and a route planning module, wherein the memory is used for storing a computer program; a processor for implementing the method of underground vehicle path planning of any embodiment of the present invention when executing a computer program stored in a memory.

The scheme of the embodiment of the invention can be used for the environment of a corridor type structure besides the underground environment. The method of the embodiment belongs to a path planning method based on images, is not only suitable for local path planning, but also can be used for global path planning of a global map of an underground environment.

The underground vehicle path planning method disclosed by the embodiment of the invention can be applied to a processor or is realized by the processor. The processor may be an integrated circuit chip having signal processing capabilities. In implementation, the steps of the underground vehicle path planning method may be accomplished by instructions in the form of hardware integrated logic circuits or software in the processor. The Processor may be a general purpose Processor, a Digital Signal Processor (DSP), or other programmable logic device, discrete gate or transistor logic device, discrete hardware components, etc. The processor is used to implement or perform the methods, steps, and logic blocks disclosed in the embodiments of the present invention. The general purpose processor may be a microprocessor or any conventional processor or the like. The steps of the method disclosed by the embodiment of the invention can be directly implemented by a hardware decoding processor, or can be implemented by combining hardware and software modules in the decoding processor. The software modules may be located in a storage medium located in a memory, and the processor reads information in the memory and performs the steps of the method for underground vehicle path planning provided by the embodiments of the present invention in combination with hardware thereof.

As will be appreciated by one skilled in the art, embodiments of the present invention may be provided as a method, system, or computer program product. Accordingly, embodiments of the present invention may take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Furthermore, embodiments of the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, optical storage, and the like) having computer-usable program code embodied therein.

Embodiments of the present invention are described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flow diagrams and/or block diagrams, and combinations of flows and/or blocks in the flow diagrams and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing system to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing system, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing system to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.

These computer program instructions may also be loaded onto a computer or other programmable data processing system to cause a series of operational steps to be performed on the computer or other programmable system to produce a computer implemented process such that the instructions which execute on the computer or other programmable system provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.

The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.

Claims (10)

1. A method of underground vehicle path planning, comprising:

acquiring point cloud data in an effective range, and converting the point cloud data into a binary image;

extracting a skeleton of the binary image by adopting a thinning algorithm;

obtaining alternative paths through the skeleton, and selecting a proper alternative path as a path line;

and smoothing the route line to obtain the planned route of the vehicle.

2. An underground vehicle path planning method according to claim 1,

the acquiring point cloud data in the effective range comprises:

the method comprises the steps of obtaining real-time point cloud data of a sensor, determining an effective range of the data according to parameters of the sensor and an actual working environment, and extracting the point cloud data in the effective range.

3. An underground vehicle path planning method according to claim 1,

the converting the point cloud data into a binary map comprises:

sequentially connecting two adjacent points in the point cloud data to form an envelope curve; gridding the area where the envelope line is located;

and defining the grid color inside the envelope as black and taking the value as 0, defining the grid color outside the envelope as white and taking the value as 1, and forming the binary image by the black and white grids.

4. An underground vehicle path planning method according to claim 3,

in the gridding of the region where the envelope line is located, the precision of the grid is defined as:

acquiring the width value of a roadway where a vehicle runs, wherein when the vehicle runs straight, the precision value of the grid is 1/6-1/4 of the width of the roadway; when the vehicle turns, the precision value of the grid is 1/20-1/10 of the width of the roadway.

5. An underground vehicle path planning method according to claim 1,

obtaining alternative paths through the skeleton, including:

acquiring a grid point which is closest to the origin and has a value of 0 as a search starting point of a search tree, sequentially searching along a set direction and judging whether the value of the grid point is 0, if so, adding the grid point into the search tree, and if so, stopping searching;

searching from leaf nodes of the search tree to father nodes layer by layer until the search starting point is found, and ending;

and generating the alternative path by connecting the search path by the polyline.

6. An underground vehicle path planning method according to claim 5,

the set direction is set to right, upper right, lower right, upper, lower, upper left, lower left, left direction.

7. An underground vehicle path planning method according to claim 6,

the selecting the appropriate alternative path as the path line includes:

acquiring a vehicle driving direction, a driving mode and a minimum path length, and if the alternative path length is greater than the minimum path length, performing the following judgment:

calculating the angle of the alternative path;

calculating a first angle formed by the alternative path angle and the vehicle driving direction;

if the current running mode is straight running, selecting the alternative path with the minimum first angle absolute value as the corresponding path line;

if the current driving mode is left turning, selecting the alternative path with the maximum first angle value as a corresponding path line;

and if the current running mode is right turn, selecting the alternative path with the minimum first angle value as the corresponding path line.

8. An underground vehicle path planning method according to claim 7,

the calculating the angle of the alternative path comprises:

and connecting the starting point and the end point of the alternative path to form a connecting line, and acquiring the angle of the connecting line, namely the angle of the alternative path.

9. An underground vehicle path planning apparatus, comprising:

the data acquisition module is used for acquiring real-time point cloud data of the sensor;

the data processing module is used for converting the point cloud data into a binary image and extracting a skeleton of the binary image by adopting a thinning algorithm; obtaining alternative paths through the skeleton, and selecting a proper alternative path as a path line;

and the path planning module is used for smoothing the path line to obtain a planned path of the vehicle.

10. An underground vehicle path planning apparatus, comprising:

a memory for storing a computer program;

a processor for implementing the method of underground vehicle path planning as claimed in any one of claims 1 to 8 when executing the computer program stored in the memory.

Images