CN116643289A - Underground roadway laser radar SLAM method with attached wire constraint - Google Patents
Underground roadway laser radar SLAM method with attached wire constraint Download PDFInfo
- Publication number
- CN116643289A CN116643289A CN202310613823.0A CN202310613823A CN116643289A CN 116643289 A CN116643289 A CN 116643289A CN 202310613823 A CN202310613823 A CN 202310613823A CN 116643289 A CN116643289 A CN 116643289A
- Authority
- CN
- China
- Prior art keywords
- point
- coordinates
- coordinate
- attached
- matched
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 77
- 238000013507 mapping Methods 0.000 claims abstract description 25
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 238000005457 optimization Methods 0.000 claims abstract description 11
- 238000010276 construction Methods 0.000 claims abstract description 3
- 230000008569 process Effects 0.000 claims description 18
- 238000012937 correction Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 6
- 239000004020 conductor Substances 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims 1
- 238000005259 measurement Methods 0.000 abstract description 18
- 238000010586 diagram Methods 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 239000000428 dust Substances 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000010801 machine learning Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
Classifications
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The application discloses an underground roadway laser radar SLAM method with attached wire constraint, which comprises the following steps: firstly, obtaining the coordinates of a reflector reference through control measurement of an attached wire; then, carrying out laser radar SLAM mapping based on the reflector references, and searching a measuring station at intervals of the same preset distance until all reflector references are measured; and then, carrying out the adjustment calculation of the attached wires on the known reflector datum and the measuring station determined by the underground roadway, correcting the coordinates of the measuring station, and finally correcting the coordinates of all map points by using a nonlinear optimization method. The method can acquire three-dimensional point cloud data of the underground tunnel in real time, establish an accurate three-dimensional point cloud map of the underground tunnel, and ensure personnel safety while achieving high-precision construction of the underground tunnel, and has the advantages of low hardware equipment cost and high automation level.
Description
Technical Field
The application relates to the technical field of machine learning, in particular to an underground roadway laser radar SLAM method with attached wire constraint.
Background
In the field of machine learning, the laser radar SLAM method plays a key role, and comprises important research directions such as searching and rescuing, disaster response, safety monitoring, infrastructure checking, accurate agriculture, exploration and drawing, robot navigation, field investigation and the like. Although lidar SLAM methods have made great progress in urban and structural environments, positioning and mapping in extremely unknown environments such as downhole roadways still face numerous challenges. Rough and slippery terrain can make the wheel odometer inaccurate, darkness and dust, and long corridor without significant features can obscure the sensor, drift easily, and false loop detection that occurs frequently in environments with repetitive appearances can cause severe distortion of the entire map, especially challenging for basic tasks such as robot positioning and mapping.
In recent years, numerous heuristics for unknown environments have added constraint benchmarks. The visual positioning reference mark SLAM is used in the prior art, the method provides a convenient, flexible and robust method, and provides a robust initial front end for the GTSAM factor graph optimizer through simple extraction, so that a series of experiments based on visual label references can be rapidly designed. The novel and flexible radar point cloud reference mark LiDARTag is also provided in the existing method, well supplements and is compatible with the existing visual positioning reference, so that efficient multi-sensor fusion and calibration tasks are realized, the concept of minimizing fitting errors between point cloud and a reference template is further provided, the posture of the reference is estimated, and the proposed method can reliably provide the posture of the reference and a unique ID code. The reflector datum is matched with the laser radar to realize laser odometer compensation, so that light-weight real-time positioning is realized. The researches show that constraint references are still required to be added for exploring unknown complex environments so as to improve the precision, and particularly, the real-time positioning and mapping of extremely unknown environments such as underground roadways and the like are aimed at, so that the current research precision can not reach practical application of production and life.
Disclosure of Invention
The application provides an underground roadway laser radar SLAM method with the constraint of a conductor, which is used for solving the technical problems that the mapping accuracy of extremely unknown environments such as an underground roadway is always a challenging problem. The method can provide more accurate constraint reference for real-time positioning and mapping of the underground roadway, embeds control measurement constraint and error propagation law constraint of the measurabilities into the laser radar SLAM method, improves positioning and mapping accuracy of the underground roadway environment, provides powerful technical support for exploration of large-scale extremely unknown environments, improves searching and rescuing speed, and ensures personnel safety.
In order to achieve the above object, the present application provides a method for laser radar SLAM of a downhole roadway with attached wire constraint, comprising:
a measuring station is arranged based on the underground roadway environment, coordinate data of the measuring station are obtained, and a reflector is arranged according to the coordinate data;
the IMU sensor is used for assisting the laser radar sensor to scan point cloud data, the existing LOAM method based on characteristics is used for carrying out real-time positioning and map building calculation, and the identified reflector reference position coordinate data is stored;
taking the reference position of the reflecting plate as a known control point, taking map point coordinates generated in the SLAM mapping process of the laser radar as initialization coordinates of all map points, and calculating the distance between the position of the laser radar and the initial position once every the same time period;
if the distance and the original position are equal to the preset distance, determining a measuring station of the attached conducting wire on map points generated near the position equal to the preset distance, repeating the process of determining the measuring station of the attached conducting wire until the robot walks out of the underground roadway and scans and identifies a reflector standard outside the underground roadway, obtaining the coordinate position of a known control point at the end point, and correcting the coordinates of all map points.
Preferably, acquiring three-dimensional coordinates of the measuring station in a world coordinate system includes:
the plane coordinates of the measuring stations are obtained through an average difference calculation method, meanwhile, the coordinate azimuth angles of all the observation sides are calculated, the angle closing difference of the coordinate azimuth angles of the attached wires is obtained, the error value of the angle closing difference is evenly distributed to the coordinate azimuth angles of all the sides, the coordinate azimuth angles after correction of all the observation sides are obtained, the coordinate increment of each measuring station is calculated by utilizing the corrected coordinate azimuth angles and the lengths of the observation sides, the coordinate increment closing difference is calculated by utilizing the sum of the coordinate increment of all the measuring stations and the difference of the coordinates of the control points, the coordinate increment closing difference is evenly distributed to all the measuring stations according to the distance, the coordinate increment after correction of all the measuring stations can be obtained, and the coordinate value of all the measuring stations is determined.
Preferably, the conformable wire measures a finite error, the finite error should satisfy:
wherein ,fβ To attach the angle closure difference of the wire, f Beta volume For the allowable limit difference of the angle closing difference of the attached wires, n is the station number of the attached wires, K is the coordinate increment closing difference of the attached wires, K Container with a cover Difference limiting for closing difference of coordinate increment of attached wire x To attach the coordinate increment of the wire in the x-axis direction, f y For the coordinate increment of the bond wire in the y-axis direction, Σd is the sum of the lengths of all the observation sides.
Preferably, the real-time positioning and mapping calculation performed by using the existing feature-based lom method includes:
evaluating the smoothness of a local surface by using a feature-based LOAM method, sorting the points in scanning according to the smoothness, selecting the point with the largest smoothness as an edge point, and the point with the smallest smoothness as a feature point of a plane point;
after the feature points are determined, performing a laser odometer process according to the feature point matching reference;
coarse processing is carried out through a laser odometer, the speed of the first frequency threshold is estimated through operation of the laser odometer at the set first frequency threshold, fine processing is carried out through laser image construction, undistorted cloud is matched and recorded on a map based on the set second frequency threshold, a map of the second frequency threshold is created, and accurate motion estimation and real-time mapping are achieved.
Preferably, the method for calculating the smoothness is as follows:
where i is a point in the current scan, k is the current scan frame, S is a set of consecutive points of i returned by the laser scanner in the same scan, S contains half of its points on each side of i, j is one point in S,for the coordinates of i in the current scan, +.>Is the coordinate of j in the current scan.
Preferably, the feature point matching includes edge point matching and plane point matching;
the method for performing the edge point matching comprises the following steps:
wherein ,dε For the distance from the edge point to be matched in the current scan to the line to be matched in the previous scan,for the coordinates of the edge point i to be matched in the current scan, +.>For the coordinates of point j on the line to be matched in the previous scan, +.>The coordinates of a point l on a line to be matched in the previous scanning;
the method for carrying out the planar point matching comprises the following steps:
wherein ,dH For the distance from the point of the plane to be matched in the current scan to the plane to be matched in the previous scan,for the coordinates of the plane point i to be matched in the current scan, +.>For the coordinates of the point j on the plane to be matched in the previous scan, +.>For the coordinates of the point l on the plane to be matched in the previous scan, +.>Is the coordinates of point m on the plane to be matched in the previous scan.
Preferably, the laser mapping is performed based on a non-linear optimized jacobian matrix, including:
wherein ,for laser radar pose transformation between the current scan and the previous scan, J is Jacobian matrix, and lambda is Levenberg-MarquaThe rdt optimization algorithm determines a factor, d, that is the sum of the distances between all points and the corresponding matches.
Preferably, the coordinates of all map points are corrected by using a nonlinear optimization method, and the method comprises the following steps:
wherein X is an unknown coordinate variable set to be solved, X * For reliability of variable X, z k To correct the map point coordinates before correction, p (X) is the prior probability of X, p (z) k |X k ) Is the probability constraint of the variable X, h k (X k ) As a nonlinear function of X, Ω k Is an information matrix.
Compared with the prior art, the application has the following advantages and technical effects:
according to the method, the three-dimensional point cloud data of the underground tunnel can be obtained in real time, and an accurate three-dimensional point cloud map of the underground tunnel is built; the method has low cost and high automation level, and ensures personnel safety;
the application can realize automatic data acquisition of the underground tunnel, greatly liberate labor force and improve the production efficiency of the underground tunnel.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application. In the drawings:
FIG. 1 is a flow chart of a method for laser radar SLAM in a downhole roadway with attached wire constraint according to an embodiment of the present application;
FIG. 2 is a schematic diagram of a conventional attached wire control measurement according to an embodiment of the present application;
FIG. 3 is a feature-based lidar SLAM flowchart of an embodiment of the present application;
fig. 4 is a schematic diagram of a conformable conductor constraint for a downhole roadway environment in accordance with an embodiment of the present application.
Detailed Description
It should be noted that, without conflict, the embodiments of the present application and features of the embodiments may be combined with each other. The application will be described in detail below with reference to the drawings in connection with embodiments.
It should be noted that the steps illustrated in the flowcharts of the figures may be performed in a computer system such as a set of computer executable instructions, and that although a logical order is illustrated in the flowcharts, in some cases the steps illustrated or described may be performed in an order other than that illustrated herein.
For extremely unknown environmental exploration of downhole roadways and the like, there are many efforts focused on applying robotic systems to increase productivity and safety and reduce costs in such processes. The exploration of a robot in a downhole tunnel is challenging because the downhole tunnel is subject to drift under non-nominal conditions of poor or absent illumination, dust, puddles, and non-lambertian surfaces. The application mainly aims at extremely unknown environments such as underground roadways and the like, and provides a method for restraining laser radar SLAM by utilizing the attached wire control measurement technology of surveying and mapping, so that a more accurate underground roadway map is obtained, and the accurate positioning is achieved while the map building precision is improved. The application can realize automatic data acquisition of the underground tunnel, greatly liberate labor force and improve the production efficiency of the underground tunnel.
The application provides an underground roadway laser radar SLAM method attached with wire constraint, as shown in figure 1, comprising the following steps:
1. light reflecting plate arrangement and control
(1) And wire control measurement: and (3) arranging a measuring station near the underground tunnel environment, and carrying out standard attachment and wire control measurement on the basis of control points of a known world coordinate system to obtain the three-dimensional coordinates of the measuring station near the underground tunnel environment under the world coordinate system.
(2) The reflector is distributed and controlled at the position of the control point: the three-dimensional coordinates of the measuring station obtained by the aid of wire control measurement are used as known control points in the laser SLAM process, and reflector references which are easy to identify by a laser radar are arranged on the known control points.
Step 1: if the open-air scene near the extremely unknown environment such as the underground tunnel does not have known control points, the closed-air scene needs to be measured by using the closed-air wires, a measuring station is arranged at a proper position near the underground tunnel environment, the closed-air wires are measured by using a common surveying instrument such as a total station, and the plane coordinates of the points are calculated by measuring the included angles and the side lengths. The method mainly comprises the steps of starting measurement from one known point, passing through a plurality of unknown points, reaching the other known point, and obtaining the plane coordinates of the measuring station through adjustment calculation.
As shown in fig. 2, which is a schematic diagram of conventional attached wire control measurement, given the coordinate positions of points a, B, C, and D, 4 stations are assumed in the middle, and if the measurement direction is from point A, B to C, D, the measured front-rear station angle is the right side of the forward direction, which is called the right angle. The coordinate azimuth angle is an included angle between a certain observation side and the positive north direction of the X axis in the plane rectangular coordinate system, and the clockwise direction rotates to obtain an angle from the positive north direction of the X axis. The coordinate azimuth of each observation edge can be calculated by using the formula (1). For example, the coordinate azimuth of the observation edge 12 is:
α 12 ＝α B1 -∠1-180° (1)
if the final result of equation (1) is less than zero, the azimuth of the coordinates of the observation edge 12 is as follows (2):
a′ 12 ＝α 12 +360° (2)
the coordinate azimuth of each observation edge can be calculated by the same method, wherein the coordinate azimuth of the CD edge is included. Since points C and D are known control points, the CD edge has a coordinate azimuth that has been determined theoretically, and thus the angle closure difference of the coordinate azimuth of the accompanying wire can be calculated as follows (3):
f β ＝α CD measurement -α CD reason (3)
wherein fβ For angle closure difference, alpha CD measurement For calculating the azimuth angle of the CD edge according to the measurement, alpha CD reason The coordinate azimuth that has been theoretically determined for the control point is C, D. The error value of the angle closure difference is averagely distributed to the coordinate azimuth angles of all sides, and all the observed sides can be obtained after correctionIs a coordinate azimuth of (a).
The coordinate increment of each measuring station can be calculated by using the corrected coordinate azimuth and the length of the observation edge, and the following formula (4) is adopted:
where d is the length of the observation edge and α is the corrected coordinate azimuth. Calculating a coordinate increment closing difference by using the sum of the coordinate increments of the respective measurement points and the difference between the coordinates of the control point B and the control point C, as shown in the following formula (5):
wherein ,ΔxCD 、Δy CD The coordinate increment of the control point C and the control point D in the x-axis direction and the y-axis direction are respectively. f (f) x 、f y And respectively obtaining coordinate increment of each measuring station after correction by equally distributing the coordinate increment closing difference to each measuring station according to the distance to the coordinate increment closing difference of the attached wire in the x-axis direction and the y-axis direction, so that the coordinate value of each measuring station can be determined.
The attached wire measures the limited difference of error, and the following formula (6) should be satisfied:
wherein ,fβ To attach the angle closure difference of the wire, f Beta volume For the allowable limit difference of the angle closing difference of the attached wires, n is the station number of the attached wires, K is the coordinate increment closing difference of the attached wires, K Container with a cover Difference limiting for closing difference of coordinate increment of attached wire x To attach the coordinate increment of the wire in the x-axis direction, f y For the coordinate increment of the bond wire in the y-axis direction, Σd is the sum of the lengths of all the observation sides. Thus, the coordinates of the control points near the underground roadway environment can be obtained.
Step 2: laser lightThe radar light is reflected by any object, but the reflection intensity of the surface of different objects is different. The reflector is a reflecting device made of special materials, and the laser irradiates the reflector to obtain the reflecting intensity exceeding that of most other materials. The reflector is arranged on a control point with known coordinates, the coordinates of the scanned reflector can be determined at the first time after the laser radar scans the reflector, and the coordinate position is determined mainly based on state quantity and control. Motion model of robot with respect to state quantity X t-1 Jacobian of (a) is represented by the following formula (7):
motion model of robot with respect to control u= [ v ] x v y ω] T Jacobian of (c) as follows (8):
therefore, the application selects to arrange the reflector datum on the well-determined control point coordinate position, and uses the reflector datum as the constraint datum added to the laser radar SLAM.
2. Laser radar SLAM
(1) LOAM-based lidar SLAM: and (3) mapping from the external environment of the underground roadway, performing point cloud data scanning by using an IMU sensor to assist a laser radar sensor, and performing real-time positioning and mapping calculation by using the existing LOAM method based on characteristics.
(2) And (5) identifying the control points of the attached wires: in the SLAM mapping process, once the laser radar recognizes the position coordinates of the reflector reference, the data record is stored, and the rear-end optimization process of the SLAM is performed based on the position.
Step 1: after the reflector references are laid, the laser radar SLAM process can be started. Laser odometers refer to methods that utilize laser radar (LiDAR) for recursive pose state estimation.
The lidar SLAM procedure is specifically shown in fig. 3, and mainly uses a feature-based LOAM method, which mainly uses the term c to evaluate the smoothness of a local surface, as shown in the following formula (9):
where i is a point in the current scan, k is the current scan frame, S is a set of consecutive points of i returned by the laser scanner in the same scan, where S contains half of its points on each side of i, j is one point in S,for the coordinates of i in the current scan, +.>Is the coordinate of j in the current scan. Thus, this equation represents the calculation of the local surface smoothness of the i-point.
And sorting the points in the scanning according to the c value, selecting the point with the largest c value as an edge point, and selecting the point with the smallest c value as a characteristic point of the plane point. After the characteristics are determined, performing a laser mileage process according to the characteristic point matching reference, wherein the edge points are matched with the following formula (10):
wherein ,dε For the distance from the edge point to be matched in the current scan to the line to be matched in the previous scan,for the coordinates of the edge point i to be matched in the current scan, +.>For the coordinates of point j on the line to be matched in the previous scan, +.>The coordinates of a point l on a line to be matched in the previous scanning;
plane point matching is as follows formula (11):
wherein ,dH For the distance from the point of the plane to be matched in the current scan to the plane to be matched in the previous scan,for the coordinates of the plane point i to be matched in the current scan, +.>For the coordinates of the point j on the plane to be matched in the previous scan, +.>For the coordinates of the point l on the plane to be matched in the previous scan, +.>Is the coordinates of point m on the plane to be matched in the previous scan.
The laser mapping is mainly based on a non-linear optimized Jacobian matrix, and the following formula (12):
wherein ,for laser radar pose transformation between the current scanning and the previous scanning, J is a Jacobian matrix, lambda is a factor determined by a Levenberg-Marquardt optimization algorithm, d is the sum of distances between all points and corresponding matches, and d is approximately zero by nonlinear iterative solution.
As can be seen from fig. 3, in the laser SLAM process, coarse processing is performed mainly by a laser odometer, running at a frequency of 10Hz, estimating the speed of a higher frequency, fine processing is performed by laser mapping, undistorted clouds are matched and recorded on a map at a frequency of 1Hz, and a map of a lower frequency is created, so that accurate motion estimation and real-time mapping are realized.
Step 2: in the SLAM mapping process, once the laser radar recognizes the position coordinate of the reflector reference, the data record is stored, the position is used as the known control point coordinate, and the rear end optimization correction process of the SLAM is carried out based on the position, which is equivalent to a loop detection module commonly known in SLAM.
3. Attached wire restraint
(1) And (5) attached wire station measurement identification: and taking the reference position of the reflector distributed and controlled outside the underground roadway as a known control point, taking map point coordinates generated in the laser radar SLAM mapping process as initialization coordinates of all map points, calculating the approximate distance between the position of the laser radar and the initial position once every certain time, and determining a measuring station of an attached wire on the map point generated near the position if the distance is 150 meters from the initial position, so that the measuring station is searched every 150 meters until the robot walks out of the underground roadway and scans and identifies the reflector reference outside the underground roadway, and obtaining the coordinate position of the known control point at the end point. And carrying out the calculation of the attached wire control measurement adjustment on the known control points and the measuring stations determined by the underground roadway, and correcting the coordinates of the measuring stations.
(2) Map pose correction: and taking the control measurement coordinates of the measuring station as final coordinates of the map points, and finally correcting all map point coordinates by using a nonlinear optimization method.
Step 1: as shown in fig. 1, in the laser radar SLAM positioning and mapping process, an algorithm for calculating a distance every a certain number of point cloud scanning frames is further added to judge whether 150 meters have passed, and if 150 meters have passed, an attached wire measuring station is added as a constraint. The constraint of the attached wires added in the laser radar SLAM is different from the traditional method for measuring the attached wires.As shown in fig. 4, the present application calculates the plane coordinates of the point not by measuring the included angle and the observation side length, but directly back calculates the coordinate azimuth angle and the observation side length of the observation side by using the coordinates of the measuring station. The azimuth angle and the side length of the coordinates of the line connecting the two points are calculated from the coordinates of the two known points, and the calculation is called coordinate back calculation. If the coordinates of the point A to the point B are known, the coordinate azimuth angle alpha of the AB edge needs to be calculated AB The quadrant angle between two points should be calculated first, as shown in the following equation (13):
if Deltax AB Positive, Δy AB Positive, straight line AB is in the first quadrant, α AB ＝R AB The method comprises the steps of carrying out a first treatment on the surface of the If Deltax AB Negative, Δy AB Positive, straight line AB is in the second quadrant, α AB ＝180°-R AB The method comprises the steps of carrying out a first treatment on the surface of the If Deltax AB Negative, Δα AB Negative, line AB is in the third quadrant, α AB ＝180°+R AB The method comprises the steps of carrying out a first treatment on the surface of the If Deltax AB Positive, Δy AB Negative, straight line AB is in the fourth quadrant, α AB ＝360°-R AB 。
After the coordinate azimuth angle of each observation edge is obtained, the coordinate of each measurement site can be obtained by continuing to calculate the adjustment of the attached wires according to the conventional attached wire calculation method in fig. 2.
Step 2: after the coordinates of each measuring station are obtained according to the constraint basis criteria of the attached wires, the map points and the driving tracks generated in the whole laser radar SLAM process are corrected according to the coordinates of each measuring station, and the coordinates of all map points are corrected mainly by using a nonlinear optimization method, wherein the main formula is as follows (14):
wherein X is an unknown coordinate variable set to be solved, X * For reliability of variable X, z k In order to correct the coordinates of the map points before correction,p (X) is the prior probability of X, and p (X) includes any prior knowledge about X, without which p (X) becomes a constant, can be deleted from the optimization, p (z) k |X k ) Is the probability constraint of the variable X, h k (X k ) As a nonlinear function of X, Ω k Is the information matrix, i.e. the inverse of the covariance matrix.
And correcting all map points to obtain a more accurate underground roadway point cloud map and a laser radar running track.
According to the method, the three-dimensional point cloud data of the underground tunnel can be obtained in real time, and an accurate three-dimensional point cloud map of the underground tunnel is built; the method has the advantages of low cost, high automation level and personnel safety guarantee.
The present application is not limited to the above-mentioned embodiments, and any changes or substitutions that can be easily understood by those skilled in the art within the technical scope of the present application are intended to be included in the scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.
Claims (8)
1. The utility model provides a underworkings laser radar SLAM method of attached wire constraint which characterized in that includes:
a measuring station is arranged based on the underground roadway environment, coordinate data of the measuring station are obtained, and a reflector is arranged according to the coordinate data;
the IMU sensor is used for assisting the laser radar sensor to scan point cloud data, the existing LOAM method based on characteristics is used for carrying out real-time positioning and map building calculation, and the identified reflector reference position coordinate data is stored;
taking the reference position of the reflecting plate as a known control point, taking map point coordinates generated in the SLAM mapping process of the laser radar as initialization coordinates of all map points, and calculating the distance between the position of the laser radar and the initial position once every the same time period;
if the distance and the original position are equal to the preset distance, determining a measuring station of the attached conducting wire on map points generated near the position equal to the preset distance, repeating the process of determining the measuring station of the attached conducting wire until the robot walks out of the underground roadway and scans and identifies a reflector standard outside the underground roadway, obtaining the coordinate position of a known control point at the end point, and correcting the coordinates of all map points.
2. The attached wire-constrained downhole roadway lidar SLAM method of claim 1, wherein obtaining three-dimensional coordinates of the survey site in a world coordinate system comprises:
the plane coordinates of the measuring stations are obtained through an average difference calculation method, meanwhile, the coordinate azimuth angles of all the observation sides are calculated, the angle closing difference of the coordinate azimuth angles of the attached wires is obtained, the error value of the angle closing difference is evenly distributed to the coordinate azimuth angles of all the sides, the coordinate azimuth angles after correction of all the observation sides are obtained, the coordinate increment of each measuring station is calculated by utilizing the corrected coordinate azimuth angles and the lengths of the observation sides, the coordinate increment closing difference is calculated by utilizing the sum of the coordinate increment of all the measuring stations and the difference of the coordinates of the control points, the coordinate increment closing difference is evenly distributed to all the measuring stations according to the distance, the coordinate increment after correction of all the measuring stations can be obtained, and the coordinate value of all the measuring stations is determined.
3. The attached conductor constrained downhole roadway lidar SLAM method of claim 2, wherein the attached conductor measures a finite error difference that satisfies:
wherein ,fβ To attach the angle closure difference of the wire, f Beta volume For the allowable limit difference of the angle closing difference of the attached wires, n is the station number of the attached wires, K is the coordinate increment closing difference of the attached wires, K Container with a cover Difference limiting for closing difference of coordinate increment of attached wire x To attach the coordinate increment of the wire in the x-axis direction, f y For the coordinate increment of the bond wire in the y-axis direction, Σd is the sum of the lengths of all the observation sides.
4. The attached wire-constrained downhole roadway lidar SLAM method of claim 3, wherein utilizing the existing feature-based LOAM method for real-time localization and mapping calculations comprises:
evaluating the smoothness of a local surface by using a feature-based LOAM method, sorting the points in scanning according to the smoothness, selecting the point with the largest smoothness as an edge point, and the point with the smallest smoothness as a feature point of a plane point;
after the feature points are determined, performing a laser odometer process according to the feature point matching reference;
coarse processing is carried out through a laser odometer, the speed of the first frequency threshold is estimated through operation of the laser odometer at the set first frequency threshold, fine processing is carried out through laser image construction, undistorted cloud is matched and recorded on a map based on the set second frequency threshold, a map of the second frequency threshold is created, and accurate motion estimation and real-time mapping are achieved.
5. The attached wire-constrained downhole roadway lidar SLAM method of claim 4, wherein the method of calculating the smoothness is:
where i is a point in the current scan, k is the current scan frame, S is a set of consecutive points of i returned by the laser scanner in the same scan, S contains half of its points on each side of i, j is one point in S,for the coordinates of i in the current scan, +.>Is the coordinate of j in the current scan.
6. The attached wire-constrained downhole roadway lidar SLAM method of claim 4, wherein the feature point matching comprises edge point matching and plane point matching;
the method for performing the edge point matching comprises the following steps:
wherein ,dε For the distance from the edge point to be matched in the current scan to the line to be matched in the previous scan,for the coordinates of the edge point i to be matched in the current scan, +.>For the coordinates of point j on the line to be matched in the previous scan, +.>The coordinates of a point l on a line to be matched in the previous scanning;
the method for carrying out the planar point matching comprises the following steps:
wherein ,dH For the distance from the point of the plane to be matched in the current scan to the plane to be matched in the previous scan,for the coordinates of the plane point i to be matched in the current scan, +.>For the coordinates of the point j on the plane to be matched in the previous scan, +.>For the coordinates of the point l on the plane to be matched in the previous scan, +.>Is the coordinates of point m on the plane to be matched in the previous scan.
7. The attached wire-constrained downhole roadway lidar SLAM method of claim 4, wherein the laser mapping is performed based on a non-linearly optimized jacobian matrix, comprising:
wherein ,for the laser radar pose transformation between the current scan and the previous scan, J is the jacobian matrix, λ is a factor determined by the Levenberg-Marquardt optimization algorithm, and d is the sum of the distances between all points and the corresponding matches.
8. The method for laser radar SLAM of a downhole roadway with attached wire constraint according to claim 1, wherein the coordinates of all map points are corrected by a nonlinear optimization method, and the method comprises the steps of:
wherein X is an unknown coordinate variable set to be solved, X * For reliability of variable X, z k To correct the map point coordinates before correction, p (X) is the prior probability of X, p (z) k |X k ) Is the probability constraint of the variable X, h k (X k ) As a nonlinear function of X, Ω k Is an information matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310613823.0A CN116643289A (en) | 2023-05-29 | 2023-05-29 | Underground roadway laser radar SLAM method with attached wire constraint |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310613823.0A CN116643289A (en) | 2023-05-29 | 2023-05-29 | Underground roadway laser radar SLAM method with attached wire constraint |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116643289A true CN116643289A (en) | 2023-08-25 |
Family
ID=87618293
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310613823.0A Pending CN116643289A (en) | 2023-05-29 | 2023-05-29 | Underground roadway laser radar SLAM method with attached wire constraint |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116643289A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118731925A (en) * | 2024-09-04 | 2024-10-01 | 泰安万川电器设备有限公司 | Underground coal mine vehicle positioning monitoring method and system based on ranging radar |
-
2023
- 2023-05-29 CN CN202310613823.0A patent/CN116643289A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118731925A (en) * | 2024-09-04 | 2024-10-01 | 泰安万川电器设备有限公司 | Underground coal mine vehicle positioning monitoring method and system based on ranging radar |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109001711B (en) | Multi-line laser radar calibration method | |
| CN114526745B (en) | Drawing construction method and system for tightly coupled laser radar and inertial odometer | |
| Zhao et al. | A robust laser-inertial odometry and mapping method for large-scale highway environments | |
| AU2007355942B2 (en) | Arrangement and method for providing a three dimensional map representation of an area | |
| CN110361027A (en) | Robot path planning method based on single line laser radar Yu binocular camera data fusion | |
| US8077913B2 (en) | Method and device for determining the actual position of a geodetic instrument | |
| CN114019552B (en) | A method for optimizing location reliability based on Bayesian multi-sensor error constraints | |
| CN115407357A (en) | Low-beam LiDAR-IMU-RTK positioning and mapping algorithm based on large scenes | |
| CN111982099B (en) | Robot hybrid positioning method, device, equipment and computer readable medium | |
| Qingqing et al. | Multi-modal lidar dataset for benchmarking general-purpose localization and mapping algorithms | |
| CN110082783B (en) | Method and device for detecting cliffs | |
| Raval et al. | A preliminary investigation of mobile mapping technology for underground mining | |
| Yang et al. | A hierarchical approach for refining point cloud quality of a low cost UAV LiDAR system in the urban environment | |
| CN118274815A (en) | Real-time positioning and mapping method in long tunnel environment | |
| CN113554705A (en) | Robust positioning method for laser radar in changing scene | |
| CN115183762A (en) | Airport warehouse inside and outside mapping method, system, electronic equipment and medium | |
| CN114964259A (en) | Attitude calculation method and system based on infrared focal plane array time domain signal and infrared image | |
| CN116643289A (en) | Underground roadway laser radar SLAM method with attached wire constraint | |
| Tao et al. | Automated processing of mobile mapping image sequences | |
| Zhao et al. | Updating a digital geographic database using vehicle-borne laser scanners and line cameras | |
| CN115728803A (en) | A continuous positioning system and method for urban driving vehicles | |
| CN113592951A (en) | Method and device for calibrating external parameters of vehicle-road cooperative middle-road side camera and electronic equipment | |
| CN118518120A (en) | A reflective column mapping and positioning method suitable for long corridor environments in underground mines | |
| CN112684797A (en) | Obstacle map construction method | |
| CN112229394B (en) | Colliery underground mobile device location attitude determination system based on infrared motion is caught |
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 |