Underground motor positioning method and system based on laser SLAM

Abstract

The invention discloses an underground motor positioning method and system based on laser SLAM (Simultaneous Localization and Mapping), and the method introduces a dynamic voxel filtering strategy based on point cloud density and reflection intensity under an FAST-LIO2 framework, and can keep key geometric features and improve the calculation efficiency while effectively eliminating dust and environmental noise points. For vertical direction accumulative errors which are easy to occur in a mine environment, an RANSAC plane fitting method is utilized to extract ground points and construct ground constraints, so that long-term drift in the Z-axis direction is effectively inhibited. In consideration of the problem of insufficient feature constraints in degradation environments such as roadways and tunnels, degradation factors are further defined through Hessian matrix singular value decomposition, outlier feature values are identified in combination with DBSCAN clustering, laser radar observation covariance is dynamically adjusted, and real-time detection and compensation of degradation scenes are achieved.

Description

Technical Field

[0001] This invention belongs to the field of underground motor positioning technology, and relates to a method and system for underground motor positioning based on laser SLAM. Background Technology

[0002] Metal mineral resource mining faces complex geological conditions and harsh working environments, resulting in prominent problems such as low levels of intelligence in mining equipment and high operational risks. Intelligent mining is the only way to improve mining efficiency, ensure personnel safety, and guarantee high-quality development of the mining industry. Using intelligent mining equipment to drive the transformation and upgrading of traditional mining can fundamentally enhance the core competitiveness of mining enterprises and promote the development of traditional mining towards high efficiency, safety, and sustainability. Precise positioning of electric locomotives is a crucial aspect of intelligent mining, playing a vital role in improving transportation efficiency and ensuring safe production. Therefore, it places more stringent requirements on the positioning accuracy and real-time performance of electric locomotives.

[0003] The underground mine environment is characterized by its unstructured nature, irregular shape, dim lighting, numerous branching tunnels, and obstacles. It is difficult to describe the underground environment using structured or regularized methods, and the poor signal reception in mines makes high-precision GPS positioning impossible. Some studies have addressed the positioning problem by employing technologies such as UWB, RFID, and Zigbee, which do not rely on the device itself for positioning. However, these technologies require the pre-deployment of numerous base stations and beacons in the mine tunnels, achieving positioning through distance measurement between locomotives and base stations. However, due to the susceptibility of signals to interference and severe multipath effects in non-line-of-sight environments, positioning accuracy fluctuates significantly. Furthermore, beacon deployment requires coverage of the entire mine space, resulting in poor equipment convenience and high overall costs, making it unsuitable for large-scale use in underground mine tunnels. The dark and narrow environment also limits the use of visual sensors. In areas with poor ventilation, there are also harsh environmental factors such as high dust levels and humidity. Therefore, using existing common positioning methods for underground mine positioning suffers from poor positioning accuracy and insufficient efficiency.

[0004] To enable autonomous and stable operation of unmanned electric locomotives in underground mines, real-time localization and mapping (SLAM) technology in unknown environments is crucial. SLAM was initially developed to allow robots to acquire data from deployed external sensors to achieve self-localization and build incremental maps when moving in unknown environments. Now widely used in surveying and navigation, SLAM's environmental perception and localization capabilities are increasingly being applied as a core technology in the auxiliary localization modules of various vehicles and mobile robots. Commonly used sensors in SLAM include LiDAR and cameras. Some researchers have proposed visual SLAM for position estimation in complex working conditions such as low light, high dust, and high water mist in mines. However, visual SLAM is prone to localization failures in weak texture and low-light environments and is not suitable for mapping large scenes. LiDAR SLAM, on the other hand, is a reliable ranging and environmental perception algorithm. Compared to other localization methods, LiDAR also demonstrates good performance and scalability in terms of anti-interference capabilities and localization accuracy. Inertial measurement units (IMUs) calculate the target's position by measuring the acceleration and angular velocity of accelerometers and gyroscopes. Due to their independence from the external environment, they are often used in conjunction with lidar.

[0005] Currently, mainstream laser SLAM methods can be divided into two categories: filtering-based methods and optimization methods. In 2014, Zhang J et al. proposed a high-precision, low-computational-complexity SLAM algorithm. As a SLAM algorithm for 3D pure LiDAR, LOAM provides a highly scalable framework, laying an important foundation for subsequent algorithm research. The Lego-LOAM algorithm further improves upon LOAM, achieving lightweight and ground-optimized laser odometry. Wang et al. proposed F-LOAM, which employs a two-stage point cloud distortion correction method to reduce computational load, achieving more efficient radar odometry. Lin et al. designed a novel feature extraction and noisy point cloud filtering method, enabling deployment on lower-cost solid-state LiDAR systems.

[0006] For environments with degraded features, such as underground mine roadways and long corridors, existing research mostly improves positioning accuracy and mapping robustness by fusing IMU data, commonly referred to as LiDAR Inertial Odometry (LIO). The LIO-Mapping algorithm borrows ideas from LOAM and VINS-Mono, replacing the front-end vision component with LiDAR for feature matching and odometry pose estimation, while using state estimation in the back-end to achieve tight coupling between the IMU and LiDAR. Shan et al.'s LIO-SAM, building upon LOAM, uses factor graphs as a back-end optimization method, fusing LiDAR factors, IMU pre-integration factors, loop closure detection factors, and GNSS factors, and employing keyframes as the optimization target, thus reducing computational costs. The FAST-LIO algorithm proposed by the Mars Laboratory at the University of Hong Kong employs backpropagation of IMU-measured pose to eliminate scanning errors in point clouds generated during the movement of vehicles and other carriers. The backend utilizes Iterative Extended Kalman Filter (IESKF) to fuse IMU and point cloud measurements. A new Kalman gain calculation formula is introduced during the inversion and state vector calculation processes, significantly improving computational efficiency and achieving a high-precision LIO system. Some studies have optimized registration methods for SLAM algorithms in geometrically sparse and degraded environments. Dai et al. proposed an intensity-assisted LiDAR SLAM method that utilizes intensity information to enhance front-end odometry and back-end loop detection.

[0007] An analysis of existing research reveals the following issues that still require immediate attention: 1. Due to limited ventilation in mines, suspended dust is present for a long time, causing significant dust interference. This dust noise will be generated during lidar scanning, resulting in a decrease in point cloud registration accuracy.

[0008] 2. Existing laser SLAM algorithms cannot achieve high-precision positioning in degraded environments (such as mine roadways and tunnels). The weak geometric constraints of point clouds in degraded environments make it impossible to obtain accurate results in the point cloud registration stage.

[0009] 3. Existing algorithms exhibit severe Z-axis drift in underground locomotive operation scenarios, including slopes and areas with varying heights, and cannot effectively suppress the cumulative Z-axis error over long-term operation. Summary of the Invention

