CN109839112B - Underground operation equipment positioning method, device and system and storage medium - Google Patents
Underground operation equipment positioning method, device and system and storage medium Download PDFInfo
- Publication number
- CN109839112B CN109839112B CN201910179674.5A CN201910179674A CN109839112B CN 109839112 B CN109839112 B CN 109839112B CN 201910179674 A CN201910179674 A CN 201910179674A CN 109839112 B CN109839112 B CN 109839112B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- pose
- roadway
- probability
- underground
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Abstract
The embodiment of the application discloses a method, a device, a system and a storage medium for positioning underground operation equipment, wherein the method comprises the following steps: acquiring a roadway waist line point cloud and a roadway bottom plate point cloud which correspond to the operation equipment at present; carrying out first point cloud matching according to the roadway waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof, and determining a pose search range for second point cloud matching based on the probability of the initial pose; and according to the roadway floor point cloud, the underground two-dimensional probability positioning map and the initial pose, performing second point cloud matching in the pose searching range based on a histogram filtering method, and determining the current pose of the operation equipment according to the registration pose meeting set conditions. The positioning precision and the response speed of the positioning of the underground operation equipment are effectively improved, and the requirement on the real-time performance of control is favorably met.
Description
Technical Field
The application relates to the field of underground operation, in particular to a method, a device and a system for positioning underground operation equipment and a storage medium.
Background
With the development of national economy, the demand and utilization of various mineral resources are higher and higher. The traditional mining mode is difficult to meet the requirements of mine enterprises on cost, efficiency, benefit, safety, environmental protection and the like, and particularly, the application of unmanned mining technology to underground metal mines with large resource quantity, high mining strength and large ore breaking scale is extremely urgent. Compared with manual operation, unmanned operation of underground operation equipment (such as underground mining equipment) has the advantages of improving mining production efficiency, reducing mining cost, improving mining operation environment, ensuring operation safety of field workers and the like.
However, to achieve control of autonomous walking, autonomous working, and the like of the underground working apparatus, it is critical to acquire the pose (i.e., position and attitude) of the apparatus. Although the vehicle pose acquisition is not a problem with the rapid development of ground unmanned technology based on the support of a GPS and the support of a high-precision map technology, due to the particularity of an underground limited space environment, GPS signals cannot be received to position the vehicle, the precise control of equipment has high requirements on pose acquisition, and the real-time and accurate acquisition of the equipment pose information is still a difficult problem and is a key technology for restricting future unmanned mining development.
Based on Wi-Fi positioning, bluetooth positioning, RFID (radio frequency identification) positioning, UWB (ultra wide band) positioning, infrared technology, ultrasonic and other positioning technologies, although the positioning technologies are applied in the related technologies, the methods mostly need to install related positioning devices in the roadway, and the erection cost and the maintenance cost are high; in addition, point cloud positioning based on a three-dimensional laser radar installed on equipment is also applied, but the related method has low matching speed due to large point cloud data volume or more three-dimensional grids, and the real-time requirement of control is difficult to meet.
Disclosure of Invention
In view of this, embodiments of the present application provide a method, an apparatus, a system, and a storage medium for positioning underground working equipment, and aim to improve positioning accuracy and response speed of positioning underground working equipment to meet real-time requirements of control.
The technical scheme of the embodiment of the application is realized as follows:
in a first aspect, an embodiment of the present application provides a method for positioning an underground working device, including:
acquiring a roadway waist line point cloud and a roadway bottom plate point cloud which correspond to the operation equipment at present;
carrying out first point cloud matching according to the roadway waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof, and determining a pose search range for second point cloud matching based on the probability of the initial pose;
and performing second point cloud matching in the pose searching range based on a histogram filtering method according to the roadway floor point cloud, the underground two-dimensional probability positioning map and the initial pose, and determining the current pose of the operating equipment according to the registration pose meeting set conditions.
In a second aspect, an embodiment of the present application provides an underground working equipment positioning device, including:
the acquisition module is used for acquiring the roadway waist line point cloud and the roadway bottom plate point cloud which correspond to the operation equipment at present;
the first matching determination module is used for performing first point cloud matching according to the roadway waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof and determining a pose search range for second point cloud matching based on the probability of the initial pose;
and the second matching determination module is used for matching the second point cloud in the pose search range based on a histogram filtering method according to the roadway floor point cloud, the underground two-dimensional probability positioning map and the initial pose, and determining the current pose of the operation equipment according to the registration pose meeting set conditions.
In a third aspect, an embodiment of the present application provides an underground working equipment positioning system, including:
a memory for storing an executable program;
and the processor is used for realizing the underground working equipment positioning method of the embodiment when executing the executable program stored in the memory.
In a fourth aspect, the present application provides a computer storage medium storing an executable program, where the executable program, when executed by a processor, implements the method for locating underground working equipment according to the foregoing embodiments.
According to the technical scheme of the embodiment of the application, first point cloud matching is carried out according to the lane waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof, and a pose searching range for second point cloud matching is determined based on the probability of the initial pose; according to the tunnel bottom plate point cloud, the underground two-dimensional probability positioning map and the initial pose, the second point cloud matching is carried out in the pose searching range based on a histogram filtering method, the current pose of the operation equipment is determined according to the registration pose meeting set conditions, the efficient matching and positioning of the three-dimensional point cloud can be carried out by fully utilizing the characteristics of the underground tunnel, the efficiency of matching operation is improved by utilizing the first point cloud matching of the tunnel waist line point cloud and the underground two-dimensional probability positioning map and the second point cloud matching of the tunnel bottom plate point cloud and the underground two-dimensional probability positioning map, the information of the three-dimensional point cloud is fully utilized, the positioning accuracy and the response speed of the underground operation equipment positioning are effectively improved, and the real-time requirement of control is favorably met.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention. In the drawings:
FIG. 1 is a schematic flow chart of a method for locating subterranean operation equipment in an embodiment of the present disclosure;
FIG. 2 is a schematic diagram of an application scenario of a positioning method for underground working equipment according to an embodiment of the present application;
FIG. 3 is a schematic diagram of an underground mining apparatus acquiring point cloud data in real time according to an embodiment of the present disclosure;
FIG. 4 is a schematic diagram of a data store of a subsurface two-dimensional probabilistic positioning map in an embodiment of the present application;
FIG. 5 is a schematic diagram of a cloud of underground roadway waistlines in an embodiment of the present application;
FIG. 6 is a schematic diagram of an underground roadway floor point cloud in an embodiment of the present application;
FIG. 7 is a schematic diagram of a positioning device for underground working equipment according to an embodiment of the present disclosure;
fig. 8 is a schematic diagram of a positioning system for underground working equipment in an embodiment of the present application.
Detailed Description
The technical solution of the present application is further described in detail with reference to the drawings and specific embodiments of the specification. It should be understood that the examples provided herein are merely illustrative of the present application and are not intended to limit the present application. In addition, the following examples are provided as partial examples for implementing the present application, not all examples for implementing the present application, and the technical solutions described in the examples of the present application 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 application belongs. The terminology used herein in the description of the present application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used herein, the term "and/or" includes any and all combinations of one or more of the associated listed items.
The embodiment of the application provides an underground operation equipment positioning method which is used for meeting the requirement of real-time positioning of underground operation equipment. The underground working apparatus may be underground mining apparatus for mining or other excavation apparatus for underground mining. It should be noted that, in this embodiment, a sensor for collecting information is arranged on the underground operation device, such as a three-dimensional laser scanning device and an IMU (Inertial measurement unit), and the underground operation device may construct a three-dimensional point cloud map in the underground roadway based on the three-dimensional laser scanning device and the IMU by using a SLAM (simultaneous localization and mapping) method.
Referring to fig. 1, in an embodiment of the present application, the underground working equipment is underground mining equipment, and the underground working equipment positioning method may include:
As shown in fig. 2, underground mining equipment may be walked within an underground roadway, with an IMU and a three-dimensional lidar mounted on the underground mining equipment. During operation, the underground mining equipment can acquire three-dimensional point cloud information of the surrounding environment in real time (as shown in figure 3). Illustratively, the three-dimensional lidar acquisition generates three-dimensional point cloud information that includes three-dimensional coordinates and reflection intensities corresponding to the points. And extracting the point cloud of the roadway waist line and the point cloud of the roadway floor corresponding to the underground mining equipment at present based on the three-dimensional point cloud information generated by the three-dimensional laser radar. Here, the point cloud of the lane waistline includes point clouds corresponding to two sides of the lane scanned by the three-dimensional laser radar, and the point cloud of the lane bottom plate includes point clouds corresponding to the lane bottom plate scanned by the three-dimensional laser radar. Alternatively, the three-dimensional point cloud information may be divided into two-dimensional lane waist line point cloud scanned by a three-dimensional lidar centerline and three-dimensional lane floor point cloud extracted by using a random sample consensus algorithm (RANSAC).
102, performing first point cloud matching according to the roadway waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof, and determining a pose search range for second point cloud matching based on the probability of the initial pose.
In this embodiment, the underground two-dimensional probabilistic positioning map may be pre-stored data, that is, the underground mining equipment has previously acquired, generated, or loaded the underground two-dimensional probabilistic positioning map.
In some embodiments, before performing step 101, further comprising:
and constructing an underground two-dimensional probabilistic positioning map based on the instant positioning and map construction SLAM method.
In some embodiments, constructing a subsurface two-dimensional probabilistic positioning map based on a SLAM method includes:
acquiring three-dimensional point cloud information generated on the operation equipment based on a sensor and an SLAM method;
extracting corresponding tunnel waist line point cloud and tunnel bottom plate point cloud based on the three-dimensional point cloud information;
determining a two-dimensional grid based on the three-dimensional point cloud information, and respectively projecting the roadway waist line point cloud and the roadway bottom plate point cloud to the two-dimensional grid to generate an underground two-dimensional probability positioning map; and storing the position parameters of the point clouds of the waist line of the roadway, the position parameters of the point clouds of the bottom plate of the roadway and the reflection intensity parameters by each two-dimensional grid of the map.
Illustratively, the underground mining equipment constructs an underground mine three-dimensional point cloud map by using a SLAM method based on a 16-line laser radar and an IMU (inertial measurement Unit), and simultaneously divides the three-dimensional point cloud into two-dimensional roadway waist line point cloud scanned by a laser radar central line and three-dimensional roadway bottom plate point cloud extracted by using RANSAC. And dividing a two-dimensional grid of the XY plane in the three-dimensional point cloud map range, as shown in FIG. 4, respectively projecting the divided point cloud of the lane waist line and the point cloud of the lane floor to the corresponding two-dimensional grid, and generating an underground two-dimensional probability positioning map. The mean value and the covariance matrix of x and y coordinates of the point cloud of the lane waist line, the mean value and the variance corresponding to the height of the point cloud of the lane bottom plate and the mean value and the variance corresponding to the reflection intensity are respectively stored in each two-dimensional grid. As a plurality of points exist in one grid, the mean value and the covariance matrix of x and y coordinates of the point cloud of the lane waist line are stored in each grid of the underground two-dimensional probability positioning map, and the covariance matrix can illustrate the correlation of x and y. And the grids store the mean value and the variance corresponding to the height and the reflection intensity of the point cloud of the roadway floor, so that the subsequent histogram filtering is facilitated.
In some embodiments, the determining an initial pose and a probability thereof by performing first point cloud matching according to the roadway waist line point cloud and an underground two-dimensional probability positioning map, and determining a pose search range for second point cloud matching based on the probability of the initial pose comprises:
performing matching calculation of first point cloud matching on the roadway waist line point cloud and the underground two-dimensional probability positioning map based on an iterative algorithm of Normal Distribution Transformation (NDT);
acquiring an output result of the matching calculation meeting a preset termination condition, and determining the initial pose and the probability of the initial pose based on the output result;
and selecting the pose with the confidence interval meeting a set threshold value as the pose search range based on the probability of the initial pose.
Referring to fig. 5, an iterative algorithm based on normal distribution transformation performs first point cloud matching on the point cloud of the lane waistline, and an initial pose may be determined based on the first point cloud matching. Here, the preset termination condition corresponding to the matching calculation may be a threshold value of the number of iterations or a threshold value of the iteration result. If the preset termination condition is met, the initial pose and the probability of the initial pose can be determined according to the output result. Optionally, a pose with a confidence interval of 95% is selected as a pose search range of the point cloud registration (i.e., second point cloud matching) of the roadway floor. The pose search range can be expressed as a search range of x, y and theta, wherein x and y represent abscissa and ordinate in the map, and theta represents a heading angle.
In some embodiments, optionally, the underground working equipment locating method further comprises:
and when the output result of the matching calculation is not acquired, determining a pose search range based on the predicted pose corresponding to the sensor on the operation equipment. The current corresponding predicted pose can be obtained based on the IMU, and the IMU can determine the predicted pose from the two frames of data and use the predicted pose as the initial pose, so that the pose search range of the embodiment is determined.
103, according to the roadway floor point cloud, the underground two-dimensional probability positioning map and the initial pose, performing second point cloud matching in the pose searching range based on a histogram filtering method, and determining the current pose of the operation equipment according to a registration pose meeting set conditions.
After the roadway floor point cloud is extracted through RANSAC based on the current three-dimensional point cloud information, the roadway floor point cloud can be projected to an underground two-dimensional probability positioning map by combining with a prediction pose provided by an IMU, and the mean value and the variance corresponding to the height (namely Z coordinate) in each grid and the mean value and the variance corresponding to the reflection intensity are calculated so as to be used for subsequent histogram filtering matching.
In some embodiments, performing the second point cloud matching within the pose search range based on a histogram filtering method according to the tunnel floor point cloud, the underground two-dimensional probabilistic positioning map, and the initial pose, and determining the current pose of the working equipment according to a registration pose meeting set conditions includes:
according to the tunnel floor point cloud, respectively calculating histogram filtering of the height and the reflection intensity of the point cloud according to the initial pose and the underground two-dimensional probability positioning map to obtain matching probabilities respectively corresponding to the height and the reflection intensity, and determining the current corresponding probability based on the matching probabilities respectively corresponding to the height and the reflection intensity;
traversing the pose searching range for the roadway floor point cloud, performing histogram filtering calculation to obtain probability distribution in the pose searching range, and selecting a point with the maximum probability as the registration pose;
and determining the current pose of the operating equipment according to the matching pose.
Referring to fig. 6, in some embodiments, the method includes:
respectively calculating histogram filtering of the height and the intensity of the point cloud according to the point cloud of the roadway floor, the underground two-dimensional probability positioning map and the initial pose to obtain matching probabilities corresponding to the height and the intensity respectively, and calculating the pose probability after fusing the height and the intensity according to the similarity (SSD) of the sample; fusing the fused pose probability of the height and the intensity of the point cloud with the probability distribution predicted by the IMU to obtain the probability corresponding to the current pose at the moment; traversing all pose searching ranges according to the pose searching range, performing histogram filtering calculation to obtain probability distribution under all traversed poses, wherein the probability distribution comprises the probability corresponding to the current pose, taking the point with the maximum probability in the probability distribution as a point cloud registration pose, and finally obtaining the pose of the mining equipment at the moment through coordinate transformation.
According to the positioning method for the underground operation equipment, the characteristics of an underground roadway are fully utilized to carry out efficient matching and positioning of the three-dimensional laser point cloud, through two times of point cloud matching, the defect that two-dimensional laser point cloud matching does not fully utilize point cloud information is avoided, meanwhile, the problem that the calculation amount of the three-dimensional laser point cloud matching is large is also avoided, an effective implementation method is provided for positioning the underground operation equipment, the positioning accuracy and the response speed of the positioning of the underground operation equipment are effectively improved, and the requirement for real-time performance of control is favorably met.
An embodiment of the present application further provides an underground working equipment positioning apparatus, please refer to fig. 7, the underground working equipment positioning apparatus includes:
an obtaining module 701, configured to obtain a roadway waistline point cloud and a roadway floor point cloud currently corresponding to the operation device;
a first matching determination module 702, configured to perform first point cloud matching according to the lane waistline point cloud and an underground two-dimensional probability positioning map to determine an initial pose and a probability thereof, and determine a pose search range for second point cloud matching based on the probability of the initial pose;
and the second matching determination module 703 is configured to perform second point cloud matching in the pose search range based on a histogram filtering method according to the roadway floor point cloud, the underground two-dimensional probability positioning map, and the initial pose, and determine the current pose of the operation device according to a registration pose meeting a set condition.
In some embodiments, the underground working equipment positioning device further comprises a mapping module (not shown in the figure) for constructing an underground two-dimensional probabilistic positioning map based on the instant positioning and mapping SLAM method.
The map building module is specifically configured to:
acquiring three-dimensional point cloud information generated on the operation equipment based on a sensor and an SLAM method;
extracting corresponding tunnel waist line point cloud and tunnel bottom plate point cloud based on the three-dimensional point cloud information;
determining a two-dimensional grid based on the three-dimensional point cloud information, and respectively projecting the roadway waist line point cloud and the roadway bottom plate point cloud to the two-dimensional grid to generate an underground two-dimensional probability positioning map; and storing the position parameters of the point clouds of the waist line of the roadway, the position parameters of the point clouds of the bottom plate of the roadway and the reflection intensity parameters by each two-dimensional grid of the map.
In some embodiments, the first match determination module 702 is specifically configured to:
performing matching calculation of first point cloud matching on the roadway waist line point cloud and the underground two-dimensional probability positioning map based on an iterative algorithm of Normal Distribution Transformation (NDT);
acquiring an output result of the matching calculation meeting a preset termination condition, and determining the initial pose and the probability of the initial pose based on the output result;
and selecting the pose with the confidence interval meeting a set threshold value as the pose search range based on the probability of the initial pose.
In some embodiments, the first match determination module 702 is further configured to:
and when the output result of the matching calculation is not acquired, determining a pose search range based on the predicted pose corresponding to the sensor on the operation equipment. The current corresponding predicted pose can be obtained based on the IMU, and the IMU can determine the predicted pose from the two frames of data and use the predicted pose as the initial pose, so that the pose search range of the embodiment is determined.
In some embodiments, the second match determining module 703 is specifically configured to:
according to the tunnel floor point cloud, respectively calculating histogram filtering of the height and the reflection intensity of the point cloud according to the initial pose and the underground two-dimensional probability positioning map to obtain matching probabilities respectively corresponding to the height and the reflection intensity, and determining the current corresponding probability based on the matching probabilities respectively corresponding to the height and the reflection intensity;
traversing the pose searching range for the roadway floor point cloud, performing histogram filtering calculation to obtain probability distribution in the pose searching range, and selecting a point corresponding to the maximum probability as the registration pose;
and determining the current pose of the operating equipment according to the matching pose.
It should be noted that: in the positioning device for underground operation equipment provided in the above embodiment, only the division of the above program modules is used for illustration when positioning the underground operation equipment, and in practical applications, the above processing allocation may be completed by different program modules as needed, that is, the internal structure of the positioning device for underground operation equipment is divided into different program modules to complete all or part of the above-described processing. In addition, the positioning device for the underground operation equipment and the positioning method for the underground operation equipment provided by the embodiment belong to the same concept, and the specific implementation process is described in the method embodiment and is not described again.
In practical applications, each of the program modules may be implemented by a Central Processing Unit (CPU), a microprocessor Unit (MPU), a Digital Signal Processor (DSP), a Field Programmable Gate Array (FPGA), or the like.
Referring to fig. 8, an embodiment of the present application further provides a positioning system 800 for an underground working device, where the system 800 includes: at least one processor 801, memory 802, and at least one network interface 803. The various components in system 800 are coupled together by a bus system 804. It will be appreciated that the bus system 804 is used to enable communications among the components. The bus system 804 includes a power bus, a control bus, and a status signal bus in addition to a data bus. For clarity of illustration, however, the various buses are labeled as bus system 804 in FIG. 8.
It will be appreciated that the memory 802 can be either volatile memory or nonvolatile memory, and can include both volatile and nonvolatile memory.
The method for locating underground working equipment disclosed by the embodiment of the application can be applied to the processor 801 or can be implemented by the processor 801. The processor 801 may be an integrated circuit chip having signal processing capabilities. In implementation, the steps of the subterranean operation device locating method can be accomplished by instructions in the form of hardware, integrated logic circuits, or software in the processor 801. The Processor 801 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 801 may implement or perform the methods, steps, and logic blocks disclosed in the embodiments of the present application. The general purpose processor may be a microprocessor or any conventional processor or the like. The steps of the method disclosed in the embodiments of the present application may be directly implemented by a hardware decoding processor, or implemented by a combination of hardware and software modules in the decoding processor. The software modules may be located in a storage medium that is located in memory 802, and processor 801 reads information in memory 802 and completes the steps of the method for locating an underground working apparatus provided by the embodiments of the present application in conjunction with its hardware.
An embodiment of the present application further provides a readable storage medium, where the storage medium may include: various media that can store program codes, such as a removable Memory device, a Random Access Memory (RAM), a Read-Only Memory (ROM), a magnetic disk, and an optical disk. The readable storage medium stores an executable program; the executable program is used for realizing the underground operation equipment positioning method of any embodiment of the application when being executed by a processor.
As will be appreciated by one skilled in the art, embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, embodiments of the present application may take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Furthermore, embodiments of the present application 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 application are described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. 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 application, but the scope of the present application 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 application, and shall be covered by the scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.
Claims (8)
1. A method of locating subterranean operation equipment, comprising:
acquiring a roadway waist line point cloud and a roadway floor point cloud which correspond to the operation equipment at present, wherein the roadway waist line point cloud comprises point clouds corresponding to two sides of a roadway scanned by a three-dimensional laser radar, and the roadway floor point cloud comprises point clouds corresponding to a floor of the roadway scanned by the three-dimensional laser radar;
carrying out first point cloud matching according to the roadway waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof, and determining a pose search range for second point cloud matching based on the probability of the initial pose;
according to the roadway floor point cloud, the underground two-dimensional probability positioning map and the initial pose, performing second point cloud matching in the pose searching range based on a histogram filtering method, and determining the current pose of the operation equipment according to a registration pose meeting set conditions;
the method comprises the following steps of carrying out first point cloud matching according to the roadway waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof, and determining a pose search range for second point cloud matching based on the probability of the initial pose, wherein the method comprises the following steps:
performing first point cloud matching calculation on the roadway waist line point cloud and the underground two-dimensional probability positioning map by using an iterative algorithm based on normal distribution transformation;
acquiring an output result of the matching calculation meeting a preset termination condition, and determining the initial pose and the probability of the initial pose based on the output result;
selecting a pose with a confidence interval meeting a set threshold value as the pose searching range based on the probability of the initial pose;
the second point cloud matching is carried out in the position and posture searching range according to the roadway floor point cloud, the underground two-dimensional probability positioning map and the initial position and posture based on a histogram filtering method, and the current position and posture of the operation equipment are determined according to the registration position and posture meeting set conditions, and the method comprises the following steps:
according to the tunnel floor point cloud, respectively calculating histogram filtering of the height and the reflection intensity of the point cloud according to the initial pose and the underground two-dimensional probability positioning map to obtain matching probabilities respectively corresponding to the height and the reflection intensity, and determining the current corresponding probability based on the matching probabilities respectively corresponding to the height and the reflection intensity;
traversing the pose searching range for the roadway floor point cloud, performing histogram filtering calculation to obtain probability distribution in the pose searching range, and selecting a point corresponding to the maximum probability as the registration pose;
determining a current pose of the work equipment according to the registration pose.
2. The method for locating underground working equipment according to claim 1, wherein before the obtaining of the point cloud of the lane waist line and the point cloud of the lane floor corresponding to the current working equipment, the method further comprises:
and constructing an underground two-dimensional probabilistic positioning map based on the instant positioning and map construction SLAM method.
3. The underground working equipment positioning method according to claim 2, wherein the constructing of the underground two-dimensional probabilistic positioning map based on the SLAM method comprises:
acquiring three-dimensional point cloud information generated on the operation equipment based on a sensor and an SLAM method;
extracting corresponding tunnel waist line point clouds and tunnel bottom plate point clouds based on the three-dimensional point cloud information;
determining a two-dimensional grid based on the three-dimensional point cloud information, and respectively projecting the roadway waist line point cloud and the roadway bottom plate point cloud to the two-dimensional grid to generate an underground two-dimensional probability positioning map; and storing the position parameters of the point clouds of the waist line of the roadway, the position parameters of the point clouds of the bottom plate of the roadway and the reflection intensity parameters by each two-dimensional grid of the map.
4. The method for locating underground working equipment according to claim 1, wherein the obtaining of the roadway waist line point cloud and the roadway floor point cloud which correspond to the working equipment currently comprises:
acquiring current three-dimensional point cloud information acquired by a sensor on the operating equipment;
extracting the point cloud of the lane waist line based on the current three-dimensional point cloud information;
and extracting the point cloud of the roadway bottom plate based on the current three-dimensional point cloud information.
5. The method of locating underground working equipment according to claim 1, further comprising:
and when the output result of the matching calculation is not acquired, determining a pose search range based on the predicted pose corresponding to the sensor on the operation equipment.
6. An underground working equipment positioning device, comprising:
the acquisition module is used for acquiring a roadway waist line point cloud and a roadway bottom plate point cloud which are currently corresponding to the operation equipment, wherein the roadway waist line point cloud comprises point clouds corresponding to two sides of a roadway scanned by a three-dimensional laser radar, and the roadway bottom plate point cloud comprises point clouds corresponding to a bottom plate of the roadway scanned by the three-dimensional laser radar;
the first matching determination module is used for performing first point cloud matching according to the lane waist line point cloud and an underground two-dimensional probability positioning map to determine an initial pose and the probability thereof, and determining a pose search range for second point cloud matching based on the probability of the initial pose;
the second matching determination module is used for performing second point cloud matching in the pose searching range based on a histogram filtering method according to the roadway floor point cloud, the underground two-dimensional probability positioning map and the initial pose and determining the current pose of the operation equipment according to the registration pose meeting set conditions;
the first matching determination module is specifically configured to:
performing first point cloud matching calculation on the roadway waist line point cloud and the underground two-dimensional probability positioning map by using an iterative algorithm based on normal distribution transformation;
acquiring an output result of the matching calculation meeting a preset termination condition, and determining the initial pose and the probability of the initial pose based on the output result;
selecting a pose of which the confidence interval accords with a set threshold value as a pose searching range based on the probability of the initial pose;
the second match determination module is specifically configured to:
according to the tunnel floor point cloud, according to the initial pose and the underground two-dimensional probability positioning map, calculating histogram filtering of the height and the reflection intensity of the point cloud respectively to obtain matching probabilities corresponding to the height and the reflection intensity respectively, and determining the current corresponding probability based on the matching probabilities corresponding to the height and the reflection intensity respectively;
traversing the pose searching range for the roadway floor point cloud, performing histogram filtering calculation to obtain probability distribution in the pose searching range, and selecting a point with the maximum probability as the registration pose;
determining a current pose of the work equipment according to the registration pose.
7. An underground work equipment positioning system, comprising:
a memory for storing an executable program;
a processor for implementing the method of locating subterranean working equipment as claimed in any one of claims 1 to 5 when executing the executable program stored in the memory.
8. A computer storage medium having stored thereon an executable program which, when executed by a processor, implements the method of locating underground working equipment according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910179674.5A CN109839112B (en) | 2019-03-11 | 2019-03-11 | Underground operation equipment positioning method, device and system and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910179674.5A CN109839112B (en) | 2019-03-11 | 2019-03-11 | Underground operation equipment positioning method, device and system and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109839112A CN109839112A (en) | 2019-06-04 |
| CN109839112B true CN109839112B (en) | 2023-04-07 |
Family
ID=66885525
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910179674.5A Active CN109839112B (en) | 2019-03-11 | 2019-03-11 | Underground operation equipment positioning method, device and system and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109839112B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110849374B (en) * | 2019-12-03 | 2023-04-18 | 中南大学 | Underground environment positioning method, device, equipment and storage medium |
| CN110967011B (en) * | 2019-12-25 | 2022-06-28 | 苏州智加科技有限公司 | Positioning method, device, equipment and storage medium |
| CN113096181B (en) * | 2020-01-08 | 2024-04-09 | 纳恩博（北京）科技有限公司 | Method and device for determining equipment pose, storage medium and electronic device |
| CN112433211B (en) * | 2020-11-27 | 2022-11-29 | 浙江商汤科技开发有限公司 | Pose determination method and device, electronic equipment and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225345A (en) * | 2016-12-22 | 2018-06-29 | 乐视汽车（北京）有限公司 | The pose of movable equipment determines method, environmental modeling method and device |
| CN108230379A (en) * | 2017-12-29 | 2018-06-29 | 百度在线网络技术（北京）有限公司 | For merging the method and apparatus of point cloud data |
| CN109059942A (en) * | 2018-08-22 | 2018-12-21 | 中国矿业大学 | A kind of high-precision underground navigation map building system and construction method |
| CN109186625A (en) * | 2018-10-24 | 2019-01-11 | 北京奥特贝睿科技有限公司 | Intelligent vehicle carries out pinpoint method and system using mixing sampling filter |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
-
2019
- 2019-03-11 CN CN201910179674.5A patent/CN109839112B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225345A (en) * | 2016-12-22 | 2018-06-29 | 乐视汽车（北京）有限公司 | The pose of movable equipment determines method, environmental modeling method and device |
| CN108230379A (en) * | 2017-12-29 | 2018-06-29 | 百度在线网络技术（北京）有限公司 | For merging the method and apparatus of point cloud data |
| CN109059942A (en) * | 2018-08-22 | 2018-12-21 | 中国矿业大学 | A kind of high-precision underground navigation map building system and construction method |
| CN109186625A (en) * | 2018-10-24 | 2019-01-11 | 北京奥特贝睿科技有限公司 | Intelligent vehicle carries out pinpoint method and system using mixing sampling filter |
Non-Patent Citations (1)
| Title |
|---|
| 基于视点特征直方图的激光点云模型的位姿估计;张彪等;《光电子.激光》;20130715(第07期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109839112A (en) | 2019-06-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109813310B (en) | Underground operation equipment positioning method, device and system and storage medium | |
| CN109839112B (en) | Underground operation equipment positioning method, device and system and storage medium | |
| EP3505869B1 (en) | Method, apparatus, and computer readable storage medium for updating electronic map | |
| CN110849374B (en) | Underground environment positioning method, device, equipment and storage medium | |
| CN108319655B (en) | Method and device for generating grid map | |
| CN109540142B (en) | Robot positioning navigation method and device, and computing equipment | |
| US20200110173A1 (en) | Obstacle detection method and device | |
| KR102414307B1 (en) | 3D map change area update system and method | |
| CN110244716A (en) | A method of the robot based on wave front algorithm is explored | |
| Farag | Bayesian localization in real-time using probabilistic maps and unscented-Kalman-filters | |
| CN112904901B (en) | Path planning method based on binocular vision slam and fusion algorithm | |
| CN113932796A (en) | High-precision map lane line generation method and device and electronic equipment | |
| Farag | Self-driving vehicle localization using probabilistic maps and Unscented-Kalman filters | |
| CN116520302A (en) | Positioning method applied to automatic driving system and method for constructing three-dimensional map | |
| CN113177980B (en) | Target object speed determining method and device for automatic driving and electronic equipment | |
| CN115619954A (en) | Sparse semantic map construction method, device, equipment and storage medium | |
| Gao et al. | Design of mobile robot based on cartographer SLAM algorithm | |
| CN112802095B (en) | Positioning method, device and equipment, and automatic driving positioning system | |
| KR102450139B1 (en) | GLOBAL LOCALIZTION APPARATUS AND METHOD IN DYNAMIC ENVIRONMENTS USING 3D LiDAR SCANNER | |
| CN114740867A (en) | Intelligent obstacle avoidance method and device based on binocular vision, robot and medium | |
| CN113587937A (en) | Vehicle positioning method and device, electronic equipment and storage medium | |
| US20230354258A1 (en) | Data processing method and apparatus | |
| CN114281915B (en) | Method, device and equipment for generating geometric road network and storage medium | |
| US20220292290A1 (en) | Distributed computing network to perform simultaneous localization and mapping | |
| Wang et al. | Robust laser location method for IGV in airport apron scene |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |