A Dynamic Weighted Fusion SLAM Method for Multi-Source Sensor Data in Coal Mines

Abstract

The present application belongs to the technical field of intelligent coal mine, and particularly relates to a kind of coal mine underground multi-source sensor data dynamic weighting fusion SLAM method.In the visual image preprocessing part, an image enhancement algorithm combining single parameter homomorphism filter and histogram equalization in HSV space is added to enhance the brightness and contrast of underground images;Based on the consistency detection method of Mahalanobis distance, the quality of sensor data is evaluated, and then the degradation of sensor data is detected, and the sensor data suitable for the current environment is selected for effective fusion;On the basis of fully considering the key parameters of each sensor, the LiDAR / IMU / Camera factor graph model is constructed, and the multi-source sensor data weight dynamic combination model is constructed according to the data quality, and the weight of each sensor data fusion factor is dynamically adjusted.Compared with LVI-SAM, it has higher pose estimation accuracy and robustness, and the root mean square error of its trajectory is 0.19m.The average point cloud direct comparison distance between the point cloud maps spliced by the station type three-dimensional laser scanning is less than 0.13m, which meets the accuracy requirements of mine robot positioning and mapping, and provides theoretical reference and technical support for intelligent mining and safety inspection of coal mine.

Description

Technical Field

[0001] This invention belongs to the field of intelligent coal mining technology, specifically relating to a dynamic weighted fusion SLAM method for multi-source sensor data in underground coal mines. Background Technology

[0002] Coal plays a dominant role in my country's energy structure and consumption. Intelligent coal mining based on robotics has become a crucial means to achieve efficient, safe, and automated coal production. SLA-based mining robots are widely used in underground coal mine operations, automated inspections, and remote dispatching, and have become one of the key technologies urgently needing to be mastered in the intelligent construction of coal mines. However, data from visual cameras degrades significantly under conditions of uneven lighting, weak texture, and high dust. Inertial Measurement Units (IMUs) are unaffected by changes in the external environment and can provide high-frequency, short-time, and high-precision pose information, but IMU measurement errors accumulate gradually, leading to rapid drift. Light Detection and Ranging (LiDAR) provides direct depth measurement, unaffected by scene lighting and texture changes, and can achieve real-time pose estimation and mapping. However, LiDAR SLAM cannot acquire sufficient geometric constraints in unstructured or self-similar scenes, resulting in degradation. In underground coal mines, current single-sensor SLAM technology can no longer meet the requirements for intelligent sensing accuracy and reliability.

[0003] To address this, a robust multi-source sensor data dynamic weighted fusion SLAM method is proposed. Summary of the Invention

[0004] To address the technical problem of insufficient accuracy in SLAM (Simultaneous Localization and Fusion) of multi-source sensor data fusion due to the narrow, complex terrain, weak texture, and uneven lighting in existing coal mines, which easily leads to degradation of multi-source sensor data, this invention proposes a robust dynamic weighted fusion SLAM method for multi-source sensor data. First, an image enhancement algorithm combining single-parameter homomorphic filtering and histogram equalization in HSV space is added to the visual image preprocessing stage to enhance the brightness and contrast of underground images. Then, a consistency detection method based on Mahalanobis distance is used to evaluate sensor data quality, detect data degradation, and adaptively select sensor data suitable for the current environment for effective fusion. Finally, based on a thorough consideration of the key parameters of each sensor, a LiDAR / IMU / Camera factor graph model is constructed, and a dynamic combination model of multi-source sensor data weights is built according to data quality. This dynamically adjusts the weights of the fusion factors for each sensor data, thereby improving the accuracy and robustness of SLAM.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A dynamic weighted fusion SLAM method for multi-source sensor data in underground coal mines includes the following steps:

[0007] (1) Data preprocessing: IMU pre-integration pose is used to remove motion distortion of point cloud, segment non-ground points from the original point cloud, and extract line and planar features; in the vision part, image enhancement algorithm is used to enhance the visual effect of downhole image, and then corner point extraction and tracking are performed.

[0008] (2) Front-end odometer: After point cloud line and surface feature matching and image feature matching, the consistency detection method based on Mahalanobis distance is used to evaluate the sensor data quality and discard the poor quality data, which will not participate in the subsequent data fusion.

[0009] (3) Backend optimization: Construct a LiDAR / IMU / Camera factor graph model, design a dynamic combination model of multi-source sensor data weights based on sensor data quality, reasonably and dynamically adjust the weights of each sensor factor, and realize joint nonlinear optimization of multi-source sensor data based on sliding window.

[0010] A further technical solution of the present invention is: the IMU pre-integration in step (1) specifically includes: using a pre-integration method.

[24] Integrate multiple IMU measurements between keyframes to construct the IMU pre-integration factor. For two consecutive keyframes i and i+1, the IMU pre-integration residual can be defined as:

[0011]

[0012]

[0013] In the formula, These are the IMU pre-integral values, representing the changes in position, velocity, and attitude caused by linear acceleration and angular velocity, respectively.

[0014] A further technical solution of the present invention is: the image enhancement algorithm in step (1) is an image enhancement algorithm that combines single-parameter homomorphic filtering and histogram equalization in HSV space, and its algorithm flow is as follows:

[0015] (1-1) After determining that the input image is a low-light image using an image brightness judgment algorithm, image enhancement processing is then performed.

[0016] (1-2) Convert the low-light image from the RGB color space to the HSV color space to avoid image distortion after processing in the RGB color space;

[0017] (1-3) Keep the hue H and saturation S unchanged, and use a single-parameter homomorphic filtering algorithm to process the brightness V. While enhancing the image brightness, compress the dynamic range of the image brightness and reduce the impact of uneven lighting on the image.

[0018] (1-4) The luminance component is then processed using a contrast-limited adaptive histogram equalization algorithm. A region-based linear interpolation method is used to eliminate the blockiness caused by this algorithm, thereby improving image contrast. Finally, the unprocessed hue (H), saturation (S), and processed luminance (V) are fused and transformed back to the RGB color space to obtain the enhanced image.

[0019] A further technical solution of the present invention is: the point cloud feature matching method in step (2) specifically includes: estimating the relative state of LiDAR using the line and surface features of continuous frame point clouds and the IMU pre-integration pose; It is a line feature point in the (i+1)th frame. and If the feature point is the line feature point corresponding to it in the i-th frame, then the distance from that point to the line is... for:

[0020]

[0021] Similarly, These are the surface feature points in the (i+1)th frame. and If a point is a surface feature point corresponding to a given surface in the i-th frame, then the distance from that point to the surface is... for:

[0022]

[0023] Based on formulas (3) and (4), the motion constraint equations between two consecutive frames of the laser point cloud can be constructed:

[0024] f(pc e ,pc p ,T L )=min{∑d e +∑d p} (5)

[0025] In the formula, T L It is the pose between LiDAR frames, and the matching residual of the laser point cloud. By d e and d pComposition. Considering that planar features extracted from underground coal mines far outnumber linear features, a two-step method from LeGO-LOAM is used to efficiently estimate the robot's pose. The IMU pre-integration state between LiDAR frames is used as the initial value for iteration. Planar features are prioritized for matching to estimate the robot's roll angle, pitch angle, and vertical displacement. These estimates are then used to match linear features to estimate the remaining horizontal displacement and yaw angle, thereby optimizing the overall pose.

[0026] A further technical solution of the present invention is: the image feature matching method in step (2) specifically involves: extracting corner features of each frame image, tracking them using sparse optical flow, reprojecting based on the matched features, and constructing a visual constraint factor. When the k-th feature of the b-th frame image is observed from the a-th frame image, the visual reprojection error is defined as:

[0027]

[0028] In the formula, The coordinates of the k-th feature observed for the first time in the a-th frame of the image, π C It is the projection function of the camera, λ k This is the inverse depth of this feature point. When calculating the depth of visual features, the depth extracted from the corresponding point cloud is used as the initial value, and this value is constructed as a prior factor and added to the optimization.

[0029] A further technical solution of the present invention is: the specific method for consistency detection in step (2) is as follows: using the IMU increment as a reference to evaluate the data quality of the other two sensors. The positional changes of the LiDAR and the camera are... The Mahalanobis distance between the two and the IMU is:

[0030]

[0031]

[0032] In the formula, σ is the standard deviation of the sensor measurement data. This represents the reliable distance between the LiDAR and the camera at time t. The mobile robot of this invention moves at a constant speed, and a reliable distance threshold is set accordingly. for:

[0033]

[0034] In the formula, S t It predicts the distance to be moved in the current cycle based on the robot's current moving speed. When If the LiDAR confidence distance is within a reasonable range, then the LiDAR measurement and IMU measurement are consistent, the LiDAR data quality is high, and its pose estimation is relatively accurate. In this case, the measurement constraints can be added to the factor graph for joint optimization. Otherwise, the LiDAR data quality is poor, and it may be an outlier, so it should be discarded directly.

[0035] A further technical solution of the present invention is: the back-end optimization method in step (3) specifically includes: constructing a dynamic combination model of multi-source sensor data weights:

[0036]

[0037]

[0038] In the formula, and These are the weights for LiDAR, IMU, and camera. λ is the accuracy coefficient.

[0039] By combining sensor measurements with the robot's state χ, and introducing multiple constraints to estimate the state, this MAP problem is transformed into a nonlinear least squares problem under the assumption of a Gaussian noise model.

[0040]

[0041] In the formula, {r p -H p χ} is the prior information obtained after the marginalization operation, which serves as the prior constraint for sliding window optimization. is the point cloud matching residual, i is the LiDAR point cloud, and s is the local map maintained by the system; It is the IMU pre-integration residual between keyframes within the sliding window; is the visual reprojection residual, and f is the visual feature tracked more than three times consecutively;

[0042] A sliding window factor graph algorithm based on MAP is used to construct a multi-source sensor data fusion factor graph optimization model. First, for real-time acquired LiDAR scan frames and sequential images, corresponding keyframes are extracted using a keyframe selection strategy, and the corresponding LiDAR and camera state nodes are added to the factor graph. Simultaneously, the corresponding LiDAR odometry (LO) and visual odometry (VO) factors are added to the factor graph. Then, IMU pre-integration factors are obtained by pre-integrating the IMU data between the LiDAR scan frames and images, and these factors are added. Finally, consistency checks are performed on the LiDAR and camera states at the same time. If the confidence distance is within a reasonable range, the LiDAR IMU Visual Odometry (LIVO) factor for that time is constructed and added to the factor graph for optimization.

[0043] The advantages of this invention compared to the prior art are:

[0044] A new image enhancement algorithm combining single-parameter homomorphic filtering and histogram equalization in HSV space is added to the visual image preprocessing section, which improves the robustness of the visual front end under uneven lighting conditions.

[0045] A consistency detection method based on Mahalanobis distance detects sensor data degradation and adaptively selects sensor data suitable for the current working environment for subsequent fusion, thus achieving anomaly isolation.

[0046] A dynamic combination model of multi-source sensor data weights was constructed to adjust the weights of the fusion factors of each sensor data in a timely and dynamic manner, thereby improving the positioning accuracy and robustness of the factor graph optimization algorithm. Attached Figure Description

[0047] To more clearly illustrate the technical solution of the present invention, the accompanying drawings used in the embodiments will be briefly described 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 of protection of the present invention.

[0048] Figure 1 This is a framework diagram of a dynamic weighted fusion SLAM method for multi-source sensor data in coal mines, as described in this invention.

[0049] Figure 2 This is a flowchart of the image enhancement algorithm in this invention;

[0050] Figure 3 The weighting function curves for the three sensors in this invention are shown.

[0051] Figure 4This is the multi-source sensor data fusion factor graph optimization model in this invention;

[0052] Figure 5 This invention provides a multi-source sensor data acquisition platform for mobile robots.

[0053] Figure 6 This is a schematic diagram of the experimental environment and the distribution of reference points in this invention;

[0054] Figure 7 These are the processing results of different image enhancement algorithms in this invention;

[0055] Figure 8 This invention provides a comparison of the SLAM trajectory plane positions.

[0056] Figure 9 This is a comparison of the point cloud map of this invention and a reference point cloud map in an underground corridor;

[0057] Figure 10 This is a comparison of the point cloud map of this invention and a reference point cloud map in an underground coal mine. Detailed Implementation

[0058] The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain the invention, and should not be construed as limiting the invention.

[0059] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0060] The method framework of this invention is as follows: Figure 1 As shown, it is mainly divided into three modules. (1) Data preprocessing: IMU pre-integration pose is used to remove point cloud motion distortion, segment non-ground points from the original point cloud, and extract line and planar features. In the vision part, this invention proposes an image enhancement algorithm to enhance the visual effect of downhole images, and then performs corner point extraction and tracking. (2) Front-end odometer: After point cloud line and surface feature matching and image feature matching, the consistency detection method based on Mahalanobis distance is used to evaluate the sensor data quality, and the data with poor quality is discarded and does not participate in subsequent data fusion. (3) Back-end optimization: Construct a LiDAR / IMU / Camera factor graph model, design a multi-source sensor data weight dynamic combination model according to sensor data quality, reasonably and dynamically adjust the weight of each sensor factor, and realize multi-source sensor data joint nonlinear optimization based on sliding window.

[0061] First, the coordinate systems and symbols used in this invention are defined. The LiDAR and camera coordinate systems are defined as L and C respectively, the IMU coordinate system as I, the carrier coordinate system B is consistent with the IMU coordinate system, the robot moves along the positive X-axis, and the world coordinate system W is defined as the center of the carrier system at system initialization. The robot state χ can be represented as:

[0062] χ=[R T ,P T ,v T ,b T ] T (1)

[0063] In the formula, R∈SO(3) is the rotation matrix. v is the robot's position, v is the velocity, and b is the IMU bias.

[0064] (1) Data preprocessing

[0065] Compared to other sensors, the IMU has advantages such as high output frequency, good stability, and strong anti-interference capability, making it an important sensor in the framework of this invention. A pre-integration method is used.

[24] Integrate multiple IMU measurements between keyframes to construct the IMU pre-integration factor. For two consecutive keyframes i and i+1, the IMU pre-integration residual can be defined as:

[0066]

[0067] In the formula, These are IMU pre-integrated values, representing the changes in position, velocity, and attitude caused by linear acceleration and angular velocity, respectively. The IMU pre-integrated factors maintain the accuracy of factor map constraints even in environments where LiDAR and camera factors fail for short periods. Furthermore, the IMU provides information about the direction of gravity, effectively reducing 4DOF estimation drift. If the sensor fails and no IMU measurement data is available, a uniform velocity model is used directly.

[0068] The low illumination levels in coal mines, coupled with significant brightness differences between illuminated and shaded areas, along with factors like camera exposure, result in darker grayscale and lower contrast in coal mine images. Visual SLAM struggles to perform robust visual tracking in unevenly lit environments, leading to large pose estimation errors and even tracking failures. This invention proposes an image enhancement algorithm in HSV space that combines single-parameter homomorphic filtering and histogram equalization to enhance image brightness and contrast under low-light conditions and improve corner point matching success rate. The image enhancement algorithm flow of this invention is as follows: Figure 2As shown, to prevent over-enhancing of images with normal illumination and to reduce the computational load of the algorithm, an image brightness judgment algorithm is used to determine that the input image is a low-illumination image before image enhancement processing. The low-illumination image is transformed from the RGB color space to the HSV color space to avoid image distortion caused by processing in the RGB color space. Hue (H) and saturation (S) are kept constant, and luminance (V) is processed using a single-parameter homomorphic filtering algorithm. This enhances image brightness while compressing the dynamic range of image brightness, reducing the impact of uneven illumination on the image. Then, a contrast-limited adaptive histogram equalization algorithm is applied to the luminance component, using a region-based linear interpolation method to eliminate the block effect and improve image contrast. Finally, the unprocessed hue (H), saturation (S), and processed luminance (V) are fused and transformed back to the RGB color space to obtain the enhanced image, as shown. Figure 2 As shown, this invention uses the Shi-Tomasi corner extraction algorithm to extract corners from the output image, and then uses the sparse optical flow method to track the corner features present in each new frame of the image.

[0069] (2) Front-end odometer

[0070] Point cloud feature matching

[0071] The relative state of LiDAR is estimated by utilizing the line and surface features of point clouds in consecutive frames and the pre-integration pose of the IMU. It is a line feature point in the (i+1)th frame. and If the feature point is the line feature point corresponding to it in the i-th frame, then the distance from that point to the line is... for:

[0072]

[0073] Similarly, These are the surface feature points in the (i+1)th frame. and If a point is a surface feature point corresponding to a given surface in the i-th frame, then the distance from that point to the surface is... for:

[0074]

[0075] Based on formulas (3) and (4), the motion constraint equations between two consecutive frames of the laser point cloud can be constructed:

[0076] f(pc e ,pc p ,T L )=min{Σd e +∑d p} (5)

[0077] In the formula, T LIt is the pose between LiDAR frames, and the matching residual of the laser point cloud. By d e and d p Composition. Considering that planar features extracted from underground coal mines far outnumber linear features, a two-step method from LeGO-LOAM is used to efficiently estimate the robot's pose. The IMU pre-integration state between LiDAR frames is used as the initial value for iteration. Planar features are prioritized for matching to estimate the robot's roll angle, pitch angle, and vertical displacement. These estimates are then used to match linear features to estimate the remaining horizontal displacement and yaw angle, thereby optimizing the overall pose.

[0078] Image feature matching

[0079] Corner features are extracted from each frame and tracked using sparse optical flow. Reprojection is then performed based on the matched features to construct a visual constraint factor. The visual reprojection error is defined as follows: when the k-th feature of the b-th frame is observed from the a-th frame, the visual reprojection error is:

[0080]

[0081] In the formula, The coordinates of the k-th feature observed for the first time in the a-th frame of the image, π C It is the projection function of the camera, λ k This is the inverse depth of this feature point. When calculating the depth of visual features, the depth extracted from the corresponding point cloud is used as the initial value, and this value is constructed as a prior factor and added to the optimization.

[0082] Consistency Detection

[0083] The computational complexity of factor graph optimization is positively correlated with the number of sensors used. Adding sensors unsuitable for the current environment does not help the fusion system but increases computational complexity; moreover, adding incorrect constraints reduces the accuracy of factor graph optimization. LiDAR degrades in highly repetitive narrow alleyways, and cameras degrade because they cannot extract stable features for tracking in weak textures and excessively dark environments. Furthermore, in complex terrain, only a small portion of the measurement data from LiDAR and cameras is usable for pose estimation, leading to mismatches and significant errors. Therefore, it is necessary to evaluate the sensor data quality using a Mahalanobis distance-based consistency detection method before multi-source sensor data fusion to determine if the current sensors are degraded and to restrict the subsequent fusion of data from sensors with poor quality. An IMU is an internal sensor that can operate stably for long periods and is highly robust to changes in the external environment. Although its noise accumulates over time, outliers are rare. As long as the deviation drift can be well constrained by the measurement information from other sensors, accurate pose information can be provided in a short time. Therefore, this invention uses the IMU increment as a reference to evaluate the data quality of the other two sensors. The positional changes of LiDAR and camera are... The Mahalanobis distance between the two and the IMU is:

[0084]

[0085]

[0086] In the formula, σ is the standard deviation of the sensor measurement data. This represents the reliable distance between the LiDAR and the camera at time t. The mobile robot of this invention moves at a constant speed, and a reliable distance threshold is set accordingly. for:

[0087]

[0088] In the formula, S t It predicts the distance to be moved in the current cycle based on the robot's current moving speed. When If the LiDAR confidence distance is within a reasonable range, then the LiDAR measurement and IMU measurement are consistent, the LiDAR data quality is high, and its pose estimation is relatively accurate. Its measurement constraints can be added to the factor graph for joint optimization. Otherwise, the LiDAR data quality is poor, and it may be an outlier, which this invention discards directly. The same applies to the vision part. This operation achieves anomaly isolation before joint data optimization, avoiding a decrease in the optimization accuracy of the entire system or even failure due to the degradation of a single sensor.

[0089] (3) Backend optimization

[0090] Multi-source sensor data weight dynamic combination model

[0091] The working conditions in coal mines are complex, and the performance of sensors varies. The multi-source sensor data acquired exhibits differences in data quality and spatial representation. Fusion of this varied data is essentially an uncertain reasoning process—determining which sensor data at a given moment are more trusted. Since the quality of data from different sensors varies at the same time, and this variation is time-varying, constructing a dynamic weighting model for multi-source sensor data is crucial for achieving high-precision SLAM. The dynamic weighting model for multi-source sensor data in this invention is designed as follows:

[0092]

[0093]

[0094] In the formula, and This refers to the weights of LiDAR, IMU, and camera. The IMU has higher reliability and more accurate state estimation compared to the other two sensors; therefore, its weight is set to 1 in this invention. The smaller the confidence distance, the higher the sensor data quality, and the larger the weight allocated during optimization. The dynamic combination models for the weights of LiDAR and camera differ, reflected in the choice of the accuracy coefficient λ. Even with the same effective distance, LiDAR and camera have different effective weights due to their different performance and measurement accuracy. For example, if the measurement accuracy of LiDAR is higher than that of the camera within its effective measurement range, and both have the same confidence distance, it can be considered that LiDAR measurement degradation is more severe, and LiDAR pose estimation is less reliable than that of the camera; therefore, a smaller weight is allocated to LiDAR. Figure 3 The slope of the LiDAR weighting function curve is greater than that of the camera, and different accuracy coefficients λ are introduced to achieve this.

[0095] Factor graph optimization

[0096] This invention designs a factor graph framework that combines sensor measurement information with the robot's state χ, and introduces multiple constraints to estimate the state. This type of problem can be described as a Maximum a Posteriori (MAP) problem. Therefore, under the assumption of a Gaussian noise model, this MAP problem is transformed into a nonlinear least squares problem:

[0097]

[0098] In the formula, {r p -H p χ} is the prior information obtained after the marginalization operation, which serves as the prior constraint for sliding window optimization. is the point cloud matching residual, i is the LiDAR point cloud, and s is the local map maintained by the system; It is the IMU pre-integration residual between keyframes within the sliding window; F represents the visual reprojection residual, and f represents the visual features tracked more than three times. The cost function of this nonlinear least squares problem can be solved using the Levenberg-Marquardt algorithm; this invention uses the Ceres Solver to solve this problem. L F I and F V This refers to sensor factor weights. Relying on a dynamically combined multi-source sensor data weight model, anomaly isolation, weight adjustment, and state estimation can be achieved during optimization without increasing computational costs excessively. By dynamically adjusting weights based on sensor node performance and preemptively eliminating the influence of abnormal nodes, a factor graph optimization algorithm with enhanced robustness and accuracy is obtained.

[0099] This invention proposes a sliding window factor graph algorithm based on MAP, and constructs a multi-source sensor data fusion factor graph optimization model as follows: Figure 4 As shown, the fused state nodes contain information such as the robot's position, velocity, and attitude. First, for real-time acquired LiDAR scan frames and sequential images, corresponding keyframes are extracted using a keyframe selection strategy. The LiDAR and camera state nodes corresponding to these keyframes are then added to the factor graph, along with the corresponding LiDAR Odometry (LO) and Visual Odometry (VO) factors. Next, IMU pre-integration factors are obtained by pre-integrating the IMU data between the LiDAR scan frames and images, and these factors are added. Finally, consistency checks are performed on the LiDAR and camera states at the same time. If the reliable distance is within a reasonable range, the LiDAR IMU Visual Odometry (LIVO) factor for that time is constructed and added to the factor graph for optimization.

[0100] Experimental Analysis

[0101] To verify the feasibility of the method of this invention, a multi-source sensor data acquisition platform for mobile robots was independently designed, such as... Figure 5As shown. The data acquisition platform of this invention is equipped with a Velodyne VLP-16 LiDAR, an Ellipse2-N IMU, and a ZED-2i binocular camera. The sampling frequency of the LiDAR and camera is set to 10Hz, and the sampling frequency of the IMU is 200Hz. This invention uses a Robot Operating System (ROS) for time synchronization, coordinate transformation, etc., and pre-calibrates the LiDAR and camera using Autoware, and the IMU and camera using Kalibr. The CPU of the onboard computer is... The Ryzen 3 3200G-powered mobile robot uses four-wheel drive to move at a speed of 0.8 m / s. It collects sequence data (01 and 02) in underground corridors and coal mine environments, and ensures the robot's trajectory is closed. Figure 6 As shown, the underground corridor has obvious structural features, but the lighting is uneven. The corridor is long and narrow, and there are large areas of weak texture. The robot's movement trajectory is 214m long. There are multiple tracks in the coal mine that need to be traversed by the mobile robot. Some roadways have low lighting, a large number of sprayed surfaces and symmetrical roadways. The robot's movement trajectory is 551m long.

[0102] Experimental results

[0103] Image enhancement experiments