[0010] The purpose of this invention is to solve the problems in the existing technology where the point cloud registration accuracy decreases due to the influence of the mining environment, and the existing laser SLAM algorithm cannot achieve high-precision positioning in degraded environments, the point cloud registration stage cannot obtain accurate results, and the existing methods cannot effectively suppress the cumulative error of the Z-axis during long-term operation, all of which lead to a decrease in positioning accuracy. This invention provides a laser SLAM-based method and system for positioning motors in mines.

[0011] To achieve the above objectives, the present invention employs the following technical solution: To address the insufficient positioning accuracy of underground electric locomotives in environments characterized by high dust, low light, and degraded geometric features, this paper proposes an improved Simultaneous Localization and Mapping (SLAM) method based on a lidar IMU. This method introduces a dynamic voxel filtering strategy based on point cloud density and reflection intensity within the FAST-LIO2 framework. This effectively removes dust and environmental noise while preserving key geometric features and improving computational efficiency. To address the vertical cumulative error that is prone to occur in mining environments, this paper utilizes the RANSAC plane fitting method to extract ground points and construct ground constraints, thereby effectively suppressing long-term drift along the Z-axis. Considering the insufficient feature constraints in degraded environments such as roadways and tunnels, this paper further defines a degradation factor through Hessian matrix singular value decomposition and combines it with DBSCAN clustering to identify outlier features, dynamically adjusting the lidar observation covariance to achieve real-time detection and compensation for degraded scenarios.

[0012] Specifically, it includes: A laser SLAM-based method for locating underground motors in mines includes the following steps: Acquire point cloud data from underground lidar and IMU measurement data, remove motion distortion from the point cloud data based on the IMU measurement data, and obtain point cloud data after removing motion distortion. Based on the point cloud data after removing motion distortion, line and surface features are extracted, and point cloud matching is performed based on the line and surface features to obtain point cloud residuals; ground features of the mine are extracted, and the ground features are registered with the local map to construct ground constraints; during point cloud matching, degradation detection is performed to generate degradation compensation factors. The IMU measurement data is forward propagated to obtain the system state prediction; the state equation is constructed based on the cloud residual and ground constraint observations to obtain the observation covariance; and the observation covariance is compensated based on the degradation compensation factor. The optimal estimate of the state variables is obtained based on the compensated observation covariance and the predicted system state value, thus yielding the positioning result.

[0013] A further improvement of the present invention is that: The process of extracting line and surface features from point cloud data after removing motion distortion, performing point cloud matching based on the line and surface features, and obtaining point cloud residuals includes: Set a curvature threshold and calculate the curvature of the midpoint in each frame; Based on the curvature threshold and the calculated curvature of the points, the points are divided into line features and planar features, resulting in line feature point sets. and planar feature point set ; Based on the set of line feature points Combined with the global map, select the two closest points. and ,according to and The formula for the residual distance from a point to a line formed by two points is used to establish the distance from the point to the line. :

[0014] Based on the set of planar feature points Based on the global map, select the three closest points. Construct the corresponding plane and establish the distance residual from the point to the plane. :

[0015] in, Indicates the first Feature points in a frame Pose in the lidar coordinate system.

[0016] The extraction of surface features from the mine, registration of these features with a local map, and construction of surface constraints include: Get local map Based on the installation height of the lidar, in the local map Preliminary screening yielded a subset of the ground point cloud.

[0017] subset Fit the data to a plane and calculate the residual from the current frame to that plane; The plane with the most interior points is denoted as the ground point of the local map, and the ground point is converted into CP parameter form. ; Ground point set extracted based on height in the current frame Perform plane fitting to obtain the initial CP parameters of the ground points in the current frame. ; Based on the initial CP parameters of the ground points in the current frame The least squares optimization method is used to iteratively calculate and minimize the distance residuals from various surface points to the plane in the current frame. According to the distance residual Optimize and update the planar parameters of the current frame to obtain the updated ground plane; The updated ground plane and the ground plane of the local map are uniformly converted to the world coordinate system, the difference in plane parameters between the two are calculated, and ground constraints are constructed based on the difference in plane parameters.

[0018] The process of performing degradation detection and generating degradation compensation factors during point cloud matching includes: During the point cloud matching process, the corresponding Hessian matrix is ​​calculated based on the point cloud residuals.

[0019] Hessian matrix according to and The relationships between them are divided into four sub-matrices. It contains only information related to rotation. It contains only information related to translation. and They represent and Information about the interactions between them; right and Perform singular value decomposition:

[0020] In the formula: sorting the eigenvalues ​​of the Hessian matrix. Let the rotational degradation factor and the translation degradation factor be defined as follows:

[0021] Among them, the degradation factor of translation is used to describe the linear characteristic covariance. The rotational degradation factor is used to describe the surface feature covariance. ; Outlier detection is performed using a clustering algorithm with rotational and translational degradation factors to determine the system's degradation state.

[0022] The step of forward propagating IMU measurement data to obtain system state prediction values ​​includes: Construct a continuous-time motion model based on IMU measurement data:

[0023] In the formula: and The pose of the IMU in the world coordinate system. For speed, the default value in the formula is taken as... This indicates the transformation from the IMU coordinate system to the world coordinate system; and Measurement noise conforming to a Gaussian distribution. and For IMU zero bias, and Modeled as zero-biased Gaussian white noise, It is the acceleration due to gravity. For IMU measurements, Indicates an antisymmetric matrix; Define the nominal state variables and error variables of the system:

[0024]

[0025]

[0026] In the formula: Defined as the manifold in which the system resides. It is an 18-dimensional error state vector; The motion model is discretized and forward propagated to obtain the predicted values ​​of the nominal state variables:

[0027] In the formula: the noise term is not considered during propagation of the nominal state variable, but is considered in the error state; therefore, the noise term is zero. Used to compensate for point cloud motion distortion; The error state is linearized to obtain the error state equation and covariance matrix. Forward propagation is then performed based on the error state equation and covariance matrix to obtain the predicted value.

[0028]

[0029]

[0030]

[0031] in, Representing the noise matrix The covariance matrix; Represent the covariance matrix; and These represent the predicted value and the predicted covariance of the error state, respectively.

[0032] The state equation is constructed based on cloud residuals and ground-constrained observations to obtain the observation covariance, including: Construct observation models for line and surface features:

[0033]

[0034]

[0035] in:

[0036] A ground point cloud observation model is constructed by incorporating ground constraints:

[0037] in:

[0038] In the formula: Defined as Measurement noise, and For the first The first frame Individual surface features and line feature points; for Real-time status The current best estimate; Represents the true state and pose of feature points; Representation: Jacobian matrix of linear characteristic residuals; The Jacobian matrix representing the surface feature residuals; The Jacobian matrix representing the ground residuals; This represents the ground plane CP parameters of the current frame; This represents the ground plane CP parameter of a local map; This represents the ground plane residual from the current frame to the local map.

