CN110849374B - Underground environment positioning method, device, equipment and storage medium - Google Patents
Underground environment positioning method, device, equipment and storage medium Download PDFInfo
- Publication number
- CN110849374B CN110849374B CN201911217135.2A CN201911217135A CN110849374B CN 110849374 B CN110849374 B CN 110849374B CN 201911217135 A CN201911217135 A CN 201911217135A CN 110849374 B CN110849374 B CN 110849374B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- map
- pose
- frame
- current
- 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
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
Abstract
The invention discloses a method, a device, equipment and a storage medium for positioning an underground environment. Wherein, the method comprises the following steps: acquiring characteristic point clouds of scanning point cloud data; taking the pose of the previous frame as the current initial pose, performing point cloud frame-to-frame matching based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame, and determining the current prediction pose; and matching the point cloud frame with a map to obtain the current correction pose based on the feature point cloud of the scanning point cloud data of the current frame, the current prediction pose and the feature map. The embodiment of the invention realizes the accurate positioning of the underground environment based on the distance weight, can improve the positioning precision in the underground environment, and meets the unmanned requirement of autonomous walking of underground equipment.
Description
Technical Field
The present invention relates to the field of positioning technologies, and in particular, to a method, an apparatus, a device, and a storage medium for positioning an underground environment.
Background
In recent years, ground unmanned projects develop rapidly, and play an important role in improving the safety and efficiency of the whole traffic industry. The special environment of the underground closed limited space is a preferred scene for realizing the rapid landing of the unmanned vehicle, the safety of operation can be greatly improved, the underground operation efficiency is improved, and further greater economic benefits are created for enterprises, wherein the accurate positioning in the underground environment is a key.
Although positioning based on GNSS (global positioning satellite system) can realize centimeter-level positioning accuracy in an open environment, underground special environments such as underground mines, subways, pipe galleries, fire fighting, civil air defense and the like cannot receive GPS (global positioning system) signals, so that difficulty in underground positioning is increased. Aiming at the positioning problem of underground environment, low-frequency electromagnetic guidance, ultrasonic sensing measurement guidance, visual beacon navigation-based underground positioning and continuous tracking and the like are applied in the early 90 s of the 20 th century. In addition, positioning technologies based on Wi-Fi (Wireless-Fidelity), bluetooth positioning, radio Frequency Identification (RFID), ultra Wide Band (UWB), infrared technology, ultrasonic wave, and the like are also widely used. However, in the above positioning technologies, corresponding auxiliary positioning devices need to be installed in the underground environment, which can improve the positioning accuracy, but increase a large amount of equipment and maintenance cost, and meanwhile, because the underground space environment is rough, an accumulated error in the positioning process is easily caused, and particularly, in the case of rotation, a pose acquisition failure easily occurs, so that the safety of the underground equipment in the autonomous walking process is greatly reduced.
Disclosure of Invention
In view of this, embodiments of the present invention provide a method, an apparatus, a device, and a storage medium for positioning an underground environment, which aim to improve positioning accuracy in the underground environment and meet an unmanned requirement for autonomous walking of underground equipment.
The technical scheme of the embodiment of the invention is realized as follows:
the embodiment of the invention provides a method for positioning an underground environment, which comprises the following steps:
acquiring characteristic point clouds of scanning point cloud data;
taking the pose of the previous frame as the current initial pose, performing point cloud frame-to-frame matching based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame, and determining the current prediction pose;
matching the point cloud frame with a map to obtain a current correction pose based on the feature point cloud of the scanning point cloud data of the current frame, the current prediction pose and a feature map;
the characteristic map is generated based on a Real-time laser odometer and Mapping in Real-time, and scanning point cloud data is corrected based on the distance between the point cloud and scanning equipment in the construction process of the characteristic map.
The embodiment of the invention also provides an underground environment positioning device, which comprises:
the acquisition module is used for acquiring the characteristic point cloud of the scanning point cloud data;
the prediction module is used for performing point cloud frame-to-frame matching on the basis of the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame to determine the current prediction pose;
the pose determining module is used for matching the point cloud frame with the map to obtain a current correction pose based on the feature point cloud of the scanning point cloud data of the current frame, the current prediction pose and the feature map;
the feature map is generated based on LOAM, and in the construction process of the feature map, scanning point cloud data are corrected based on the distance between the point cloud and scanning equipment.
The embodiment of the invention also provides underground environment positioning equipment, which comprises: a processor and a memory for storing a computer program capable of running on the processor, wherein the processor, when running the computer program, is adapted to perform the steps of the method according to any of the embodiments of the present invention.
The embodiment of the invention also provides a storage medium, wherein a computer program is stored on the storage medium, and when the computer program is executed by a processor, the steps of the method of any embodiment of the invention are realized.
According to the technical scheme provided by the embodiment of the invention, point cloud frame-to-frame matching is carried out on the basis of the characteristic point cloud of the scanning point cloud data of the current frame and the characteristic point cloud of the scanning point cloud data of the previous frame, and the current prediction pose is determined; the method comprises the steps of matching a point cloud frame with a map based on a feature point cloud of scanning point cloud data of a current frame, the current prediction pose and a feature map to obtain a current correction pose, wherein the feature map is generated based on a LOAM, and in the construction process of the feature map, the scanning point cloud data is corrected based on the distance between the point cloud and scanning equipment, so that accurate positioning of an underground environment based on distance weight is realized, the positioning precision in the underground environment can be improved, and the autonomous walking unmanned driving requirement of underground equipment is met.
Drawings
FIG. 1 is a schematic flow chart of a method for locating a subsurface environment according to an embodiment of the invention;
FIG. 2 is a schematic diagram of a point cloud coordinate system transformation according to an embodiment of the present invention;
FIG. 3 is a schematic view of a subsurface environment locating frame, in accordance with one embodiment of the present invention;
FIG. 4 is a schematic structural diagram of a subsurface environment locating device in accordance with an embodiment of the present invention;
FIG. 5 is a schematic structural diagram of an underground environment locating apparatus according to an embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
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 related art, the appearance of three-dimensional laser radar (also called 3D laser radar) brings convenience to accurate positioning of underground environment, and high-precision map construction and positioning application can be realized by acquiring abundant 3D environment point cloud information. In the process of implementing the invention, the inventor finds that the farther the distance of the acquisition point of the three-dimensional laser radar is, the larger the measurement error is, and the closer the distance of the acquisition point is, the smaller the measurement error is. Meanwhile, in an underground environment, the ground is not very flat, and a little shake can increase the measurement error of a long-distance data point (namely point cloud) in the moving process of the three-dimensional laser radar. In the conventional SLAM (instantaneous positioning and mapping) system, grids are generally divided for a map in the mapping process, point values in the grids are continuously weighted and updated according to the continuous increase of real-time point cloud data to obtain the mean value and the variance of the grids, the measurement error is rarely considered, and although distortion removal processing is performed on motion distortion in the LOAM, the influence of the error of the measurement on the mapping is not considered.
Based on this, in various embodiments of the present invention, in the map construction process based on the LOAM, the scanned point cloud data is corrected based on the distance between the point cloud and the scanning device, so that accurate positioning of the underground environment based on distance weight is achieved, the positioning accuracy in the underground environment can be improved, and the unmanned driving requirement of autonomous walking of underground equipment is met.
An embodiment of the present invention provides a method for locating an underground environment, as shown in fig. 1, including:
102, taking the pose of the previous frame as the current initial pose, performing point cloud frame-to-frame matching based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame, and determining the current predicted pose;
and 103, matching the point cloud frame with a map to obtain a current correction pose based on the feature point cloud of the scanning point cloud data of the current frame, the current prediction pose and a feature map.
The feature map is generated based on a real-time Laser Odometer and A Mapping (LOAM), and scanning point cloud data is corrected based on the distance between the point cloud and scanning equipment in the construction process of the feature map.
In the embodiment of the invention, because the feature map is generated after the distance between the point cloud and the scanning equipment is corrected, the point cloud frame is matched with the map to obtain the current corrected pose based on the feature point cloud of the scanning point cloud data of the current frame, the current predicted pose and the feature map, the accurate positioning of the underground environment based on distance weight is realized, the positioning precision in the underground environment can be improved, and the unmanned driving requirement of the autonomous walking of underground equipment is met.
In an embodiment, the scanning device is a three-dimensional lidar. Here, the VLP-16 three-dimensional lidar is exemplified by a three-dimensional lidar manufactured by Velodyne corporation. Its vertical resolution is about 2 ° and its horizontal resolution is at most 0.4 °. A single line bundle is called a Scan, and a frame of point cloud composed of all 16 lines is called a Sweep. All point clouds in a frame are scanned in sequence and in series, and only one time of sending and one time of receiving are carried out at the same time point. Scanning the depth of all 16 points (corresponding to 16 SCANs) in the vertical direction at a horizontal first angle, generally about 0 degrees, of course, the 16 points are also in serial sequence, then turning to a next horizontal angle, for example, starting at 0.3 degrees and having a horizontal resolution of 0.4 degrees, then the next angle is 0.7 degrees and then 1.1 degrees, and completing the acquisition of Sweep data all the way clockwise, wherein the output form is (X, Y, Z, I) corresponding to each point cloud, wherein (X, Y, Z) is a three-dimensional coordinate, and I is a reflection intensity value.
Here, the underground environment positioning device obtains scanning point cloud data obtained by scanning of the three-dimensional laser radar, wherein one frame of scanning point cloud data is Sweep data acquired by scanning of the three-dimensional laser radar.
In the embodiment of the invention, after the underground environment positioning equipment acquires the scanning point cloud data, a characteristic map, a point cloud frame-to-frame matching and a point cloud frame-to-map matching positioning frame are constructed for the scanning point cloud data based on a lossless Kalman Filter (UKF), so that the accurate positioning of the underground environment is realized. The UKF is the combination of lossless transform (UT) and a standard Kalman (Kalman) filtering system, and the nonlinear system equation is suitable for the standard Kalman filtering system under the linear assumption through the lossless transform. The UKF is based on UT transformation, the traditional method of carrying out linearization on a nonlinear function is abandoned, a Kalman linear filtering frame is adopted, and for a one-step prediction equation, the UT is used for processing nonlinear transfer of a mean value and a covariance, so that the UKF algorithm is formed. The UKF approximates the probability density distribution of a nonlinear function by approximating the posterior probability density of the state with a series of deterministic samples, rather than approximating the nonlinear function, without the need for derivation to compute Jacobian matrices. The UKF ignores high-order terms without linearization, so the calculation precision of the nonlinear distribution statistic is higher.
Here, the underground environment positioning device performs noise removal on the acquired scanning point cloud data based on a clustering method, and extracts a feature point cloud of the scanning point cloud data. Specifically, feature extraction may be performed on the scanning point cloud data after the noise is removed by using a feature extraction method in the LOAM to obtain a feature point cloud of the scanning point cloud data, for example, extracting a point cloud corresponding to a corner map and a surface map as the feature point cloud. In other embodiments, the scanning device may perform feature extraction on the scanning point cloud data obtained by scanning to obtain a feature point cloud of the scanning point cloud data, and transmit the feature point cloud of the scanning point cloud data to the underground environment positioning device, so that the underground environment positioning device obtains the feature point cloud of the scanning point cloud data.
The underground environment positioning of the UKF-based positioning frame comprises the following steps: a pose predicting stage and a pose correcting stage. The pose prediction phase is the step 102, and the pose correction phase is the step 103.
Here, the pose of the previous frame may be used as the current initial pose, and the current predicted pose may be determined by performing point cloud frame-to-frame matching based on the feature point cloud of the scan point cloud data of the current frame and the feature point cloud of the scan point cloud data of the previous frame.
In practical application, the pose state vector of the three-dimensional laser radar is defined as follows:
wherein p is t Three-dimensional coordinates of the three-dimensional lidar at time t, q t Is the quaternion of the three-dimensional laser radar at the time t.
The subsurface environment locating device determines a predicted pose of the three-dimensional lidar by iteratively applying the extracted feature point cloud between successive frames. Specifically, point cloud frame-to-frame (Scan to Scan) matching can be performed based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame, so as to obtain the relative pose between the current frame and the previous frame; and obtaining the current predicted pose based on the relative pose and the pose of the previous frame.
Illustratively, the feature point cloud of the current frame acquired in real time can be matched with the feature point cloud of the previous adjacent frame to obtain the relative pose Δ x from the time t of the rescanning frame to the time t-1 of the nearest frame t-1,t ＝[Δp t ,Δq t ] T And simultaneously the pose of the three-dimensional laser radar at the time of t-1 in world coordinates is x t-1 And calculating the predicted pose of the three-dimensional laser radar at the time t as follows:
x t ＝x t-1 Δx t-1,t
in an embodiment, the performing point cloud frame and map matching based on the feature point cloud of the scanning point cloud data of the current frame, the current predicted pose, and the feature map to obtain the current corrected pose includes:
and matching the point cloud frame and the Map (Scan to Map) of the feature point cloud of the scanning point cloud data of the current frame with the feature Map by taking the current predicted pose as an initial value for matching the point cloud frame and the Map to obtain a registration pose, and taking the registration pose as a current correction pose. It should be noted that, in an embodiment, the current corrected pose may be used as the initial pose of the next frame. If the previous frame is the first frame, the initial position of the scanning device can be selected from the current initial pose.
In practical application, the predicted pose x is used t As an initial value of the point cloud frame and the map, obtaining the registration pose of
And the measured variable is used as an observation variable of the lossless Kalman filtering.
When the point cloud frame is matched with the map, the feature map is needed. Based on this, in an embodiment, the method further comprises:
carrying out map construction on the acquired scanning point cloud data based on the LOAM to obtain an initial feature map;
and giving a weight to the distance of the characteristic points to the initial characteristic map based on a distance power inverse ratio method to obtain a modified characteristic map.
Here, the mapping the acquired scanning point cloud data based on the LOAM to obtain an initial feature map includes:
matching the acquired cloud data of the scanning points based on inter-frame according to the first frequency to obtain a matching splicing pose between data of adjacent frames;
splicing the data of the adjacent frames based on the corresponding splicing pose according to the second frequency to obtain an initial angle corner map and an initial surface map;
wherein the first frequency is greater than the second frequency.
The LOAM can use a two-axis laser radar moving in a three-dimensional space to obtain low drift and low complexity, does not need a high-precision distance measurement and real-time laser odometer and a map of inertial measurement, and has the core idea that the positioning and mapping are divided through two algorithms: one is to execute high-frequency odometer but low-precision motion estimation (positioning), and obtain an ODOM (odometer) with poor precision by registering each frame of laser, because no IMU (inertial measurement unit) provides the initial POSE matched between frames, the LOAM is slightly modified, and a uniform motion model is used to calculate the splicing POSE (POSE) registered by frames; another algorithm performs matching and registering point cloud information (mapping and odometer correction) at a frequency of one order of magnitude lower, forming a feature point cloud map (i.e., the aforementioned feature map) by stitching multiple frames of laser feature point clouds based on stitching poses. Specifically, the method may include: a corner map and a surface map. Thus, two characteristic point cloud maps are established. In addition, the newly-entered frame and the characteristic point cloud map can be registered to obtain a more accurate splicing pose, and the more accurate splicing pose is spliced to obtain an updated characteristic point cloud map; the two algorithms are combined to obtain the laser odometer with high precision and real-time performance.
In the embodiment of the present invention, it is further required to assign a weight to the distance of the feature point based on the inverse distance power ratio method to obtain a modified feature map, including:
converting the initial corner map and the three-dimensional point cloud of the initial corner map into a world coordinate system and dividing the world coordinate system into three-dimensional grids;
counting the number of point clouds in each grid according to each grid, carrying out weight assignment on each point cloud based on the distance between each point cloud and a set position, and determining a coordinate corresponding to each grid;
and updating the initial corner map and the corner map according to the coordinates corresponding to the grids to obtain the corrected feature map.
In practical application, the method for obtaining the corrected feature map by giving a weight to the distance of the feature point on the basis of a distance inverse power ratio method to the feature point cloud map comprises the following steps:
step 1, converting a three-dimensional point cloud coordinate under a laser radar coordinate system into a world coordinate system according to the obtained pose information of the three-dimensional laser radar, as shown in fig. 2. Let the coordinate of the three-dimensional point under the three-dimensional laser radar coordinate system be p L ＝(x L ,y L ,z L ) Rotation matrix T for three-dimensional laser radar pose information WL Representing, three-dimensional point cloud coordinates p in world coordinate system W ＝(x W ,y W ,z W ) The following formula is satisfied:
p W ＝T WL ·p L
under a three-dimensional laser radar coordinate system, calculating the distance from any point in the three-dimensional point cloud to the three-dimensional laser radar coordinate origin
And 2, dividing the real-time conversion point cloud and the local map point cloud under the world coordinate into three-dimensional grids. When the real-time converted point cloud is first frame data, initializing a point cloud map, and only dividing the first frame converted point cloud into three-dimensional grids. In the subsequent three-dimensional laser frame, the conversion point cloud and the local map are divided into three-dimensional grids.
Counting the number n of point clouds in each grid, and measuring the measuring error of the points with long distance according to the measuring characteristics of the laser radarThe difference is relatively large, and each point is endowed with a corresponding weight according to the calculation method of the distance S from the point to the origin of the laser radar coordinate system
The value of p in this test is 2.
Calculating the final point coordinate p at the moment in each grid according to the converted three-dimensional point coordinates and the corresponding weights thereof WC ＝(x WC ,y WC ,z WC ) It satisfies the following formula:
according to the final point coordinate p WC ＝(x WC ,y WC ,z WC ) Calculating the coordinate p of the three-dimensional laser radar coordinate system LC ＝(x LC ,y LC ,z LC ) It satisfies the following formula:
and p is calculated by the following formula LC The distance from the three-dimensional laser radar coordinate origin at the moment:
and 3, continuously iterating the two steps according to the obtained pose and the three-dimensional point cloud information at each moment, and gradually updating the point cloud map to obtain a complete three-dimensional point cloud map (namely a corrected feature map) with small influence of point cloud noise.
In one embodiment, as shown in fig. 3, the VLP-16 of Velodyne is adopted as the three-dimensional lidar, and the underground environment precise positioning framework is mainly divided into off-line distance weight point cloud map construction based on the 3D lidar and real-time precise positioning based on the distance weight point cloud map. Aiming at off-line map construction, firstly, a Cornern map and a Surface map are generated based on a LOAM, meanwhile, in the process of generating the map, according to the data acquisition characteristics of a 3D laser radar, the offset distance error of an obstacle return value which is far away and is closer to an obstacle return value is large, different weight values are given according to the distance of each point by adopting a distance power inverse ratio method, and then the generated feature map is corrected to obtain the feature map based on the LOAM and the distance power inverse ratio method. Aiming at the real-time accurate positioning based on the distance weight map, in order to ensure the subsequent processing efficiency and the positioning accuracy, firstly, noise removal is carried out based on a clustering method, and meanwhile, a feature extraction method in the LOAM is adopted to carry out feature extraction on the real-time point cloud so as to obtain a feature point cloud; secondly, real-time positioning of the underground environment is achieved based on lossless Kalman filtering. In the stage of predicting the pose, the pose is estimated mainly through point cloud inter-frame matching to obtain the predicted pose; in the stage of pose correction, point cloud frames and maps are carried out through the real-time point cloud and the constructed distance weight map, the current correction pose is obtained, the real-time accurate pose is finally output, and meanwhile, the correction pose is also used for the initial pose of the next cycle.
In practical application, the underground environment positioning method provided by the embodiment of the invention is applied to testing in multiple underground scenes, including a smooth-surface straight tunnel, a smooth-surface loop tunnel, a rough non-straight tunnel and a rough slope tunnel, the algorithm is proved to realize accurate positioning of the underground environment and the robustness of the algorithm through positioning error and positioning time analysis, comparative analysis is carried out on the aspects of positioning error, positioning time and the like, the positioning error can be controlled to be about 4cm, the positioning time is averagely controlled to be about 60ms, and the result shows that the algorithm can realize low-drift real-time positioning.
In order to implement the method according to the embodiment of the present invention, an embodiment of the present invention further provides an underground environment positioning apparatus, as shown in fig. 4, the apparatus includes: an acquisition module 401, a prediction module 402, and a pose determination module 403. Wherein, the first and the second end of the pipe are connected with each other,
an obtaining module 401, configured to obtain a feature point cloud of the scanning point cloud data;
the prediction module 402 is used for performing point cloud frame-to-frame matching based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame to determine the current prediction pose;
a pose determining module 403, configured to perform point cloud frame and map matching based on a feature point cloud of scan point cloud data of a current frame, the current predicted pose, and a feature map, to obtain a current corrected pose;
the feature map is generated based on LOAM, and in the construction process of the feature map, scanning point cloud data are corrected based on the distance between the point cloud and scanning equipment.
In one embodiment, the subsurface environment locating device further comprises: a map building module 404 configured to:
carrying out map construction on the acquired scanning point cloud data based on the LOAM to obtain an initial feature map;
and giving a weight to the distance of the characteristic points to the initial characteristic map based on a distance power inverse ratio method to obtain a modified characteristic map.
In an embodiment, the map building module 404 is specifically configured to:
matching the acquired cloud data of the scanning points based on inter-frame according to the first frequency to obtain a matching splicing pose between data of adjacent frames;
splicing the data of the adjacent frames based on the corresponding splicing pose according to the second frequency to obtain an initial angle corner map and an initial surface map;
wherein the first frequency is greater than the second frequency.
In an embodiment, the map building module 404 is specifically configured to:
converting the initial corner map and the three-dimensional point cloud of the initial corner map into a world coordinate system and dividing the world coordinate system into three-dimensional grids;
counting the number of point clouds in each grid aiming at each grid, carrying out weight assignment on each point cloud based on the distance between each point cloud and a set position, and determining a coordinate corresponding to each grid;
and updating the initial corner map and the corner map according to the coordinates corresponding to the grids to obtain the corrected feature map.
In one embodiment, the prediction module 402 is specifically configured to:
performing point cloud frame-to-frame matching based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame to obtain the relative pose between the current frame and the previous frame;
and obtaining the current predicted pose based on the relative pose and the pose of the previous frame.
In an embodiment, the pose determination module 403 is specifically configured to:
and taking the current predicted pose as an initial value for matching the point cloud frame with the map, performing point cloud frame and map matching on the feature point cloud of the scanning point cloud data of the current frame and the feature map to obtain a registration pose, and taking the registration pose as a current correction pose.
In practical application, the obtaining module 401, the predicting module 402, the pose determining module 403, and the map constructing module 404 may be implemented by a processor in the underground environment positioning apparatus. Of course, the processor needs to run a computer program in memory to implement its functions.
It should be noted that: in the above embodiment, when the device for locating an underground environment is used for locating an underground environment, only the division of the program modules is taken as an example, and in practical applications, the above processing distribution may be completed by different program modules according to needs, that is, the internal structure of the device is divided into different program modules to complete all or part of the above-described processing. In addition, the underground environment positioning device provided by the above embodiment and the underground environment positioning method embodiment belong to the same concept, and the specific implementation process thereof is detailed in the method embodiment and is not described herein again.
Based on the hardware implementation of the program module, and in order to implement the method according to the embodiment of the present invention, an embodiment of the present invention further provides an underground environment positioning apparatus. Fig. 5 shows only an exemplary structure of the apparatus and not the entire structure, and a part of or the entire structure shown in fig. 5 may be implemented as necessary.
As shown in fig. 5, an underground environment locating apparatus 500 provided by an embodiment of the present invention includes: at least one processor 501, memory 502, a user interface 503, and at least one network interface 504. The various components in the subsurface environment locating apparatus 500 are coupled together by a bus system 505. It will be appreciated that the bus system 505 is used to enable communications among the components of the connection. The bus system 505 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 505 in FIG. 5.
The user interface 503 may include a display, a keyboard, a mouse, a trackball, a click wheel, a key, a button, a touch pad, a touch screen, or the like, among others.
The method for locating the underground environment disclosed by the embodiment of the invention can be applied to the processor 501, or can be implemented by the processor 501. The processor 501 may be an integrated circuit chip having signal processing capabilities. In implementation, the steps of the subsurface environment locating method may be performed by instructions in the form of hardware, integrated logic circuits, or software in the processor 501. The Processor 501 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. Processor 501 may implement or perform the methods, steps, and logic blocks disclosed in 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 the memory 502, and the processor 501 reads the information in the memory 502 to complete the steps of the underground environment positioning method provided by the embodiments of the present invention in combination with the hardware thereof.
In an exemplary embodiment, the subsurface environment locating Device may be implemented by one or more Application Specific Integrated Circuits (ASICs), DSPs, programmable Logic Devices (PLDs), complex Programmable Logic Devices (CPLDs), FPGAs, general purpose processors, controllers, micro Controllers (MCUs), microprocessors (microprocessors), or other electronic components for performing the aforementioned methods.
In an application example, the underground environment positioning device includes a 16-line 3D lidar, a mobile power source, and a notebook computer, where the mobile power source supplies power to the 16-line 3D lidar and the notebook computer, the notebook computer is in communication connection with the 16-line 3D lidar, and when the notebook computer runs a computer program, the underground environment positioning method according to any of the foregoing embodiments is implemented.
It will be appreciated that the memory 502 can be either volatile memory or nonvolatile memory, and can include both volatile and nonvolatile memory. Among them, the nonvolatile Memory may be a Read Only Memory (ROM), a Programmable Read Only Memory (PROM), an Erasable Programmable Read-Only Memory (EPROM), an Electrically Erasable Programmable Read-Only Memory (EEPROM), a magnetic random access Memory (FRAM), a magnetic random access Memory (Flash Memory), a magnetic surface Memory, an optical Disc, or a Compact Disc Read-Only Memory (CD-ROM); the magnetic surface storage may be disk storage or tape storage. Volatile Memory can be Random Access Memory (RAM), which acts as external cache Memory. By way of illustration and not limitation, many forms of RAM are available, such as Static Random Access Memory (SRAM), synchronous Static Random Access Memory (SSRAM), dynamic Random Access Memory (DRAM), synchronous Dynamic Random Access Memory (SDRAM), double Data Rate Synchronous Dynamic Random Access Memory (DDRSDRAM), enhanced Synchronous Dynamic Random Access Memory (ESDRAM), enhanced Synchronous Dynamic Random Access Memory (Enhanced DRAM), synchronous Dynamic Random Access Memory (SLDRAM), direct Memory (DRmb Access), and Random Access Memory (DRAM). The described memory for embodiments of the present invention is intended to comprise, without being limited to, these and any other suitable types of memory.
In an exemplary embodiment, the present invention further provides a storage medium, which is a computer storage medium, and may be a computer readable storage medium, for example, a memory 502 storing a computer program, which is executable by a processor 501 of a subsurface environment positioning device to perform the steps described in the method of the present invention. The computer readable storage medium may be a ROM, PROM, EPROM, EEPROM, flash Memory, magnetic surface Memory, optical disk, or CD-ROM, among others.
It should be noted that: "first," "second," and the like are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order.
In addition, the technical solutions described in the embodiments of the present invention may be arbitrarily combined without conflict.
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 claims.
Claims (6)
1. A method of locating an underground environment, comprising:
acquiring a characteristic point cloud of scanning point cloud data;
taking the pose of the previous frame as the current initial pose, performing point cloud frame-to-frame matching based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame, and determining the current prediction pose;
matching the point cloud frame with a map to obtain a current correction pose based on the feature point cloud of the scanning point cloud data of the current frame, the current prediction pose and a feature map;
the characteristic map is generated based on a real-time laser odometer and a map building LOAM, and scanning point cloud data are corrected based on the distance between point cloud and scanning equipment in the construction process of the characteristic map;
the method further comprises the following steps:
carrying out map construction on the acquired scanning point cloud data based on the LOAM to obtain an initial feature map;
giving a weight to the distance of the characteristic points to the initial characteristic map based on a distance power inverse ratio method to obtain a modified characteristic map;
the map construction of the acquired scanning point cloud data based on the LOAM to obtain an initial feature map comprises the following steps:
matching the acquired cloud data of the scanning points based on inter-frame according to the first frequency to obtain a matching splicing pose between data of adjacent frames;
splicing the data of the adjacent frames based on the corresponding splicing poses according to the second frequency to obtain an initial corner map and an initial surface map;
wherein the first frequency is greater than the second frequency;
the method for giving weight to the distance of the characteristic points to the initial characteristic map based on the inverse ratio of distance power to obtain the corrected characteristic map comprises the following steps:
converting the initial corner map and the three-dimensional point cloud of the initial corner map into a world coordinate system and dividing the world coordinate system into three-dimensional grids;
counting the number of point clouds in each grid according to each grid, carrying out weight assignment on each point cloud based on the distance between each point cloud and a set position, and determining a coordinate corresponding to each grid;
and updating the initial corner map and the corner map according to the coordinates corresponding to the grids to obtain the corrected feature map.
2. The method of claim 1, wherein performing point cloud frame-to-frame matching based on the feature point cloud of the scan point cloud data of the current frame and the feature point cloud of the scan point cloud data of the previous frame to determine the current predicted pose comprises:
performing point cloud frame-to-frame matching based on the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame to obtain the relative pose between the current frame and the previous frame;
and obtaining a current predicted pose based on the relative pose and the current initial pose.
3. The method of claim 1, wherein the performing point cloud frame-to-map matching based on the feature point cloud of the scan point cloud data of the current frame, the current predicted pose, and a feature map to obtain a current corrected pose comprises:
and taking the current predicted pose as an initial value for matching the point cloud frame with the map, performing point cloud frame and map matching on the feature point cloud of the scanning point cloud data of the current frame and the feature map to obtain a registration pose, and taking the registration pose as a current correction pose.
4. An underground environment locating device, comprising:
the acquisition module is used for acquiring the characteristic point cloud of the scanning point cloud data;
the prediction module is used for taking the pose of the previous frame as the current initial pose, performing point cloud frame-to-frame matching on the basis of the feature point cloud of the scanning point cloud data of the current frame and the feature point cloud of the scanning point cloud data of the previous frame, and determining the current prediction pose;
the pose determining module is used for matching the point cloud frame with the map to obtain a current correction pose based on the feature point cloud of the scanning point cloud data of the current frame, the current prediction pose and the feature map;
the characteristic map is generated based on LOAM, and in the construction process of the characteristic map, scanning point cloud data are corrected based on the distance between the point cloud and scanning equipment;
a map construction module to: carrying out map construction on the acquired scanning point cloud data based on the LOAM to obtain an initial feature map; giving a weight to the distance of the characteristic points to the initial characteristic map based on a distance power inverse ratio method to obtain a modified characteristic map;
the map construction of the acquired scanning point cloud data based on the LOAM to obtain an initial feature map comprises the following steps:
matching the acquired cloud data of the scanning points based on inter-frame according to the first frequency to obtain a matching splicing pose between data of adjacent frames;
splicing the data of the adjacent frames based on the corresponding splicing poses according to the second frequency to obtain an initial corner map and an initial surface map;
wherein the first frequency is greater than the second frequency;
the method for giving a weight to the distance of the characteristic point to the initial characteristic map based on the inverse distance power ratio method to obtain the corrected characteristic map comprises the following steps:
converting the initial corner map and the three-dimensional point cloud of the initial corner map into a world coordinate system and dividing the world coordinate system into three-dimensional grids;
counting the number of point clouds in each grid according to each grid, carrying out weight assignment on each point cloud based on the distance between each point cloud and a set position, and determining a coordinate corresponding to each grid;
and updating the initial corner map and the corner map according to the coordinates corresponding to the grids to obtain the corrected feature map.
5. An apparatus for locating an underground environment, comprising: a processor and a memory for storing a computer program capable of running on the processor, wherein,
the processor, when executing the computer program, is adapted to perform the steps of the method of any of claims 1 to 3.
6. A storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, performs the steps of the method of any one of claims 1 to 3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911217135.2A CN110849374B (en) | 2019-12-03 | 2019-12-03 | Underground environment positioning method, device, equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911217135.2A CN110849374B (en) | 2019-12-03 | 2019-12-03 | Underground environment positioning method, device, equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110849374A CN110849374A (en) | 2020-02-28 |
| CN110849374B true CN110849374B (en) | 2023-04-18 |
Family
ID=69607168
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911217135.2A Active CN110849374B (en) | 2019-12-03 | 2019-12-03 | Underground environment positioning method, device, equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110849374B (en) |
Families Citing this family (27)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111442722B (en) * | 2020-03-26 | 2022-05-17 | 达闼机器人股份有限公司 | Positioning method, positioning device, storage medium and electronic equipment |
| CN111461982B (en) * | 2020-03-30 | 2023-09-22 | 北京百度网讯科技有限公司 | Method and apparatus for splice point cloud |
| CN111461980B (en) * | 2020-03-30 | 2023-08-29 | 北京百度网讯科技有限公司 | Performance estimation method and device of point cloud stitching algorithm |
| CN111461981B (en) * | 2020-03-30 | 2023-09-01 | 北京百度网讯科技有限公司 | Error estimation method and device for point cloud stitching algorithm |
| CN111735451B (en) * | 2020-04-16 | 2022-06-07 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
| CN112739983B (en) * | 2020-04-24 | 2022-07-12 | 华为技术有限公司 | Method for correcting point cloud data and related device |
| WO2021232227A1 (en) * | 2020-05-19 | 2021-11-25 | 深圳市大疆创新科技有限公司 | Point cloud frame construction method, target detection method, ranging apparatus, movable platform, and storage medium |
| WO2021253068A1 (en) * | 2020-06-18 | 2021-12-23 | Commonwealth Scientific And Industrial Research Organisation | Navigation of an underground mining machine |
| CN111812669B (en) * | 2020-07-16 | 2023-08-04 | 南京航空航天大学 | Winding machine inspection device, positioning method thereof and storage medium |
| CN111983635B (en) * | 2020-08-17 | 2022-03-29 | 浙江商汤科技开发有限公司 | Pose determination method and device, electronic equipment and storage medium |
| CN112130168B (en) * | 2020-09-11 | 2023-04-14 | 北京埃福瑞科技有限公司 | Train position state detection method and system for turn-back control |
| CN112101229B (en) * | 2020-09-16 | 2023-02-24 | 云南师范大学 | Point cloud data feature point extraction method and device, computer equipment and storage medium |
| CN114526720B (en) * | 2020-11-02 | 2024-04-16 | 北京四维图新科技股份有限公司 | Positioning processing method, device, equipment and storage medium |
| CN114088093A (en) * | 2020-11-09 | 2022-02-25 | 北京京东乾石科技有限公司 | Point cloud map construction method, device and system and storage medium |
| CN113761090B (en) * | 2020-11-17 | 2024-04-05 | 北京京东乾石科技有限公司 | Positioning method and device based on point cloud map |
| CN112700495A (en) * | 2020-11-25 | 2021-04-23 | 北京旷视机器人技术有限公司 | Pose determination method and device, robot, electronic device and storage medium |
| CN112630787B (en) * | 2020-12-03 | 2022-05-17 | 深圳市优必选科技股份有限公司 | Positioning method, positioning device, electronic equipment and readable storage medium |
| CN112683268A (en) * | 2020-12-08 | 2021-04-20 | 中国铁建重工集团股份有限公司 | Roadway real-time positioning navigation method and system based on extended Kalman filtering |
| CN112630745A (en) * | 2020-12-24 | 2021-04-09 | 深圳市大道智创科技有限公司 | Environment mapping method and device based on laser radar |
| CN113155126B (en) * | 2021-01-04 | 2023-10-20 | 航天时代飞鸿技术有限公司 | Visual navigation-based multi-machine cooperative target high-precision positioning system and method |
| CN112883134A (en) * | 2021-02-01 | 2021-06-01 | 上海三一重机股份有限公司 | Data fusion graph building method and device, electronic equipment and storage medium |
| CN112862894B (en) * | 2021-04-12 | 2022-09-06 | 中国科学技术大学 | Robot three-dimensional point cloud map construction and expansion method |
| CN113219486B (en) * | 2021-04-23 | 2023-03-31 | 北京云迹科技股份有限公司 | Positioning method and device |
| CN113310484B (en) * | 2021-05-28 | 2022-06-24 | 杭州艾米机器人有限公司 | Mobile robot positioning method and system |
| CN113503883B (en) * | 2021-06-22 | 2022-07-19 | 北京三快在线科技有限公司 | Method for collecting data for constructing map, storage medium and electronic equipment |
| CN115661798B (en) * | 2022-12-23 | 2023-03-21 | 小米汽车科技有限公司 | Method and device for determining target area, vehicle and storage medium |
| CN117452429B (en) * | 2023-12-21 | 2024-03-01 | 江苏中科重德智能科技有限公司 | Robot positioning method and system based on multi-line laser radar |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225341A (en) * | 2016-12-14 | 2018-06-29 | 乐视汽车（北京）有限公司 | Vehicle positioning method |
| CN109507677A (en) * | 2018-11-05 | 2019-03-22 | 浙江工业大学 | A kind of SLAM method of combination GPS and radar odometer |
| CN109682385A (en) * | 2018-11-05 | 2019-04-26 | 天津大学 | A method of instant positioning and map structuring based on ORB feature |
| CN109813310A (en) * | 2019-03-11 | 2019-05-28 | 中南大学 | Underground working apparatus localization method, device, system and storage medium |
| CN109839112A (en) * | 2019-03-11 | 2019-06-04 | 中南大学 | Underground working apparatus localization method, device, system and storage medium |
| CN109945856A (en) * | 2019-02-18 | 2019-06-28 | 天津大学 | Based on inertia/radar unmanned plane autonomous positioning and build drawing method |
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-12-03 CN CN201911217135.2A patent/CN110849374B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108225341A (en) * | 2016-12-14 | 2018-06-29 | 乐视汽车（北京）有限公司 | Vehicle positioning method |
| CN109507677A (en) * | 2018-11-05 | 2019-03-22 | 浙江工业大学 | A kind of SLAM method of combination GPS and radar odometer |
| CN109682385A (en) * | 2018-11-05 | 2019-04-26 | 天津大学 | A method of instant positioning and map structuring based on ORB feature |
| CN109945856A (en) * | 2019-02-18 | 2019-06-28 | 天津大学 | Based on inertia/radar unmanned plane autonomous positioning and build drawing method |
| CN109813310A (en) * | 2019-03-11 | 2019-05-28 | 中南大学 | Underground working apparatus localization method, device, system and storage medium |
| CN109839112A (en) * | 2019-03-11 | 2019-06-04 | 中南大学 | Underground working apparatus localization method, device, system and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110849374A (en) | 2020-02-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110849374B (en) | Underground environment positioning method, device, equipment and storage medium | |
| CN112347840B (en) | Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method | |
| US11295472B2 (en) | Positioning method, positioning apparatus, positioning system, storage medium, and method for constructing offline map database | |
| US10571270B2 (en) | Fusion of sensor and map data using constraint based optimization | |
| Badino et al. | Visual topometric localization | |
| Rosinol et al. | Incremental visual-inertial 3d mesh generation with structural regularities | |
| CN111402339B (en) | Real-time positioning method, device, system and storage medium | |
| CN109813310B (en) | Underground operation equipment positioning method, device and system and storage medium | |
| CN104833354A (en) | Multibasic multi-module network integration indoor personnel navigation positioning system and implementation method thereof | |
| Ibrahim et al. | Inertial measurement unit based indoor localization for construction applications | |
| US20200100066A1 (en) | System and Method for Generating Floor Plans Using User Device Sensors | |
| Atia et al. | Particle-filter-based WiFi-aided reduced inertial sensors navigation system for indoor and GPS-denied environments | |
| CN111665512A (en) | Range finding and mapping based on fusion of 3D lidar and inertial measurement unit | |
| CN109839112B (en) | Underground operation equipment positioning method, device and system and storage medium | |
| CN113706612A (en) | Underground coal mine vehicle positioning method fusing UWB and monocular vision SLAM | |
| CN104848861A (en) | Image vanishing point recognition technology based mobile equipment attitude measurement method | |
| Jia et al. | A cross-correction LiDAR SLAM method for high-accuracy 2D mapping of problematic scenario | |
| CN114119886A (en) | High-precision map point cloud reconstruction method and device, vehicle, equipment and storage medium | |
| CN111678513A (en) | Ultra-wideband/inertial navigation tight coupling indoor positioning device and system | |
| Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
| CN111679303A (en) | Comprehensive positioning method and device for multi-source positioning information fusion | |
| Khoshelham et al. | Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry | |
| Shu et al. | 3d point cloud-based indoor mobile robot in 6-dof pose localization using a wi-fi-aided localization system | |
| CN111578939B (en) | Robot tight combination navigation method and system considering random variation of sampling period | |
| CA2894863A1 (en) | Indoor localization using crowdsourced data |
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 |