[0104] The image enhancement algorithm proposed in this invention was compared with commonly used homomorphic filtering, histogram equalization, and Retinex algorithms to verify that the image enhancement algorithm of this invention can effectively enhance the brightness and contrast of images, thereby improving the robustness of the visual front end. Figure 7The results before and after processing by four image enhancement algorithms are shown. The Retinex algorithm excessively enhances image brightness, causing the entire image to appear washed out and losing image detail. Homomorphic filtering only enhances the brightness of the backlit area and preserves the information of the illuminated area, while losing some detail information in the illuminated area of ​​the well image. Histogram equalization stretches gray levels, which improves image contrast and restores more detail information, but its brightness improvement is not as good as the first two algorithms. The algorithm of this invention uses single-parameter homomorphic filtering and histogram equalization to enhance image brightness while preserving more detail information. It can also suppress the over-enhancement phenomenon of traditional homomorphic filtering on images with too large a difference between light and dark areas, thus improving the overall visual effect of the image. Table 1 shows the number of corner points matched and the matching success rate of the enhanced image. The brightness and contrast of the image affect the corner point matching results. This invention sets the maximum number of corner points extracted per frame to 200. The original image has low brightness and contrast, resulting in insufficient number of extracted corner points and even fewer successful matches, making it difficult for visual odometry to operate normally. After preprocessing by the image enhancement algorithm of this invention, a higher matching success rate is obtained, improving the robustness of visual SLAM in uneven lighting environments. The image enhancement algorithm of this invention processes grayscale images with a resolution of 1280×720 in an average time of only 30ms, which meets the real-time requirements of SLAM. In subsequent localization and mapping experiments, the image enhancement algorithm was incorporated into all algorithms involving the vision component. Since the original vision algorithm struggled to accurately estimate the robot's pose in unevenly lit scenes, this invention does not separately compare and analyze the SLAM results before and after image enhancement.

[0105] Table 1 Comparison of corner point matching before and after image enhancement.

[0106]

[0107] Trajectory Analysis

[0108] Reference points are set up at important locations using a total station, and manual markers are affixed to the robot. The robot moves to the reference point and pauses briefly. The average position output by SLAM during the pause period is used as the measured value, and the position of the reference point is used as the reference value. The two are quantitatively compared and analyzed to determine the absolute accuracy of the robot's movement trajectory. Figure 8 This is the trajectory planar position comparison result of the SLAM method. In long corridors, although the laser point cloud can provide sufficient lateral constraints, the longitudinal constraints at both ends of the corridor are severely insufficient, resulting in significant degradation of the forward direction in LOAM pose estimation. This degradation also occurred in several long tunnels in the coal mine, and the robot's trajectory drifted severely when crossing the track, making normal operation impossible. Even with high-frequency navigation information provided by the IMU to assist in visual SLAM front-end feature matching, ORB-SLAM3 pose estimation showed some degradation when there were large areas of white walls and low lighting, with the entire trajectory exhibiting a "shrinking" phenomenon. In the coal mine, it could only operate normally to the air door. Figure 6LIO-SAM, by incorporating IMU pre-integration constraints for joint optimization, can compensate to some extent for the degradation of LiDAR pose estimation. It does not show significant degradation in underground corridors, but it does degrade in excessively narrow, semi-structured tunnels. Figure 8 When the mobile robot traverses the track, the LiDAR shakes violently, resulting in significant noise in the point cloud and a large deviation in pose estimation at this point. Figure 8 In LVI-SAM, LiDAR odometry, visual odometry, IMU pre-integration constraints, and loop closure constraints are jointly optimized to ensure global consistency between localization and mapping. However, when the camera or LiDAR data quality is poor, introducing incorrect constraints into graph optimization can lead the system in the wrong optimization direction. Near reference point B7, the robot turns in a narrow corner in a dark environment, resulting in a large deviation in visual pose estimation, which affects the final result of joint optimization, causing some trajectories to "tilt". When crossing tracks, the robot's perspective changes drastically, and the sensor observation data also changes drastically. The pose estimation of the other two methods produces large deviations. The method of this invention discards abnormal data and fuses the estimated state increment of the robot, resulting in a smoother output trajectory.

[0109] In degraded scenarios, the method of this invention directly restricts the use of poor-quality sensor data for joint optimization, and constructs a weighted dynamic combination model based on the quality of each sensor data, thereby reducing the impact of degraded sensor data on the optimization results, ensuring the robustness of the factor graph algorithm, improving the accuracy of pose estimation, and making the positioning trajectory closer to the reference point.

[0110] The positional errors between the trajectory points and reference points output by the five SLAM methods are shown in Table 2 below. LOAM and ORB-SLAM3 have poor positioning accuracy in underground corridors, with positional errors (RMSE) greater than 1m, making them unsuitable for complete operation in coal mines. While LIO-SAM and LVI-SAM did not show significant trajectory drift in underground corridors, their accuracy improved during turns (…). Figure 8 The mobile robot exhibits significant swaying, and the constraints constructed from sensor data are inaccurate, resulting in considerable deviations in pose estimation. The maximum position errors are 0.29m and 0.18m, respectively. Both methods degrade severely in semi-structured narrow tunnels. LVI-SAM, by adding camera constraints, mitigates the degradation of system pose estimation to some extent. Figure 8 However, its positional error RMSE exceeds 1.5m, making it difficult to provide effective navigation for mobile robots in underground coal mines. The method of this invention selects appropriate sensor data for fusion in degraded scenarios and dynamically adjusts the sensor factor weights based on a multi-source sensor data weight dynamic combination model, thereby improving the overall optimization performance and robustness. Its positional error RMSE can reach 0.19m, achieving accurate and robust pose estimation in the extremely challenging underground coal mine environment.