[0039] The method involves compensating the observation covariance based on a degradation compensation factor, obtaining the optimal estimate of the state variables based on the compensated observation covariance and the system state prediction value, and thus obtaining the positioning result, including: Update the state variables of the prediction process:

[0040]

[0041] In the formula: These are the covariance matrices of the line-surface feature residuals and the ground feature residuals, respectively. Represents the Kalman filter Jacobian matrix; This represents the new observation residual; Represents the new covariance matrix

[0042] Introducing a compensation factor to correct the covariance matrix:

[0043] In the formula: These are the compensation coefficients for the covariance matrix; Iterative Kalman filter gain:

[0044]

[0045] In the formula: Let be the posterior covariance of the Kalman filter; The posterior estimate of the error state is obtained:

[0046]

[0047] When satisfied When the convergence condition is met, the iteration is complete, and the optimal estimate of the state variables is obtained. The covariance matrix has been updated.

[0048] Based on optimal estimation , the current frame point cloud Through the optimized pose Transform to world coordinate system to obtain This will generate an updated global map.

[0049] A laser SLAM-based underground motor positioning system for mines includes: The point cloud data processing module is used to acquire point cloud data from the underground lidar and IMU measurement data, and to remove motion distortion from the point cloud data based on the IMU measurement data to obtain point cloud data after removing motion distortion. The point cloud matching module is used to extract line and surface features from point cloud data after removing motion distortion, perform point cloud matching based on line and surface features, and obtain point cloud residuals; extract ground features of the mine, register the ground features with the local map, and construct ground constraints; and perform degradation detection during point cloud matching to generate degradation compensation factors. The prediction and compensation module is used to forward propagate IMU measurement data to obtain system state prediction values; construct state equations based on cloud residuals and ground constraint observations to obtain observation covariance; and compensate for the observation covariance based on degradation compensation factors. The positioning module is used to obtain the optimal estimate of the state variables based on the compensated observation covariance and the predicted system state value, thus obtaining the positioning result.

[0050] A terminal device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of any of the methods described in this invention.

[0051] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of any of the methods described in this invention.

[0052] Compared with the prior art, the present invention has the following beneficial effects: This invention discloses a laser SLAM-based method for motor localization in mines. Addressing point cloud noise caused by high dust environments, it removes motion distortion from point cloud data based on IMU measurement data, effectively eliminating dust noise. To address the Z-axis drift problem of existing algorithms, it constructs ground constraints and establishes a stable ground height benchmark, effectively suppressing height errors. In geometrically degraded environments such as tunnels, it performs degradation detection, generates degradation compensation factors, and dynamically adjusts the laser radar observation covariance to compensate for insufficient constraints. This method overcomes the limitations of visual SLAM in environmental conditions, better meets the positioning needs of mines, and improves the accuracy of pose estimation in underground mine environments. Attached Figure Description

[0053] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0054] Figure 1 This is a flowchart of the method disclosed in an embodiment of the present invention; Figure 2 This is a schematic diagram of dynamic voxel subdivision disclosed in an embodiment of the present invention; Figure 3 This is a schematic diagram of voxel downsampling disclosed in an embodiment of the present invention; Figure 4 The above are the overall point cloud trajectory diagrams of the three algorithms for the UGV1 sequence disclosed in this embodiment of the invention (where a represents the overall point cloud trajectory diagram of the FAST-LIO2 algorithm; b represents the overall point cloud trajectory diagram of the FAST-LIO algorithm; and c represents the overall point cloud trajectory diagram of the Ours algorithm). Figure 5 This is a magnified view of region A1 disclosed in an embodiment of the present invention (where a represents a magnified view of the FAST-LIO2 algorithm; b represents a magnified view of the FAST-LIO algorithm; and c represents a magnified view of Ours). Figure 6 This is a magnified view of region A2 disclosed in an embodiment of the present invention (where a represents a magnified view of the FAST-LIO2 algorithm; b represents a magnified view of the FAST-LIO algorithm; and c represents a magnified view of the Ours algorithm). Figure 7These are the overall point cloud trajectory diagrams of the three UGV3 sequence algorithms disclosed in this embodiment of the invention. (Where a represents the overall point cloud trajectory diagram of the FAST-LIO2 algorithm; b represents the overall point cloud trajectory diagram of the FAST-LIO algorithm; and c represents the overall point cloud trajectory diagram of the Ours algorithm.) Figure 8 This is a magnified view of region B1 disclosed in an embodiment of the present invention (where a represents a magnified view of the FAST-LIO2 algorithm; b represents a magnified view of the FAST-LIO algorithm; and c represents a magnified view of the Ours algorithm). Figure 9 This is a magnified view of region B2 disclosed in an embodiment of the present invention (where a represents a magnified view of the FAST-LIO2 algorithm; b represents a magnified view of the FAST-LIO algorithm; and c represents a magnified view of the Ours algorithm). Figure 10 This is a height point cloud analysis diagram of UGV1 disclosed in an embodiment of the present invention (where a represents the FAST-LIO2 algorithm analysis diagram; b represents the FAST-LIO algorithm analysis diagram; and c represents the Ours algorithm analysis diagram). Figure 11 This invention discloses a UGV3 height point cloud analysis method (where a represents the FAST-LIO2 algorithm analysis diagram; b represents the FAST-LIO algorithm analysis diagram; and c represents the Ours algorithm analysis diagram). Figure 12 This is an overall trajectory diagram of the three algorithms for UGV1 sequence disclosed in this embodiment of the invention; Figure 13 The XYZ axis trajectory diagrams of the three algorithms for UGV1 sequence disclosed in this embodiment of the invention; Figure 14 This is an overall trajectory diagram of the three algorithms for UGV3 sequence disclosed in this embodiment of the invention; Figure 15 The image shows the XYZ axis trajectory diagrams of the three algorithms for UGV3 sequence disclosed in this embodiment of the invention. Detailed Implementation

[0055] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0056] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0057] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0058] In the description of the embodiments of the present invention, it should be noted that if terms such as "upper," "lower," "horizontal," or "inner" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship commonly used when the product of the invention is in use, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention. Furthermore, terms such as "first" and "second" are only used to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0059] Furthermore, the use of the term "horizontal" does not imply that the component must be absolutely horizontal, but rather that it can be slightly tilted. For example, "horizontal" simply means that its direction is more horizontal than "vertical," and does not mean that the structure must be completely horizontal, but can be slightly tilted.

[0060] In the description of the embodiments of the present invention, it should also be noted that, unless otherwise explicitly specified and limited, the terms "set," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in the present invention according to the specific circumstances.

[0061] The present invention will now be described in further detail with reference to the accompanying drawings: See Figure 1This embodiment discloses a laser SLAM-based method for locating underground motors in mines. The algorithm disclosed in this embodiment is based on the FAST-LIO2 algorithm. The system receives data from lidar and IMU and provides real-time 6-DoF (six degrees of freedom) attitude estimation. First, to address the noise interference in the point cloud caused by high dust levels in the mine, a dynamic voxel filtering downsampling method based on density and intensity is used in the preprocessing stage to remove outliers from the point cloud. Next, distortion correction is performed on the received point cloud data based on IMU information. Then, feature extraction and constraint construction are performed, and line and surface features are extracted based on the smoothness of points. Simultaneously, to suppress Z-axis drift, ground points in the current frame and local map are extracted and fitted to construct a ground plane residual function. Subsequently, to address the accuracy degradation problem caused by the feature degradation of underground roadways, this embodiment separates the rotation and translation-related matrices in the Hessian matrix of the feature matching residual, uses the DBSCAN algorithm to detect outliers and confirm the degradation direction, and compensates for insufficient constraints by adjusting the lidar observation covariance. Finally, the residual is input into the error state iterative Kalman filter to estimate the optimal state. After iterating until the error state converges, the point cloud map is updated, and a globally consistent point cloud map is output at a frequency of 10Hz.

[0062] Specifically, the following steps are included: Step 1: Acquire point cloud data from the underground lidar and IMU measurement data. Perform motion distortion removal on the point cloud data based on the IMU measurement data to obtain the point cloud data after motion distortion removal. Specifically, the following steps are included: Step 1.1: Point Cloud Preprocessing Step 1.1.1 Intensity and Density-Based Voxel Mesh Filtering After acquiring a new point cloud frame, the point cloud is usually in a messy and disordered state. If lidar is applied to an underground mining environment, the high dust levels result in a large amount of noise in the original point cloud, severely affecting the accuracy of point cloud registration. Furthermore, downsampling the original point cloud removes noise while preserving key geometric features, significantly reducing redundant information, improving the overall computational efficiency of the algorithm, and making it more suitable for the limited computing resources of lidar deployed on locomotives. To address the unique characteristics of point clouds in underground mines, this embodiment proposes a dynamic voxel filtering algorithm based on intensity and density. The algorithm flow is as follows: (1) Initialize voxel partitioning: The input point cloud is divided into meshes according to the initial voxel size, and the point cloud is determined along... Within the boundary along the axial direction, The maximum and minimum values ​​of the axis side length are respectively used and This means that the side length of the initial voxel block can be changed from... express: (1) (2) Unit voxel division: The initial voxel block is divided into unit voxel blocks. The axes are used to calculate the number of unit voxel blocks to be divided, and the three axes are then divided into equal parts. Partial. The three directions are divided as follows: (2) In the formula: Indicates rounding down. This represents the side length of the unit voxel mesh. It ensures that the point cloud space is completely and non-overlappingly divided into discrete unit voxel blocks, providing the basic mesh units for subsequent dust and noise removal and dynamic voxel subdivision.

[0063] (3) Dust and noise removal: For each voxel Calculate the average reflection intensity and point density of the point cloud: (3) (4) In the formula: Voxel blocks The average reflection intensity, express Point density, This indicates the number of point clouds within a voxel. express The first The reflection intensity at each point Voxel blocks The volume. After obtaining the strength and density, a judgment is made if the following conditions are met. and These are considered dust and noise and are discarded. and This is a preset threshold.

[0064] (4) Dynamic voxel grid subdivision: Based on the point density and average intensity of each voxel, voxels are subdivided into voxel blocks of different sizes. High-density regions typically contain important geometric features and require finer processing using smaller voxels; low-density regions are mainly empty spaces, allowing for the use of larger voxels to reduce computation; medium-density regions can maintain the original voxel size to balance accuracy and efficiency. The density variation coefficient effectively reflects the uniformity of intensity distribution within a voxel. A large variation coefficient indicates significant intensity variations in the region, potentially containing important boundary features that require finer processing, such as... Figure 2 As shown.

[0065] (5) Voxel downsampling: Calculate the centroid point within each voxel block, retain the centroid point, and discard the remaining points within the voxel block. If the centroid point does not exist, replace all points in the voxel with the point cloud closest to the centroid point. This embodiment stores the downsampled centroid point set, the principle of which is as follows: Figure 3 As shown.

[0066] (6) Edge feature preservation: This embodiment detects the normal vector of a point cloud to identify its edges. Assuming a point cloud set... A point cloud Its normal vector is , around the point The set of adjacent points is composed of This indicates that the point cloud... Covariance of occupied voxel grid It can be calculated as shown in the formula: (5) In the formula: Point The mean of the 3D point cloud coordinates of the surrounding point set. The number of surrounding points, points The normal vector is given by the following equation: (6) In the formula: Represents the covariance matrix The eigenvalues ​​are obtained by eigenvalues, resulting in three eigenvalues. The smallest eigenvalue The corresponding eigenvectors are defined as points. normal vector Similarly, we obtain the neighboring points. normal vector To quantify the consistency of the normal vector direction between a target point and its neighboring points, and to determine whether the point is located in an edge region, an eigenvalue of the angle between the normal vectors is defined. Represented as: (7) In the formula: The dot product of the normal vectors of the target point and its neighboring points. and These represent the magnitudes of the target point and its neighboring points, respectively. In flat regions, the angle between the normal vectors is very small; in completely horizontal regions, this angle may even be zero. However, in edge or angular regions, the angle between the normal vectors is relatively large. According to... The value is used to determine whether a point is an edge point. The detected edge point set is combined with the downsampled centroid point set to create a new point cloud, resulting in the final downsampled point cloud. Step 1.1.2: Point Cloud Distortion Removal LiDAR operates by acquiring and processing point clouds frame by frame, with all points within a single frame considered to have been acquired at the same time and in the same coordinate system. However, when the LiDAR is deployed on an electric locomotive, the locomotive's movement causes motion distortion to the fixed LiDAR. Therefore, it is necessary to combine the IMU's measurement information to calculate the rotation matrix of each point relative to the starting point, projecting the entire point cloud to the point cloud's ending time to obtain the point cloud information after removing motion distortion. In this embodiment, it is assumed that the LiDAR and IMU have been synchronized and calibrated in time and space. The world coordinate system is defined as... The lidar coordinate system is The IMU coordinate system is The carrier coordinate system is considered to be the same as the IMU coordinate system. The extrinsic parameter transformation matrix between the lidar coordinate system and the IMU coordinate system is expressed as: (8) In the formula: This represents the coordinate transformation matrix from the lidar coordinate system to the inertial coordinate system. This embodiment has the following relational expression. Typically, the sampling frequency of an IMU is much higher than that of a lidar. In this embodiment, based on the start time of the current lidar frame, the two closest IMU data points before and after that timestamp are selected for interpolation. Similarly, the IMU data at the end time of the current frame can be obtained. The IMU pose of the point cloud in the current frame is calculated by integrating all IMU data within a lidar frame.