[0111] Table 2. Position Errors of SLAM Trajectory Points and Reference Points

[0112]

[0113] Note: "N / A" indicates that the method failed.

[0114] Map building analysis

[0115] To verify the accuracy of the SLAM mapping method of this invention, the experimental environment was scanned using a Leica ScanStation C10 3D laser scanner. The multi-station scanned point clouds were precisely registered to construct a complete scene point cloud map, which this invention uses as a reference point cloud map. The reference map and the global map output by this invention's method were registered using the Iterative Closest Point (ICP) algorithm to ensure that the reference point cloud map and the point cloud map of this invention are in the same coordinate system. As can be seen from the previous trajectory analysis, other methods produce poor localization results in complex environments, with their output point cloud maps differing significantly from the reference map. Therefore, this invention does not compare or analyze the point cloud maps of these methods. Instead, the differences between the two point cloud maps are directly visually examined to qualitatively analyze the global integrity of the point cloud map of this invention; the accuracy of the point cloud map of this invention is quantitatively determined by calculating the C2C distance between the point cloud maps.

[0116] exist Figure 9 , 10 In the diagram, (a) shows a detailed comparison between the point cloud map of this invention and the reference point cloud map after unifying the coordinate system. The red dots represent the point cloud of this invention, the green dots represent the reference point cloud, and the purple dots represent the robot's motion trajectory. (b) shows the color-coded display based on the C2C distance between the two point cloud maps. The point cloud map constructed by the method of this invention performs excellently in terms of global consistency and geometric realism, accurately reflecting the actual environment. By zooming in, it can be clearly seen that the wall surface of the point cloud map of this invention is almost identical to that of the reference point cloud map. The hydraulic support of the coal mine at the reference point B6 in the coal mine is clearly visible, with a high degree of overlap, and the robot's motion trajectory also conforms to the actual motion situation. In underground corridors, the maximum deviation between the point cloud map of this invention and the reference point cloud map is less than 0.77m (only a small part), and the average deviation is less than 0.05m (accounting for the vast majority); in underground coal mines, the maximum deviation is less than 1.60m (only a small part, near reference point B3, there are a large number of dynamic objects, and the mapping in this part has a lot of noise), and the average deviation is less than 0.13m (accounting for the vast majority), further verifying the overall accuracy of the mapping method of this invention.

[0117] To verify the real-time performance of the method of this invention, taking data sequence 02 as an example, the average running time of each module of the SLAM algorithm was statistically analyzed, as shown in Table 3. Both LVI-SAM and the algorithm of this invention, after adding the image enhancement step, can run completely, although the average running time of the preprocessing module increases. The average running time of the odometry module in the algorithm of this invention is slightly longer than that of LVI-SAM because of the addition of a consistency detection algorithm; however, this prevents lower-quality sensor data from entering factor graph optimization, thus reducing optimization time to some extent. The average running time of each module in the algorithm of this invention is less than the sampling interval of LiDAR and camera (0.1s), indicating that the system can run in real time.

[0118] Table 3 shows the average running time of each module in data sequence 02.

[0119]

[0120] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in the embodiments of this application can be arbitrarily combined with each other.

Claims

1. A coal mine underground multi-source sensor data dynamic weighting fusion SLAM method, characterized in that, Comprising the following steps: (1) Data preprocessing: IMU pre-integration pose is used to remove point cloud motion distortion, segment non-ground points from the original point cloud, and extract line and plane features; in the vision part, an image enhancement algorithm is used to enhance the visual effect of the underground image, and then corner extraction and tracking are performed; (2) Front-end odometry: after point cloud line, surface feature matching and image feature matching, a consistency detection method based on Mahalanobis distance is used to evaluate the quality of sensor data, and the data with poor quality is discarded and does not participate in subsequent data fusion; (3) Back-end optimization: a LiDAR / IMU / Camera factor graph model is constructed, a multi-source sensor data weight dynamic combination model is designed according to the quality of sensor data, the weight of each sensor factor is dynamically adjusted, and multi-source sensor data joint nonlinear optimization is realized based on a sliding window; The back-end optimization method in step (3) specifically comprises: constructing a multi-source sensor data weight dynamic combination model: wherein and are weights for LiDAR, IMU, and camera; λ is a precision coefficient; Combine the measurement information of the sensor and the state χ of the robot, and introduce multiple constraints to estimate the state; under the assumption of Gaussian noise model, this MAP problem is converted into a nonlinear least squares problem: where {r p -H p χ} is the prior information obtained after the marginalization operation, as a prior constraint for the sliding window optimization; is the point cloud matching residual, i is the LiDAR point cloud, s is the local map maintained by the system; is the IMU pre-integration residual between keyframes within the sliding window; is the visual re-projection residual, f is the visual feature tracked continuously more than three times.