[0067] (9) In the formula: This represents the start time of the lidar frame. The end time, express The transformation matrix between the coordinate system at time and the lidar coordinate system. This indicates the pose transformation in the IMU coordinate system during the current frame.

[0068] Calculate the coordinate transformation matrix of each point in the lidar point cloud, and transform all points to the coordinate system at the end of the lidar frame, thereby completing the motion distortion removal of the point cloud.

[0069] Step 2: Based on the point cloud data after removing motion distortion, extract line features and surface features, perform point cloud matching based on line features and surface features, and obtain point cloud residuals; extract the ground features of the mine, register the ground features with the local map, and construct ground constraints; perform degradation detection during point cloud matching and generate degradation compensation factors; Specifically, the following steps are included: Step 2.1: Obtain point cloud residuals After obtaining reliable point cloud data, point cloud matching is performed using a scan-to-map approach. Following the LOAM method, line and surface features are extracted for each frame based on curvature. To ensure uniform distribution of feature points, each frame's point cloud is divided into six sub-regions, and the curvature of points in each region is calculated. The first sub-region is defined as... The first frame The points are represented in the lidar coordinate system by Therefore, the curvature is calculated as follows: (10) In the formula: This represents the set of point clouds in the target point sub-region. and Style Point and points Pose in the lidar coordinate system.

[0070] Furthermore, based on the preset curvature threshold At non-ground points, Points with high smoothness values ​​are selected as line feature points and added to the line feature point set. . Points with low smoothness values ​​are selected as planar feature points and added to the planar feature point set. .

[0071] Furthermore, based on the obtained line feature points It can search for two nearest points on the global map. and ,according to and The formula for the residual distance from a point to a line formed by two points is used to establish the distance from the point to the line. Corresponding opposite features This embodiment can search for three nearest points. To construct the corresponding plane, the distance residual from the point to the plane is calculated. To obtain line features Kneading characteristics The distance residual is expressed as: (11) (12) Step 2.2: Constructing Ground Constraints In structured environments such as mine tunnels, the ground surface, as the most stable geometric feature, provides reliable prior information for solving the drift problem of Z-axis position and pitch angle during locomotive operation. Based on the extraction of ground features, this embodiment chooses to use the HF method to represent the plane equation and convert it into CP parameters to extract ground points. Assume there is a plane in a frame acquired by lidar, and its HF parameters are... ,point If it lies on this plane, then the following relationship is satisfied: (13) In the formula: It is a plane normal vector. This is the distance from the origin of the lidar coordinate system to this plane. Based on the above formula, the parameters can be derived in this embodiment. and Expression in IMU coordinate system: (14) Converted to CP parameters: (15) After obtaining the parameterization of the ground point cloud, this embodiment adopts a scan-to-map strategy to construct stable ground constraints: First, accumulate a local map of a certain number of frames. Preliminary screening, based on the installation height of the lidar, initially identifies a subset that may belong to the ground point cloud. ; Furthermore, by applying Random Sample Consensus (RANSAC) iterative fitting to a plane to the selected ground points, this method can estimate the most likely ground plane from data containing interference points such as tracks and debris, and calculate the residual from the current frame to that plane.

[0072] Furthermore, after the RANSAC algorithm completes its iterations, the plane with the most interior points is denoted as the ground point of the local map, and converted into CP parameter form according to equation (14). .

[0073] Furthermore, after obtaining the distortion-free current frame point cloud... Afterwards, take measures and localized approaches. Figure 1 This strategy first yields the filtered set of ground points. Define the HF parameters of the ground plane in the current frame as follows: ,for a point in Satisfying Relationship: (16) To obtain stable ground parameters, the RANSAC algorithm is used to perform plane fitting on the ground points to obtain the initial CP parameters of the ground points in the current frame. The plane fitted by RANSAC may contain biases. A least-squares optimization problem is established to minimize the distance residuals from ground points to the plane. : (17) (18) (19) (20) In the formula: the Gauss-Newton method is used for iterative solution. The number of ground point sets, The linearization point in the iteration step, The information matrix of the residuals is defined as the inverse of the covariance matrix. Given the Jacobian matrix, the increment is calculated iteratively. Update the CP parameters of the current frame. The corresponding covariance matrix is .

[0074] After obtaining the stable ground planes of the current frame and the local map, the ground planes of the current frame and the local map are uniformly transformed to the world coordinate system to construct the residual formula: (twenty one) (twenty two) In the formula: This indicates the transformation from the lidar coordinate system to the world coordinate system. This is the ground plane residual from the current frame to the local map. The calculated residual function will be used in the filter's observations as a ground constraint to limit Z-axis drift. Step 2.3: Degradation Detection When lidar is used in underground mine tunnels, the high similarity and lack of geometric features in this environment leads to multiple possible solutions during point cloud matching, thus affecting the accuracy of pose estimation. Typically, degradation occurs in the translational and rotational (roll) directions along the X-axis, resulting in… Insufficient directional constraints make it difficult to obtain accurate estimates. The Hessian matrix can be approximated using formulas (33) and (34) as follows: (36) Hessian matrix according to and The relationships between them are divided into four sub-matrices. It contains only information related to rotation. It contains only information related to translation. and They represent and Information on the interactions between them. Due to the difference in scale between rotation and translation, this embodiment only considers... and Perform SVD (Singular Value Decomposition): (37) In the formula: sorting the eigenvalues ​​of the Hessian matrix. In this embodiment, the degradation factors for rotation and translation are defined as follows: (38) The degradation factor, defined as the ratio of the maximum to the minimum eigenvalues ​​of the Hessian matrix, is used to measure whether a lidar system degrades in an underground mining environment. A smaller degradation factor indicates a stable system, while a degradation factor exceeding a threshold indicates poor observability of rotations or translations. The covariance of lidar observations is increased in the Kalman filter IESKF to compensate for degradation. When degradation occurs due to a lack of sufficient geometric information in the environment, the degraded Hessian matrix eigenvalues ​​are outliers relative to normal eigenvalues. The DBSCAN clustering algorithm is used to detect outliers, thus determining whether degradation has occurred.

[0075] Step 3: Forward propagate the IMU measurement data to obtain the system state prediction; construct the state equation based on the cloud residual and ground constraint observations to obtain the observation covariance; and compensate the observation covariance based on the degradation compensation factor. Specifically, the following steps are included: Step 3.1: Forward Propagation Define symbols: Represents the nominal state variable. Indicates the error status. Represents the truth value of the state variable. This represents the prior estimate obtained during the filter prediction process. This represents the posterior estimate obtained after filter update, from which the continuous-time motion model is derived using IMU integration: (twenty three) In the formula: and The pose of the IMU in the world coordinate system. For speed, the default value in the formula is taken as... This indicates the transformation from the IMU coordinate system to the world coordinate system. and Measurement noise conforming to a Gaussian distribution. and For IMU zero bias, and Modeled as zero-biased Gaussian white noise, It is the acceleration due to gravity. For IMU measurements, This represents an antisymmetric matrix. The nominal state variables and error variables of the system are defined as follows: (twenty four) (25) (26) In the formula: Defined as the manifold in which the system resides. The error state vector is 18-dimensional. The motion model is discretized and forward propagated to obtain the predicted values ​​of the nominal state variables:

[0076] (27) In the formula: the noise term is not considered during the propagation of the nominal state variable, but is considered in the error state, so the noise term is zero. Used to compensate for cloud motion distortion at node 2.1.2. The error state is linearized to obtain the error state equation and covariance matrix, which are then used for forward propagation as follows: (28) (29) (30) (31) In the formula: Representing the noise matrix The covariance matrix, Represents the covariance matrix. and The predicted value (i.e., prior estimate) represents the error state and its predicted covariance.

[0077] Step 3.2: Iteration of the observation equation definition and For the first The first frame Individual surface features and line feature points, for Real-time status The current best estimate then determines the true state and pose of the feature point. This will make the residual of feature matching zero.

[24] The observation model for line features and surface features is expressed as follows: (32) (33) (34) in:

[0078] In the formula: Defined as Measurement noise, Based on the residual formula obtained in step 2, the ground point cloud observation model is as follows: (35) in:

[0079] Furthermore, in this embodiment, the state variables of the prediction process are updated based on the observation results. Combining equations (33), (34), and (35), this embodiment can define a new Kalman filter Jacobian matrix. , observation residual and its covariance matrix : (36) (40) (41) In the formula: These are the covariance matrices of the line surface feature residuals and the ground feature residuals, respectively. In step 2, the degradation scenario was identified. To suppress the impact of observations on filter updates under degradation conditions, this embodiment introduces a compensation factor to correct the covariance matrix:

[0080] In the formula: is the compensation coefficient of the covariance matrix. Its magnitude is inversely proportional to the degradation factor. When the degradation factor decreases, it indicates that the observation constraints in the current direction are insufficient, and the corresponding coefficient increases, thereby reducing the weight of the residual in the state update. When the degradation factor is within the normal range, the compensation coefficient approaches 1.

[0081] The corrected covariance matrix is ​​used for subsequent state updates of the Kalman filter, improving robustness in degenerate environments. It should be noted that different types of geometric features impose different constraints on degrees of freedom; this embodiment uses the translation degradation factor and the line feature covariance... Correlate the degradation factor of rotation with the surface feature covariance. Correlated, and when the surface points in the mine are relatively stable, their covariance usually remains unchanged. The following assumes the current iteration number is... Based on the definition of the SMW identity, the iterative Kalman filter gain can be simplified to: (42) (43) In the formula: Given the posterior covariance of the Kalman filter, this embodiment can finally obtain the posterior estimate of the error state: (44) (45) By iterating the above process continuously until the condition is met... The convergence condition is met, the system completes the iterative process, and obtains the optimal estimate of the state variables. Then update the covariance matrix:

[0082] Step 4: Based on the compensated observation covariance and the system state prediction value, obtain the optimal estimate of the state variables and get the positioning result. Obtain the optimal estimate Then, the current frame point cloud Through the optimized pose Transform to world coordinate system to obtain .

[0083] To maintain the effective range of the local map, this embodiment adopts the dynamic map management based on incremental kd-tree proposed by FAST-LIO2. This method uses a cube-box deletion strategy centered on the current location to achieve efficient insertion, deletion and nearest neighbor search of point clouds, while maintaining the balance of the tree structure, and can achieve an odometry output frequency of 10Hz.

[0084] This embodiment also discloses the verification of this method: (1) Qualitative analysis 1.1 Qualitative Analysis of Overall Point Cloud Trajectory The UGV1 dataset represents a typical environment with simple features, such as tunnel walls, which can easily lead to insufficient feature constraints and feature degradation. For example... Figure 4 As shown, the point cloud map displays the localization and map mapping results of Ours, FAST-LIO2, and FAST-LIO algorithms. It can be seen that the point cloud map constructed by the method in this embodiment clearly and accurately reflects the actual situation of the tunnel environment, with clear tunnel wall features and no obvious ghosting. Compared to the other two algorithms, the trajectory is more stable, with no obvious drift, and the global map continuity is superior. Figure 5 and Figure 6 The image shows a magnified view of a portion of the UGV1 sequence. Regions A1 and A2 are typical degraded scenarios, prone to matching failures. The SLAM algorithm incorrectly assumes the carrier hasn't moved, causing its mapped length to be shorter than the actual trajectory. Furthermore, the effectiveness of the dynamic voxel filtering proposed in this embodiment is evident; the resulting point cloud map reduces point density while preserving key features.

[0085] Figure 7 The results of running the UGV3 dataset are shown. This sequence simulates a more complex underground mine environment, including multiple forks, irregular turning areas, and significant height changes, which poses a greater challenge to the SLAM algorithm. Figure 8 and Figure 9 A magnified view of a portion of the UGV3 sequence is shown. In region B2, FAST-LIO2's loop is closed due to accumulated errors after traveling a long distance. The overall point cloud map of FAST-LIO is similar to Ours, but the trajectory still shows a slight offset, drifting in the X-axis direction in region B1. The point cloud map generated by the method in this embodiment can intuitively reflect the actual situation of the underground mine roadway environment, exhibiting good robustness and accuracy.

[0086] 1.2 Qualitative Analysis of Altitude Point Cloud Trajectory See Figure 10 and Figure 11 The data shows the point cloud height variations for the UGV1 and UGV3 sequences. In UGV1, FAST-LIO2 exhibits severe Z-axis drift, causing the overall point cloud height to be below the ground truth. While FAST-LIO2 mitigates this to some extent, drift still exists, failing to converge the Z-axis position in regions with rapid height changes, resulting in an excessively thick overall point cloud. Ours' method uses ground constraints to optimize the Z-axis error, clearly defining the boundary between the top and ground, and exhibiting no drift. In UGV3, FAST-LIO2 shows Z-axis drift in the first half of the dataset, causing the overall point cloud to tilt and resulting in layered misalignment of the closed loop. In subsequent runs, insufficient Z-axis constraints lead to excessive errors, with the ground position deviating significantly from the ground truth. Ours maintains a stable height benchmark through ground constraints, making it suitable for complex mining environments with slopes and height variations.