2. The coal mine underground multi-source sensor data dynamic weighted fusion SLAM method according to claim 1, characterized in that: The IMU pre-integration in step (1) specifically comprises: using a pre-integration method to integrate multiple IMU measurements between key frames to construct an IMU pre-integration factor; for two consecutive key frames i and i+1, the pre-integration residual of the IMU is defined as: In the formula, are IMU pre-integration values, respectively representing changes in position, velocity, and attitude caused by linear acceleration and angular velocity.

3. The coal mine underground multi-source sensor data dynamic weighted fusion SLAM method according to claim 1, characterized in that: The image enhancement algorithm in step (1) is an image enhancement algorithm combining single-parameter homomorphic filtering and histogram equalization in HSV space, and the algorithm flow is: (1-1) After determining that the input image is a low-illumination image by using an image brightness judgment algorithm, image enhancement processing is performed; (1-2) Transform the low-illumination image from the RGB color space to the HSV color space to avoid image distortion after processing in the RGB color space; (1-3) Keep the hue H and saturation S unchanged, and use a single-parameter homomorphic filtering algorithm to process the brightness V, which enhances the image brightness while compressing the image brightness dynamic range and reducing the influence of uneven illumination on the image; (1-4) Then, the brightness component is processed by a contrast-limited adaptive histogram equalization algorithm, a region-based linear difference calculation method is used to eliminate the block effect caused, and the image contrast is improved; finally, the unprocessed hue H, saturation S and processed brightness V are fused, and transformed back to the RGB color space to obtain the enhanced image.

4. The coal mine underground multi-source sensor data dynamic weighted fusion SLAM method according to claim 1, characterized in that: The point cloud feature matching method in step (2) specifically comprises: estimating the LiDAR relative state by using the line, surface features of the continuous frame point cloud and the IMU pre-integrated pose; is a line feature point in the i+1th frame, and is a line feature point corresponding to the line feature point in the ith frame, and the distance of the point to the line is: Similarly, is the facial feature point in the i+1th frame, and is its corresponding facial feature point in the ith frame, then the distance of the point to the face is: According to formulas (3) and (4), a motion constraint equation between two consecutive frames of laser point cloud is constructed: f(pc e ,pc p ,T L ) = min{∑d e +∑d p} (5) In the formula, T L is the pose between LiDAR frames, the matching residual of the laser point cloud is composed of d e and d p ; considering that the planar features extracted in the coal mine are much more than the line features, the pose of the robot is efficiently estimated using the two-step method in LeGO-LOAM; the IMU pre-integrated state between LiDAR frames is used as the initial value of iteration, the planar features are preferentially matched to estimate the roll angle, the pitch angle and the vertical displacement of the robot, the estimated values are brought in, and then the line features are matched to estimate the remaining horizontal displacement and the yaw angle, so as to optimize the overall pose.

5. The coal mine underground multi-source sensor data dynamic weighted fusion SLAM method according to claim 1, characterized in that: The image feature matching method in step (2) specifically comprises: extracting the corner features of each image, and tracking using a sparse optical flow method, constructing a visual constraint factor according to the matched features; when the kth feature of the bth image is observed from the ath image, the visual re-projection error is defined as: wherein, is the coordinate of the kth feature first observed by the ath frame image, π C is the projection function of the camera, λ k is the inverse depth of this feature point; the depth extracted from the corresponding point cloud is used as the initial value when calculating the depth of the visual feature, and this value is constructed as a prior factor added to the optimization.

6. The coal mine underground multi-source sensor data dynamic weighted fusion SLAM method according to claim 1, characterized in that: The specific method of consistency detection in step (2) is: using IMU increment as a reference to evaluate the data quality of the other two sensors; the position change amount of LiDAR and the camera is Then the Mahalanobis distance of the two with the IMU is: where σ is the standard deviation of the sensor measurement data, are the Mahalanobis distances of the LiDAR and camera at time t, respectively, which are defined as the trusted distances of the LiDAR and camera at time t, respectively; The mobile robot advances at a constant speed, and a reliable distance threshold is set accordingly is: In the formula, S t is to predict the moving distance of the current period according to the moving speed of the robot at this time; when , the LiDAR credible distance is in a reasonable range, then the LiDAR measurement and the IMU measurement are consistent, the LiDAR data quality is high, the pose estimation is accurate, and the measurement constraint can be added to the factor graph for joint optimization; otherwise, the LiDAR data quality is poor and may be an outlier, which is directly discarded.

7. The coal mine underground multi-source sensor data dynamic weighted fusion SLAM method according to claim 1, characterized in that: The multi-source sensor data fusion factor graph optimization model is constructed by using a sliding window factor graph algorithm based on a MAP. Firstly, for the real-time collected LiDAR scan frame and sequence image, the corresponding key frame is extracted through a key frame selection strategy, and the LiDAR and camera state nodes corresponding to the key frame are added to the factor graph, and the corresponding laser odometry factor and visual odometry factor are added to the factor graph. Then, the IMU data between the LiDAR scan frame and the image is pre-integrated to obtain an IMU pre-integrated factor, and the IMU pre-integrated factor is added. Finally, the consistency of the LiDAR state and the camera state at the same time is detected, if the reliable distance is within a reasonable range, the laser inertial navigation visual odometry factor at the same time is constructed, and is added to the factor graph for optimization.

Images