[0087] (2) Quantitative analysis 2.1 Overall Trajectory Analysis To quantitatively evaluate the accuracy of the algorithm in this embodiment, the trajectory lengths and their deviations obtained from Ours, FAST-LIO2, and FASTER-LIO were analyzed using the EVO tool. EVO is a Python package specifically designed for evaluating SLAM algorithms, used to process, evaluate, and compare trajectories from SLAM algorithms. Figure 12 and Figure 13This paper demonstrates the overall localization trajectory and the variation trends of the X, Y, and Z axes in the UGV1 dataset. This embodiment employs height constraints and degradation compensation to obtain a more accurate localization trajectory than FAST-LIO2 and FAST-LIO. On the X and Y axes, the other two algorithms exhibit degradation due to the alleyway environment, while Ours uses degradation detection and compensation to maintain higher accuracy compared to the other two algorithms. On the Z-axis trajectory, FAST-LIO shows significant Z-axis drift from approximately 100s to 480s, deviating considerably from the ground truth. FAST-LIO2's Z-axis drift occurs from approximately 650s to 1250s, resulting in significant errors during this period. This embodiment is closer to the ground truth, but still contains some errors. This is because in some scenes within the UGV1 and UGV3 sequences, sufficient ground points cannot be obtained to fit the ground plane. Figure 14 and Figure 15 For the overall and three-axis trajectories of UGV3, it is clear that the algorithm proposed in this embodiment consistently outperforms FAST-LIO2 and FASTER-LIO on the Z-axis. Furthermore, in UGV3, the error of this algorithm is consistently less than 1m. The other two algorithms exhibit significant errors on the Z-axis due to the limited scanning range on both sides of the vehicle in the scene and the lack of height constraints. Regarding the X and Y axis trajectories, the FAST-LIO2 algorithm shows significant cumulative error in the later stages of the sequence, particularly a substantial shift within the 1000s-1500s timeframe. While the FASTER-LIO algorithm improves overall accuracy, it still suffers from insufficient positioning accuracy in the height trajectory. The method in this embodiment, through the effective use of ground constraint mechanisms and ground plane fitting, provides a reliable height reference for the system, thus improving the error in the Z-axis direction. (2) Pose estimation accuracy analysis Table 1 shows the errors between the total trajectory length of the carrier obtained by the three algorithms and the actual trajectory length. The trajectory length obtained by Ours is about 52.8% and 87.7% less than that of FAST-LIO2, and about 60.0% and 68.6% less than that of FAST-LIO. The algorithm in this embodiment is better than the other two algorithms and is more consistent with the actual trajectory length.

[0088] Table 1. Analysis of the total trajectory length of the three algorithms

[0089] The experimental results of APE values ​​are shown in Tables 2 and 3. To ensure the reliability of the data in the tables, each experiment was repeated 5 times in this embodiment, and the average value was used as the data for comparative analysis. Based on the error statistics obtained by comparing with the ground truth, the algorithm proposed in this embodiment improves the error of both algorithms on both sequences, especially in areas with high frequency of height changes, where the algorithm can achieve smaller errors. The algorithm in this embodiment adds dynamic voxel downsampling, ground constraints, and degradation compensation to the FAST-LIO2 algorithm, resulting in a significant improvement in trajectory accuracy. The root mean square error (RMSE) is improved by 61.24% and 51.43% on UGV1 and UGV3, respectively. Compared with FAST-LIO, the RMSE is also improved by 45.65% and 20.31% in the two sequences, respectively, verifying the improvement of the accuracy of pose estimation in the underground mine environment by the method in this embodiment.

[0090] Table 2 Comparison of absolute pose errors of three algorithms for UGV1 sequences

[0091] Table 3. Comparison of absolute pose errors of three algorithms for UGV3 sequences

[0092] This research aims to solve the problem of reliable positioning of electric locomotives in underground mines, and proposes an improved method for positioning in the underground environment. First, to address the point cloud noise caused by high dust environments, a dynamic voxel filtering based on density and intensity is proposed to effectively remove dust noise. Second, to address the Z-axis drift problem of existing algorithms, a stable ground height benchmark is constructed through RANSAC plane fitting and scan-to-map registration strategies, effectively suppressing height errors. Finally, in geometrically degraded environments such as tunnels, rotation and translation degradation factors are calculated through Hessian matrix SVD decomposition, combined with DBSCAN clustering to identify outlier features, and the covariance of lidar observations is dynamically adjusted to compensate for insufficient constraints. The proposed method is validated through an embodiment based on a publicly available underground mine dataset. Compared to mainstream lidar SLAM algorithms such as FAST-LIO2 and FASTER-LIO, this method achieves superior positioning accuracy in mine scenes. The RMSE of UGV1 and UGV3 sequences is reduced by 61% and 51% respectively compared to the original algorithm. Furthermore, it also shows improved trajectory stability and ground... Figure 1Significant improvements were also observed in consistency, validating the effectiveness of the improved method. Compared to existing positioning technologies that rely on external base stations, the laser SLAM algorithm proposed in this embodiment eliminates the need for base station installation and debugging, reducing equipment costs and maintenance difficulty. Furthermore, it overcomes the environmental limitations of visual SLAM, making it more suitable for positioning needs in mines. However, the research still faces challenges such as small Z-axis errors in sparse ground point scenarios and interference from dynamic objects. Future research will focus on optimizing the ground point extraction algorithm and removing dynamic targets, as well as exploring multi-sensor fusion to achieve high-precision positioning.

[0093] A schematic diagram of a terminal device according to an embodiment of the present invention. The terminal device of this embodiment includes: a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps in the various method embodiments described above. Alternatively, when the processor executes the computer program, it implements the functions of each module / unit in the various device embodiments described above.

[0094] The computer program can be divided into one or more modules / units, which are stored in the memory and executed by the processor to complete the present invention.

[0095] The terminal device can be a desktop computer, laptop computer, cloud server, or other device with strong computing power. The terminal device may include, but is not limited to, a processor and memory.

[0096] The optimal choice for the processor is a multi-core high-speed central processing unit (CPU).

[0097] The memory can be used to store the computer program and / or module. The processor implements various functions of the terminal device by running or executing the computer program and / or module stored in the memory and calling the data stored in the memory.

[0098] If the modules / units integrated into the terminal device are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0099] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A laser SLAM-based method for locating underground motors in mines, characterized in that, Includes the following steps: Acquire point cloud data from underground lidar and IMU measurement data, remove motion distortion from the point cloud data based on the IMU measurement data, and obtain point cloud data after removing motion distortion. Based on the point cloud data after removing motion distortion, line and surface features are extracted, and point cloud matching is performed based on the line and surface features to obtain point cloud residuals; ground features of the mine are extracted, and the ground features are registered with the local map to construct ground constraints; during point cloud matching, degradation detection is performed to generate degradation compensation factors. The IMU measurement data is forward propagated to obtain the system state prediction; the state equation is constructed based on the cloud residual and ground constraint observations to obtain the observation covariance; and the observation covariance is compensated based on the degradation compensation factor. The optimal estimate of the state variables is obtained based on the compensated observation covariance and the predicted system state value, thus yielding the positioning result.

2. The method for locating underground motors in mines based on laser SLAM according to claim 1, characterized in that, The process of extracting line and surface features from point cloud data after removing motion distortion, performing point cloud matching based on the line and surface features, and obtaining point cloud residuals includes: Set a curvature threshold and calculate the curvature of the midpoint in each frame; Based on the curvature threshold and the calculated curvature of the points, the points are divided into line features and planar features, resulting in line feature point sets. and planar feature point set ; Based on the set of line feature points Combined with the global map, select the two closest points. and ,according to and The formula for the residual distance from a point to a line formed by two points is used to establish the distance from the point to the line. : Based on the set of planar feature points Based on the global map, select the three closest points. Construct the corresponding plane and establish the distance residual from the point to the plane. : in, Indicates the first Feature points in a frame Pose in the lidar coordinate system.

3. The method for locating underground motors in mines based on laser SLAM according to claim 1, characterized in that, The extraction of surface features from the mine, registration of these features with a local map, and construction of surface constraints include: Get local map Based on the installation height of the lidar, in the local map Preliminary screening yielded a subset of the ground point cloud. subset Fit the data to a plane and calculate the residual from the current frame to that plane; The plane with the most interior points is denoted as the ground point of the local map, and the ground point is converted into CP parameter form. ; Ground point set extracted based on height in the current frame Perform plane fitting to obtain the initial CP parameters of the ground points in the current frame. ; Based on the initial CP parameters of the ground points in the current frame The least squares optimization method is used to iteratively calculate and minimize the distance residuals from various surface points to the plane in the current frame. According to the distance residual Optimize and update the planar parameters of the current frame to obtain the updated ground plane; The updated ground plane and the ground plane of the local map are uniformly converted to the world coordinate system, the difference in plane parameters between the two are calculated, and ground constraints are constructed based on the difference in plane parameters.

4. The method for locating underground motors in mines based on laser SLAM according to claim 1, characterized in that, The process of performing degradation detection and generating degradation compensation factors during point cloud matching includes: During the point cloud matching process, the corresponding Hessian matrix is ​​calculated based on the point cloud residuals. Hessian matrix according to and The relationships between them are divided into four sub-matrices. It contains only information related to rotation. It contains only information related to translation. and They represent and Information about the interactions between them; right and Perform singular value decomposition: In the formula: sorting the eigenvalues ​​of the Hessian matrix. Let the rotational degradation factor and the translation degradation factor be defined as follows: Among them, the degradation factor of translation is used to describe the linear characteristic covariance. The rotational degradation factor is used to describe the surface feature covariance. ; Outlier detection is performed using a clustering algorithm with rotational and translational degradation factors to determine the system's degradation state.

5. A laser SLAM-based underground motor positioning method according to claim 1, characterized in that, The step of forward propagating IMU measurement data to obtain system state prediction values ​​includes: Construct a continuous-time motion model based on IMU measurement data: In the formula: and This represents the pose of the IMU in the world coordinate system. For speed, the default value in the formula is taken as... This indicates the transformation from the IMU coordinate system to the world coordinate system; and Measurement noise conforming to a Gaussian distribution; and For IMU zero bias, and Modeled as zero-biased Gaussian white noise, It is the acceleration due to gravity. For IMU measurements, Indicates an antisymmetric matrix; Define the nominal state variables and error variables of the system: In the formula: Defined as the manifold in which the system resides. It is an 18-dimensional error state vector; The motion model is discretized and forward propagated to obtain the predicted values ​​of the nominal state variables: In the formula: the noise term is not considered during propagation of the nominal state variable, but is considered in the error state; therefore, the noise term is zero. Used to compensate for motion distortion in point clouds; The error state is linearized to obtain the error state equation and covariance matrix. Forward propagation is then performed based on the error state equation and covariance matrix to obtain the predicted value. in, Representing the noise matrix The covariance matrix; Represent the covariance matrix; and These represent the predicted value and the predicted covariance of the error state, respectively.

6. A laser SLAM-based underground motor positioning method according to claim 5, characterized in that, The state equation is constructed based on cloud residuals and ground-constrained observations to obtain the observation covariance, including: Construct observation models for line and surface features: in: A ground point cloud observation model is constructed by incorporating ground constraints: in: In the formula: Defined as Measurement noise, and For the first The first frame Individual surface features and line feature points; for Real-time status The current best estimate; Represents the true state and pose of feature points; Representation: Jacobian matrix of linear characteristic residuals; The Jacobian matrix representing the feature residuals of the surface; The Jacobian matrix representing the ground residuals; This represents the ground plane CP parameters of the current frame; This represents the ground plane CP parameter of a local map; This represents the ground plane residual from the current frame to the local map.

7. A laser SLAM-based underground motor positioning method according to claim 6, characterized in that, The method involves compensating the observation covariance based on a degradation compensation factor, obtaining the optimal estimate of the state variables based on the compensated observation covariance and the system state prediction value, and thus obtaining the positioning result, including: Update the state variables of the prediction process: In the formula: These are the covariance matrices of the line-surface feature residuals and the ground feature residuals, respectively. Represents the Kalman filter Jacobian matrix; This represents the new observation residual; Represents the new covariance matrix Introducing a compensation factor to correct the covariance matrix: In the formula: These are the compensation coefficients for the covariance matrix; Iterative Kalman filter gain: In the formula: Let be the posterior covariance of the Kalman filter; The posterior estimate of the error state is obtained: When satisfied When the convergence condition is met, the iteration is complete, and the optimal estimate of the state variables is obtained. The covariance matrix has been updated. Based on optimal estimation , the current frame point cloud Through the optimized pose Transform to world coordinate system to obtain This will generate an updated global map.

8. A laser SLAM-based underground motor positioning system for mines, characterized in that, include: The point cloud data processing module is used to acquire point cloud data from the underground lidar and IMU measurement data, and to remove motion distortion from the point cloud data based on the IMU measurement data to obtain point cloud data after removing motion distortion. The point cloud matching module is used to extract line and surface features from point cloud data after removing motion distortion, perform point cloud matching based on line and surface features, and obtain point cloud residuals; extract ground features of the mine, register the ground features with the local map, and construct ground constraints; and perform degradation detection during point cloud matching to generate degradation compensation factors. The prediction and compensation module is used to forward propagate IMU measurement data to obtain system state prediction values; construct state equations based on cloud residuals and ground constraint observations to obtain observation covariance; and compensate for the observation covariance based on degradation compensation factors. The positioning module is used to obtain the optimal estimate of the state variables based on the compensated observation covariance and the predicted system state value, thus obtaining the positioning result.

9. A terminal device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1-7.

10. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1-7.

Images