A method and medium for fusion positioning in a long corridor environment of a mine

Abstract

The application discloses a kind of long corridor environment fusion positioning method and medium of mine, comprising: obtaining the sensor data of laser radar and IMU;Sensor data is preprocessed and motion is deformed;According to the deformed point cloud data, the target area is extracted by vertical slice and RANSAC fitting, and the dense point cloud area containing pipeline and signboard is obtained;According to the deformed point cloud data and dense point cloud area, dynamic obstacle detection is carried out using the method based on multidimensional grid map descriptor, and the detected dynamic point cloud is eliminated, to obtain static point cloud;According to static point cloud and IMU measurement data, construct tight coupling factor graph model, and solve tight coupling factor graph model by nonlinear optimization algorithm, output global pose estimation result.For the geometric degeneration problem specific to long corridor environment of mine, the application effectively solves the problem of positioning error accumulation along the direction of roadway and the problem of underground dynamic obstacle interference.

Description

Technical Field

[0001] This application relates to the field of underground mining, and in particular to a fusion positioning method and medium for long corridor environments in underground mines. Background Technology

[0002] With the increasing application of intelligent and unmanned technologies in underground spaces, the demand for unmanned driving in long corridor environments such as mine shafts and tunnels is becoming increasingly urgent. Positioning is the foundation and prerequisite for achieving unmanned driving, directly determining the reliability and safety of autonomous navigation systems. Long corridor environments such as mine shafts and tunnels have significant geometric characteristics: narrow and elongated spaces, strong structural repetition, and limited cross-sectional dimensions. This unique geometry leads to severe degradation of positioning observation along the longitudinal direction of the shaft, making it difficult for sensors to obtain effective positional constraints in this direction. Simultaneously, the underground working environment is complex and variable, with frequent dynamic obstacles such as mining transport vehicles, inspection personnel, and maintenance equipment, and widespread environmental interference such as dust and water mist, placing extremely high demands on the robustness of the positioning system.

[0003] LiDAR (Light Detection and Ranging) can acquire three-dimensional geometric information of the environment, while inertial measurement units (IMUs) can provide high-frequency motion measurements. The fusion of these two technologies in laser inertial positioning systems has been widely used in the field of autonomous driving. However, existing laser inertial positioning technologies still face many challenges in complex and degraded environments such as long corridors in underground mines, making it difficult to meet the stringent requirements of positioning accuracy, robustness, and real-time performance for practical engineering applications. Therefore, there is an urgent need to explore high-precision, robust positioning solutions specifically designed for the geometrically degraded characteristics of long corridors and dynamic, complex working conditions.

[0004] Existing laser inertial positioning technology has the following main technical shortcomings when applied in long corridor environments such as mines and tunnels:

[0005] (1) Traditional laser SLAM technology relies on predefined fixed rules for feature extraction, such as edge point and planar point extraction based on curvature. This method performs well in structured indoor environments or open outdoor environments, but faces severe challenges in the long corridor environment of underground mines. The geometric shape and spatial distribution of key positioning features such as pipelines, signs, and support structures in underground mine roadways have significant domain specificity, and traditional fixed rules are difficult to accurately identify these target area features that are crucial for positioning. Especially in complex scenarios with temporary obstacles and multiple similar structures, feature extraction methods based on global fitting or simple geometric constraints are prone to errors, resulting in inaccurate extraction results, loss of key positioning information, and inability to provide reliable observation constraints for subsequent pose estimation.

[0006] (2) Existing positioning algorithms generally treat all observed point clouds simply as a static environment, assuming that the environment is unchanging. This assumption is seriously inconsistent with the actual situation in underground mining operations. Dynamic obstacles such as mining vehicles, pedestrians, and mobile equipment frequently appear underground, and their movement paths are highly concentrated due to the constraints of the roadways. If these dynamic point clouds are included in the positioning calculation, it will cause the dynamic objects to form ghosting phenomena in the map, which will seriously damage the spatiotemporal consistency of the map. The participation of dynamic point clouds in point cloud registration will introduce incorrect pose constraints, especially in the longitudinal direction of the geometrically degraded roadways in underground mines. The registration error introduced by dynamic obstacles will accumulate rapidly, misleading subsequent path planning and motion control, and seriously affecting the safety and reliability of the unmanned driving system. Existing technologies lack dynamic obstacle detection and removal mechanisms that are specific to the characteristics of long corridor environments in underground mines, and cannot guarantee the purity and consistency of the map information on which positioning depends.

[0007] (3) Existing laser inertial positioning systems mostly adopt a loosely coupled architecture, where the IMU and lidar calculate pose independently. The IMU is only used for motion compensation (point cloud distortion correction) between lidar frames, without deep sensor fusion under a unified optimization framework. This loosely coupled approach cannot fully leverage the complementary advantages of IMU high-frequency motion measurement and lidar absolute position constraints. In the long corridor environment of underground mines, due to geometric degradation, lidar observation in the longitudinal direction of the tunnel is insufficient, and the loosely coupled system cannot effectively suppress the accumulated error in this direction. At the same time, the loosely coupled architecture cannot achieve online calibration and real-time correction of IMU zero bias. The zero bias drift of the IMU will accumulate over time, further reducing the positioning accuracy. The accumulated error of pose estimation is unavoidable. The positioning accuracy of a loosely coupled system lacking a global optimization mechanism will continue to deteriorate after long-term operation, failing to meet the long-term positioning stability requirements of unmanned driving in underground mines.

[0008] In summary, existing technologies suffer from prominent problems in long corridor environments such as mines and tunnels, including inaccurate feature extraction, lack of dynamic obstacle handling, and insufficient sensor fusion. There is an urgent need to develop new spatiotemporal consistency-constrained laser inertial positioning methods that address the geometric degradation characteristics and complex dynamic conditions of long corridors. Summary of the Invention

[0009] To address the unique geometric degradation problem in long underground mine corridor environments, this application provides a fusion positioning method and medium for such environments, effectively solving the problems of accumulated positioning errors along the tunnel route and interference from dynamic obstacles underground.

[0010] One aspect of this application provides a fusion positioning method for a long corridor environment in an underground mine, comprising: S1, acquiring sensor data from a lidar and an IMU, the sensor data including raw point cloud data from the lidar and measurement data from the IMU; S2, preprocessing and motion distortion correction of the sensor data to obtain distorted point cloud data; S3, extracting target regions from the distorted point cloud data through vertical slicing and RANSAC fitting to obtain a dense point cloud region containing pipes and signs; S4, performing dynamic obstacle detection using a method based on multidimensional grid map descriptors based on the distorted point cloud data and the dense point cloud region, and removing detected dynamic point clouds to obtain a static point cloud; S5, constructing a tightly coupled factor graph model based on the static point cloud and the IMU measurement data, solving the tightly coupled factor graph model through a nonlinear optimization algorithm, and outputting a global pose estimation result;

[0011] Further, in step S2, the sensor data is preprocessed and motion distortion corrected to obtain distorted point cloud data, including: calibrating the spatial coordinate relationships between the lidar and the vehicle body, and between the lidar and the IMU, to obtain the coordinate transformation matrix between the lidar, the vehicle body, and the IMU; downsampling and outlier processing are performed on the raw lidar point cloud data to obtain preprocessed point cloud data; using IMU measurement data, based on the IMU kinematic model and median integration method, the relative pose transformation of each sampling time within the lidar scanning cycle relative to the preset reference time tref is calculated; based on the relative pose transformation... In addition, rotation and translation compensation are performed on each laser point in the preprocessed point cloud data. The laser points collected at different times during the scanning cycle are uniformly transformed to the lidar coordinate system at the preset reference time tref to eliminate the point cloud distortion caused by lidar movement and obtain distorted point cloud data. In particular, when mining vehicles travel in narrow spaces in underground mine roadways, the vehicle posture changes drastically and frequently due to factors such as limited roadway width, uneven road surface, and frequent turns. In addition, the continuous movement of the vehicle during the lidar scanning cycle makes the point cloud distortion problem more serious than in normal environments.

[0012] This solution establishes the foundation for multi-sensor collaborative operation by calibrating the precise spatial coordinate relationship between the lidar, vehicle body, and IMU. Utilizing high-frequency measurement data from the IMU, based on the IMU kinematic model and median integral method, it can capture drastic attitude changes of vehicles in underground mine roadways in real time and accurately calculate the relative pose transformation at each sampling moment within the lidar scanning cycle. By performing rotation and translation compensation on each lidar point, the point clouds collected at different times are unified under the same reference coordinate system, effectively eliminating point cloud distortion caused by vehicle bumps, turns, and other movements.

[0013] Furthermore, in step S3, based on the distortion-free point cloud data, the target region is extracted through vertical slicing and RANSAC fitting to obtain a dense point cloud region containing pipes and signs. This includes: performing vertical Z-axis filtering on the distortion-free point cloud data and extracting the height within a preset range. Point cloud slices are used to obtain linear calibration features of pipes or signs in long corridor environments of underground mines. Based on the RANSAC algorithm, two straight lines L1 and L2 are fitted from the point cloud slices, each line being derived from a reference point. The unit direction vector is determined; the average of the direction vectors of the two lines L1 and L2 is calculated, and the average value is projected onto the horizontal XY plane to obtain the average direction vector; the angle between the average direction vector and the positive X-axis is calculated as the rotation correction angle for the distortion-removed point cloud data. Correction angle based on rotation A rotation transformation matrix around the Z-axis is constructed, and this matrix is ​​used to perform a rotation transformation on the distorted point cloud data to obtain rotated point cloud data, aligning the point cloud coordinate system with the main direction of the long corridor. The centroids of the point clouds corresponding to two straight lines L1 and L2 are calculated, and their Y-coordinate values ​​in the rotated coordinate system are compared. The straight line corresponding to the centroid with the larger Y-coordinate value is taken as the left calibration line. Using the left calibration line as a reference, a strip-shaped target region is constructed, with the horizontal distance from the left calibration line to the target region. and vertical height range Common definition; among which, This represents the minimum horizontal distance threshold from the point to the left calibration line, used to exclude noise points or wall points that are close to the calibration line. This represents the maximum horizontal distance threshold from the point to the left calibration line, used to limit the width range of the strip region and ensure that only pipe and sign features within a reasonable distance from the calibration line are extracted. This indicates the minimum height threshold for pipes and signs in the vertical direction, used to filter out ground and low-level interference features; This indicates the maximum height threshold for pipes and signs in the vertical direction, used to filter out ceiling and high-level interference features;

[0014] Iterate through each point in the distortion-free point cloud data, use the rotated coordinates of the corresponding point to determine its horizontal position, and use the original coordinates of the corresponding point to determine its height. Filter out points that simultaneously meet the horizontal distance interval condition and the vertical height interval condition to form a dense point cloud region containing pipes and signs, which is then used as the target region.

[0015] In particular, underground mine roadways exhibit a typical long and narrow structure, with highly similar and uniform geometric features along the roadway direction. This leads to significant positioning drift when using traditional point cloud matching methods along the roadway's axis. However, the roof of underground mine roadways is generally densely covered with linear facilities such as ventilation ducts, water supply pipes, and cables, as well as markers such as safety signs. These facilities extend along the roadway direction and possess stable geometric shapes and spatial relationships, representing a unique structural feature that distinguishes the underground mine environment from ordinary long corridors.

[0016] This scheme uses vertical slicing to accurately locate the height range of pipelines and signs on the roof of underground mine roadways. The RANSAC algorithm robustly extracts two straight lines representing the directions of pipelines on both sides of the roadway from a massive point cloud. By calculating the average direction of the two straight lines and performing coordinate system rotation correction, precise alignment of the point cloud coordinate system with the main direction of the underground mine roadway is achieved. This alignment process transforms the originally ambiguous geometric information in the degenerate direction into explicit constraints in the direction perpendicular to the roadway.

[0017] The strip-shaped target area constructed based on the left-side calibration line accurately covers the spatial distribution range of pipelines and signs on the roof of underground mines. It effectively filters out redundant information from tunnel walls, floors, and ceilings that contribute little to positioning, while retaining the dense feature cloud with the most positioning value. This feature extraction strategy, designed specifically for the structural characteristics of underground mine tunnels, transforms the structural disadvantages (geometric degradation) of the underground mine environment into positioning advantages (stable linear features). It provides a reliable data foundation for subsequent high-precision point cloud registration based on these features, fundamentally solving the problem of insufficient positioning accuracy along the tunnel direction in long corridor environments of underground mines.

[0018] Among them, the X-axis: In the lidar coordinate system, the X-axis is the horizontal forward axis, which usually points to the forward direction of the lidar or vehicle. In this scheme, it is used to establish the horizontal reference of the point cloud coordinate system and as a reference axis for calculating the rotation correction angle.

[0019] Y-axis: In the lidar coordinate system, the Y-axis is the horizontal axis, perpendicular to the X-axis, and usually points to the left of the lidar or vehicle. In the environment of underground mine roadways, it corresponds to the width of the roadway and is used to distinguish the positions of pipes on the left and right sides of the roadway.

[0020] Z-axis: In the lidar coordinate system, the Z-axis is the vertical axis, perpendicular to the horizontal XY plane and pointing upwards. In the environment of underground mine roadways, it corresponds to the height direction of the roadway and is used to represent the vertical height of the point cloud relative to the ground. It is the basis axis for vertical slicing and height interval filtering.

[0021] Positive X-axis direction: refers to the direction in which the values ​​on the X-axis increase, i.e. the forward direction of the lidar or vehicle. In this scheme, the rotation correction angle θ is determined by calculating the angle between the average direction vector and the positive X-axis direction, so as to achieve the alignment of the point cloud coordinate system with the main direction of the underground mine roadway.

[0022] Vertical direction: refers to the direction along the Z-axis, that is, the vertical direction perpendicular to the ground, corresponding to the height direction of the roadway in an underground mining roadway environment. The vertical slices and vertical height intervals in this scheme... These operations are all spatial screening performed in the vertical direction, used to accurately locate the height range of pipelines and signs on the roof of underground mines.

[0023] Furthermore, in step S4, based on the distortion-free point cloud data and dense point cloud regions, a method based on multidimensional grid map descriptors is used to detect dynamic obstacles and remove the detected dynamic point clouds to obtain static point clouds. This includes: calculating the relative rotation angle and relative translation distance between the current frame pose and the previous keyframe pose; when the relative rotation angle or relative translation distance exceeds a preset threshold, the current frame is set as a new keyframe; constructing a keyframe sub-map using a sliding window mechanism, the keyframe sub-map consists of the latest N keyframes, and when a new keyframe is added, the earliest keyframe in the time sequence is removed; converting the dense point cloud regions of each keyframe in the keyframe sub-map into a two-dimensional grid map; for each grid in the grid map, calculating the two-dimensional Euclidean distance from all points in the grid to the current scanning position of the lidar on the horizontal XY plane to obtain a distance set;

[0024] Based on the distance set, the mean, covariance, and interquartile range are calculated as multidimensional raster map descriptors for the raster. The mean reflects the central tendency of the distance from points within the raster to the carrier and is sensitive to changes in the overall position. The covariance characterizes the dispersion of the distance distribution and is sensitive to changes in point cloud density and distribution pattern. The interquartile range provides a robust statistical description through quantile differences and is insensitive to outliers and noise. These three statistical measures describe the distance distribution characteristics of the raster from different perspectives, complementing and verifying each other to improve the accuracy and robustness of dynamic obstacle recognition.

[0025] Compare the differences between the multidimensional raster map descriptors of the corresponding grid cells in the current frame and the keyframe sub-image. When any difference in the mean, covariance, or interquartile range exceeds the corresponding preset threshold, the corresponding grid cell is determined to be a dynamic region. The mean range is [0.3, 0.8]; the covariance range is [0.05, 0.258]; and the interquartile range IQR_d = Q3 - Q1 (75th percentile minus 25th percentile).

[0026] Remove the point cloud from the dynamic region to obtain the static point cloud.

[0027] In particular, on the one hand, the geometric degradation characteristics of narrow tunnels make the positioning system highly dependent on stable features such as pipelines and signs; on the other hand, dynamic obstacles such as mining transport vehicles, inspection personnel, and maintenance equipment frequently appear in the underground working environment, and their movement paths are highly concentrated due to the constraints of the tunnels. If these dynamic obstacles are misjudged as static features and used in positioning calculations, they will introduce serious registration errors in the direction of geometric degradation, leading to a rapid deterioration or even failure of positioning accuracy.

[0028] This application utilizes a keyframe sub-map constructed through a sliding window to leverage the inherent difference between static features (pipelines, signs) in underground mine roadways, which are highly stable in the time dimension, and dynamic obstacles, which are constantly changing in the spatial dimension. By converting dense point cloud regions into a two-dimensional grid map and calculating the distance distribution characteristics from the point cloud to the lidar within each grid, this design fully utilizes the characteristic that dynamic obstacles in underground mine roadways inevitably cause changes in local distance distribution due to spatial constraints.

[0029] Furthermore, in the multidimensional raster map descriptor (mean, covariance, interquartile range), the mean can capture the overall distance shift caused by underground transport vehicles entering or leaving the raster area; the covariance can identify the changes in the original pipeline point cloud distribution caused by dynamic obstacles; and the interquartile range provides robust discrimination capability under noise interference such as dust and water mist in underground mines. The three complement each other to ensure that dynamic obstacles can be detected sensitively in the complex working conditions of underground mines, while avoiding misjudging key static features such as pipelines and signs as dynamic.

[0030] By comparing the temporal sequence with the keyframe sub-graph, this solution can accurately distinguish between static structures (pipelines, signs) that should exist in underground mine roadways and dynamic obstacles (vehicles, personnel) that should not exist for a long time. The pure static point cloud obtained after removing the dynamic point cloud ensures that the subsequent point cloud registration and positioning calculation are based entirely on the stable geometric features of the underground mine roadway. This fundamentally eliminates the interference of dynamic obstacles on positioning accuracy in the direction of geometric degradation, ensuring the long-term stability and high-precision characteristics of the positioning system in the long corridor environment of underground mines.

[0031] Furthermore, in step S5, based on the static point cloud and IMU measurement data, a tightly coupled factor graph model is constructed, and the model is solved using a nonlinear optimization algorithm to output the global pose estimation result. This includes: performing ICP point cloud registration between the static point cloud of the current frame and the static point clouds of each key frame in the key frame sub-image to obtain the relative pose estimation between the current frame and each key frame; the relative pose estimation is used as the observed value of the lidar odometry factor; defining state variables, including pose state, velocity state, and IMU zero-bias state, where the pose state includes the rotation matrix and translation vector, the velocity state represents the three-dimensional velocity vector of the lidar in the world coordinate system, and the IMU zero-bias state includes the accelerometer zero bias and the gyroscope zero bias; constructing a tightly coupled factor graph model, which includes the initial prior factor, the lidar odometry factor, and the IMU pre-integration factor; and using a Trust-based approach. Region's nonlinear optimization algorithm iteratively solves the tightly coupled factor graph model. By minimizing the sum of squared residuals of all factors, it obtains the optimal linear increment of the state variables. The state variables are updated according to the optimal linear increment to obtain the optimized pose state, velocity state, and IMU zero bias state, and the global pose estimation result of the current frame is output.

[0032] The tightly coupled factor graph model is a probabilistic graphical optimization framework for multi-sensor fusion localization. In this scheme, it specifically refers to a mathematical model that deeply fuses lidar observations and IMU observations in a unified state space. Model structure: Nodes: Represent the state variables to be estimated, including pose states (rotation matrix R and translation vector t) at different times, velocity states v, and IMU zero-bias states (accelerometer zero-bias). gyroscope zero bias Factors: Represent the constraints of sensor observations on state variables, including: Initial prior factors: provide prior constraints on the initial state; LiDAR odometry factors: generated from the ICP registration results of each key frame in the current frame and key frame sub-graph, constraining the relative pose relationship between adjacent or non-adjacent frames; IMU pre-integration factors: calculated by pre-integration of continuous measurement data of the IMU between two LiDAR frames, constraining the relative motion relationship (relative pose, velocity change) between the two frames and the IMU zero bias state.

[0033] In this scheme, tight coupling is reflected in the fact that the lidar and IMU share the same set of state variables (pose, velocity, and zero bias). The observation factors of both types of sensors simultaneously impose constraints on these state variables in the factor graph, and are solved simultaneously through global nonlinear optimization, achieving deep fusion of observation information in both time and space dimensions. This differs from loosely coupled schemes (where the lidar and IMU estimate pose independently before fusion). Tight coupling can more fully utilize the high-frequency motion information of the IMU and the absolute position constraints of the lidar, achieving complementary enhancement in the geometrically degraded environment of underground mining.

[0034] The Trust Region Nonlinear Optimization Algorithm is a class of iterative optimization methods used to solve large-scale nonlinear least squares problems. In this scheme, it is used to find the optimal state estimate of a tightly coupled factor graph model. In each iteration, the Trust Region method constructs a quadratic approximation model of the objective function near the current point (usually a Taylor second-order expansion) and solves for the optimal solution of this approximation model within the radius of a trust region.

[0035] In particular, in the longitudinal direction (direction of travel) of underground mine corridors, due to the high structural repetition, relying solely on lidar point cloud registration can easily lead to significant drift errors. While in the lateral and vertical directions of the corridors, although features such as pipes and signs provide strong constraints, these constraints are limited to local areas. This significant difference in positioning accuracy across different directions is the essential characteristic that distinguishes underground mine corridors from open environments or structured indoor environments.

[0036] This application utilizes a tightly coupled factor graph model to deeply integrate the lidar odometer factor and the IMU pre-integration factor within a unified optimization framework, fully leveraging the complementary advantages of the two types of sensors in underground mining environments: the lidar, through ICP registration with multiple keyframes in the keyframe sub-graph, provides strong constraints in the transverse and vertical directions of the roadway using stable features such as pipes and signs, effectively suppressing error accumulation in these two directions; the IMU pre-integration factor, through high-frequency inertial measurement, provides continuous motion constraints in the longitudinal direction of the roadway where lidar observation is insufficient, compensating for the weak observability of point cloud registration in the geometrically degraded direction.

[0037] The special significance of the tightly coupled architecture lies in the fact that the lidar odometry factor and the IMU pre-integration factor share state variables (pose, velocity, and IMU bias). All states are solved simultaneously through a nonlinear optimization algorithm, achieving globally consistent fusion of observation information in the spatiotemporal domain. This deep coupling allows the IMU's short-term high-precision characteristics to support lidar point cloud registration when degradation occurs, while the lidar's long-term stability continuously corrects the IMU's bias drift. The two reinforce each other, forming a robust positioning capability against geometric degradation in underground mine roadways.

[0038] The strategy of registering with multiple keyframes in the keyframe subgraph further enhances the robustness of the system in underground mining environments. Underground vehicles frequently start, stop, turn, and climb slopes in roadways, and the point cloud overlap between adjacent frames may be insufficient. However, registering with multiple historical keyframes can establish more spatial constraints. Through the global optimization mechanism of the factor graph, these constraints are transmitted and accumulated in the time dimension, effectively suppressing the cumulative error along the roadway direction in the long corridor environment of underground mines.

[0039] The Trust Region-based nonlinear optimization algorithm minimizes the total residual sum of squares of all factors and adaptively balances the weights of lidar and IMU observations during the solution process. This enables the system to dynamically adjust the fusion strategy according to the actual observation quality of each sensor under different working conditions in underground mines (such as straight sections, turning sections, and areas with dense dynamic obstacles), ensuring the continuity and stability of positioning.

[0040] Furthermore, the initial prior factors provide Gaussian prior constraints on the pose state, velocity state, and IMU zero bias state at the initial time, serving as the starting benchmark for optimization.

[0041] The initial prior factor expression is as follows: ;in, This represents the pose state at the initial moment, including the initial rotation matrix and the initial translation vector; This indicates the initial velocity state; This indicates the initial IMU bias state, including the initial accelerometer bias and the initial gyroscope bias. This represents the prior estimate of the pose state at the initial moment; This represents the prior estimate of the velocity state at the initial moment; This represents the prior estimate of the IMU's zero-bias state at the initial moment;

[0042] Furthermore, the lidar odometry factor uses the relative pose estimate obtained from ICP point cloud registration as the observation value to provide relative motion constraints on the pose state between the current frame and the corresponding key frame, establishes the geometric relationship between the pose of the current frame and the pose of the key frame, and reflects the geometric observation information based on static point cloud.

[0043] The expression for the lidar odometry factor is as follows: ;in, This represents the pose state of the i-th keyframe, including the rotation matrix. Translation vector ; This represents the pose state of the (i+1)th keyframe, including the rotation matrix. Translation vector ; This represents the relative pose observation between the i-th keyframe and the (i+1)-th keyframe obtained through ICP point cloud registration, including relative rotation. and relative translation ;

[0044] Furthermore, the IMU pre-integration factor, based on the relative position increment, relative velocity increment, and relative rotation increment calculated through pre-integration of IMU measurement data, provides continuous motion constraints between the pose state, velocity state, and IMU zero bias state at adjacent time points, reflecting inertial measurement information.

[0045] The expression for the IMU pre-integral factor is as follows: ;in, Let represent the rotation matrix at time i; Let represent the translation vector at time i+1; Let represent the translation vector at time i; This represents the velocity state at time i. This represents the velocity state at time i+1. This represents the time interval between time i and time i+1. represents the IMU zero-bias state at time i; g represents the gravitational acceleration vector; Indicates based on IMU zero bias state The relative rotation increment from time i to time j is obtained through pre-integration calculation; Indicates based on IMU zero bias state The relative velocity increment from time i to time j is obtained through pre-integration calculation;

[0046] Another aspect of this application provides a computer-readable storage medium that stores computer instructions that, when executed by a processor, implement the method provided in this application.

[0047] Compared to existing technologies, the advantages of this application are:

[0048] To address the unique geometric degradation issues inherent in the long corridor environment of underground mines (narrow tunnel structures, dense distribution of roof pipes, and single-sided walls leading to a dominant structure in a single direction, resulting in significant errors in traditional point cloud matching in the degradation direction and affecting positioning accuracy) and the interference from dynamic obstacles caused by frequent movement of mining vehicles and personnel, this application fully utilizes the typical features of underground mine tunnel roof pipes, signs, and other facilities. It automatically extracts these stable linear calibration features through vertical slicing and RANSAC fitting, and uses the average direction of two straight lines for coordinate system rotation correction to align the coordinate system with the main direction of the tunnel. This constructs a precise strip-shaped target area tailored to the characteristics of underground mine pipe layout, preserving key features and filtering irrelevant information such as walls and ground surfaces. A method based on multi-dimensional raster map descriptors (mean, covariance, interquartile range) is used to detect dynamic obstacles such as mining vehicles and pedestrians and remove dynamic point clouds. Finally, a tightly coupled factor graph model is constructed by combining static point clouds and IMU measurement data, and nonlinear optimization is performed by fusing lidar odometer factors and IMU pre-integration factors. This application enables high-precision and robust real-time positioning in typical geometrically degraded environments such as narrow tunnels in underground mines. It effectively solves the problems of positioning error accumulation along the tunnel route and interference from dynamic obstacles underground, thereby improving the positioning stability and reliability under complex working conditions in underground mines. Attached Figure Description

[0049] This application will be further described by way of exemplary embodiments, which will be described in detail with reference to the accompanying drawings. These embodiments are not limiting; in these embodiments, the same reference numerals denote the same structures, wherein:

[0050] Figure 1 This is an exemplary flowchart of a fusion positioning method for a long corridor environment in an underground mine, according to some embodiments of this application;

[0051] Figure 2 This is an exemplary flowchart of distortion removal according to some embodiments of this application. Detailed Implementation

[0052] The methods and systems provided in the embodiments of this application will now be described in detail with reference to the accompanying drawings.

[0053] like Figure 1As shown, sensor data from LiDAR and IMU are acquired, including raw point cloud data from LiDAR and measurement data from IMU. The sensor data is preprocessed and subjected to motion distortion correction to obtain distorted point cloud data. Based on the distorted point cloud data, the target region is extracted through vertical slicing and RANSAC fitting, resulting in a dense point cloud region containing pipes and signs. Based on the distorted point cloud data and the dense point cloud region, a method based on multidimensional grid map descriptors is used for dynamic obstacle detection, and the detected dynamic point cloud is removed to obtain a static point cloud. Based on the static point cloud and IMU measurement data, a tightly coupled factor graph model is constructed, and a nonlinear optimization algorithm is used to solve the tightly coupled factor graph model, outputting the global pose estimation result.

[0054] Specifically, the spatial coordinate relationship between the lidar and the vehicle body, and between the lidar and the IMU is obtained. The original point cloud usually contains noise and outliers. A relatively high-quality point cloud is obtained by downsampling and removing outliers.

[0055] Using high-frequency measurement data from the IMU, rotation and translation compensation are applied to each laser point within the scanning cycle to eliminate motion distortion. The IMU kinematic model and median integral method calculate the relative pose transformation, providing the system with high-frequency continuous motion information. The distortion correction process is as follows: Figure 2 As shown. Through this transformation, all points in the point cloud of this frame are uniformly represented in the radar coordinate system at time t_ref, thereby eliminating the distortion caused by the radar's own motion.

[0056] Having obtained the preprocessed and distortion-corrected point cloud from the lidar, the next step is to extract and segment the dense point cloud containing elements such as pipes and signs:

[0057] S1: Vertical Slice Point Cloud Acquisition: Vertical (Z-axis) filtering is performed on the original LiDAR point cloud Poriginal to extract point cloud slices with heights within a specific range. It is used to capture calibration line features in a scene. Its mathematical expression is: ;

[0058] S2: Double-line feature fitting and parameter extraction: Based on the Random Sample Consensus (RANSAC) algorithm, from sliced ​​point clouds Two linear models, L1 and L2, are fitted. A linear model L is formed by a point... and direction vector The equation is uniquely determined and expressed using point-direction equations: ;in, As a reference point on the straight line, Let be the unit direction vector of the line.

[0059] S3: Calculation of average direction and solution of rotation angle: Calculation of the direction vectors of two lines and The average value is calculated and projected onto the horizontal plane (XY plane) to obtain the average direction vector. :

[0060] ; ;

[0061] Calculate the angle θ between this vector and the positive X-axis, which is the angle that the point cloud as a whole needs to be rotated and corrected.

[0062] ;

[0063] S4: Point cloud coordinate system rotation correction: Construct a rotation transformation matrix around the Z-axis based on the calculated rotation angle θ. This transformation is then applied to the original point cloud. The rotated point cloud is obtained. The transformation matrix and transformation formula are as follows: ;in, and These represent the coordinates of the point before and after the transformation, respectively.

[0064] S5: Calibration line identification and spatial region construction: Calculating the centroid of point clouds of two straight lines. and :

[0065] Compare their Y-coordinate values ​​in the rotated coordinate system, and determine the line corresponding to the centroid with the larger Y-value as the left calibration line, with its Y-coordinate marked as... Using this calibration line as a reference, construct a horizontal strip-shaped target area, which extends from the distance range to the left calibration line. , and vertical height range , Common definition.

[0066] Traverse the original point cloud, determine the horizontal position using the rotated coordinates, and determine the height using the original coordinates. Select points that simultaneously meet both conditions to form the initial pipeline point cloud. : ;in, It is a point The Y-coordinate after rotation transformation.

[0067] To remove dynamic obstacles such as vehicles and people, a region mapping relationship is established between the current frame keyframe sub-image and the historical keyframe sub-image, and raster descriptors are used to identify dynamic objects.

[0068] To reduce computational load, not every frame of laser data needs to be included in the mapping. Using keyframes as the core, data from their neighboring spatiotemporal locations are aggregated to construct a locally consistent and lightweight sub-map. Compared to a global map, the graph, as the basic unit of a local map, can be used for efficient local matching (scan-sub-map matching). Keyframe determination is generally completed by judging the motion distance and rotation angle. Assume the pose of the previous keyframe is... , ;

[0069] The pose of the current keyframe is Then the relative transformation between the two can be expressed as: If relative rotation angle and relative translation distance If the threshold is exceeded, the autonomous vehicle is considered to have entered a new area and provided different observation information. This is then set as a new keyframe and added to the local sub-graph. ;

[0070] The submap is not composed of all historical keyframes, but rather of the N most recent keyframes within a sliding window. As new keyframes are added, the oldest keyframes are removed from the window. The constructed point cloud map is then converted into a raster map.

[0071] Dynamic objects in a tunnel (such as moving pedestrians and vehicles) typically move in a plane, for any grid... Calculate the distance from all points within the grid to the current position of the carrier. Two-dimensional Euclidean distance: ;

[0072] Based on the set of two-dimensional Euclidean distances within the raster, the mean, covariance, and interquartile range (IQR) are calculated to form the raster descriptor. Mean This reflects the central tendency of the distance distribution within the grid. Covariance It characterizes the dispersion of the distance distribution. The interquartile range (IQR) is calculated by taking the 75th percentile. With the 25th percentile Interpolation provides a robust description of the distance distribution. Combining statistics from three different perspectives, the raster descriptor can be defined as: .

[0073] Combining raster descriptors, the pre-screening problem of dynamic regions is transformed into a difference comparison problem between the descriptors of the raster corresponding to the current frame and the keyframe sub-map: ;

[0074] By setting a priori threshold , , The criteria for judging the dynamism of a grid are: .

[0075] Based on the obtained keyframe sub-map, frame-to-map ICP matching can be performed between the feature point cloud of the current frame and the keyframe sub-map to obtain the relative pose estimation result. After obtaining the relative pose estimation, since the system's output error accumulates continuously with the increase of running time and mileage, a global optimization method using factor graphs is used to achieve stable, absolutely high-precision pose estimation.

[0076] Let the state variables of this system be: In the formula, Represents the pose state, including the rotation matrix. With translation vector . . Indicates the speed status. This indicates the zero bias of the IMU, including the zero bias of the accelerometer. Zero bias of gyroscope The factor graphical model, to achieve a global estimate of the state variables, introduces the following factors:

[0077] (1) Initial prior factors

[0078] Introduce a Gaussian prior for pose, velocity, and bias at the initial moment:

[0079] In the formula, , , This represents the initial value.

[0080] (2) LiDAR odometry factor

[0081] Definition of the first Frame and the Between frames, the relative motion obtained through scan matching is Then the odometer factor is:

[0082] ;

[0083] (3) IMU pre-integration factor

[0084] IMU measurements obtain motion constraints between states, including relative positions, through pre-integration. Relative velocity and relative rotation The IMU pre-integration factor is then:

[0085] ;

[0086] The factor graph uses the state variables at each time step as nodes and various factors as edges connecting these nodes, reflecting the modeling approach of the laser inertial positioning method adopted in this paper. Each factor describes the probabilistic constraints between different nodes by associating them. By iteratively solving the factor graph using a Trust Region-based nonlinear optimization method, the linear increment of the state variables can be obtained, thereby updating the system state and ultimately achieving efficient and accurate global pose estimation.

[0087] The above three types of constraints, together with the state vector nodes, construct a factor graph model. Joint optimization using a nonlinear optimization algorithm yields the precise location.

[0088] The foregoing illustrative description of the present application and its embodiments is not restrictive and can be implemented in other specific forms without departing from the spirit or essential characteristics of the present application. The accompanying drawings are only one embodiment of the present application, and the actual structure is not limited thereto. Therefore, if those skilled in the art are inspired by this description and design similar structures and embodiments without departing from the spirit of the present application, such designs should fall within the scope of protection of this application. Furthermore, the word "comprising" does not exclude other elements or steps, and the word "a" preceding an element does not exclude the inclusion of "a plurality" of that element. Terms such as "first," "second," etc., are used to indicate names and do not indicate any specific order.

Claims

1. A fusion positioning method for a long corridor environment in underground mines, characterized in that, include: S1, acquire sensor data from the lidar and IMU, including raw point cloud data from the lidar and measurement data from the IMU; S2, preprocess the sensor data and perform motion distortion correction to obtain distorted point cloud data; S3. Based on the distortion-free point cloud data, the target region is extracted by vertical slicing and RANSAC fitting to obtain a dense point cloud region containing pipes and signs. S4. Based on the distortion-free point cloud data and dense point cloud regions, a method based on multidimensional grid map descriptors is used to detect dynamic obstacles and remove the detected dynamic point clouds to obtain static point clouds. S5. Based on static point cloud and IMU measurement data, construct a tightly coupled factor graph model, solve the tightly coupled factor graph model through a nonlinear optimization algorithm, and output the global pose estimation result. S3 yields a dense point cloud region containing pipes and signs, including: Vertical Z-axis filtering is applied to the distortion-reduced point cloud data to extract heights within a preset range. Point cloud slices are used to obtain linear calibration features of pipes or signs in the long corridor environment of underground mines. Based on the RANSAC algorithm, two straight lines L1 and L2 are fitted from the point cloud slices. Each line is derived from a reference point. The unit direction vector is determined; Calculate the average of the direction vectors of the two lines L1 and L2, and project the average value onto the horizontal XY plane to obtain the average direction vector; Calculate the angle between the average direction vector and the positive X-axis direction, and use it as the rotation correction angle for the distortion-free point cloud data. ; Based on rotation correction angle Construct a rotation transformation matrix around the Z-axis, and use the rotation transformation matrix to perform rotation transformation on the distorted point cloud data to obtain rotated point cloud data, so as to align the point cloud coordinate system with the main direction of the long corridor. Calculate the centroids of the point clouds corresponding to the two straight lines L1 and L2, compare the Y-coordinate values ​​of the centroids in the rotated coordinate system, and take the straight line corresponding to the centroid with the larger Y-coordinate value as the left calibration line. Using the left calibration line as a reference, construct a strip-shaped target area. The strip-shaped target area is defined by the horizontal distance from the left calibration line. and vertical height range Common definition; among which, This represents the minimum horizontal distance threshold from the point to the left calibration line; This represents the maximum horizontal distance threshold from the point to the left calibration line; Indicates the minimum height threshold for pipes and signs in the vertical direction; Indicates the maximum height threshold for pipes and signs in the vertical direction; Iterate through each point in the distortion-free point cloud data, use the rotated coordinates of the corresponding point to determine its horizontal position, and use the original coordinates of the corresponding point to determine its height. Filter out points that simultaneously meet the horizontal distance interval condition and the vertical height interval condition to form a dense point cloud region containing pipes and signs, which is then used as the target region.

2. The fusion positioning method for the long corridor environment of underground mines according to claim 1, characterized in that: S2, obtain the distortion-free point cloud data, including: The spatial coordinate relationships between the lidar and the vehicle body, and between the lidar and the IMU are calibrated to obtain the coordinate transformation matrix between the lidar, the vehicle body, and the IMU. The raw point cloud data from the lidar is downsampled and outlier points are processed to obtain preprocessed point cloud data. Using IMU measurement data, based on the IMU kinematic model and the median integral method, the relative pose transformation of each sampling time relative to the preset reference time tref within the lidar scanning cycle is calculated; Based on the relative pose transformation, rotation compensation and translation compensation are performed on each laser point in the preprocessed point cloud data. The laser points collected at different times within the scanning cycle are uniformly transformed to the lidar coordinate system at the preset reference time tref to eliminate the point cloud distortion caused by the lidar motion and obtain the distortion-free point cloud data.

3. The fusion positioning method for the long corridor environment of underground mines according to claim 1, characterized in that: S4, obtain the static point cloud, including: Calculate the relative rotation angle and relative translation distance between the current frame pose and the previous keyframe pose. When the relative rotation angle or relative translation distance exceeds a preset threshold, set the current frame as a new keyframe. A sliding window mechanism is used to construct a keyframe subgraph, which consists of the latest N keyframes. When a new keyframe is added, the earliest keyframe in the time sequence is removed. The dense point cloud region of each key frame in the keyframe sub-image is converted into a two-dimensional grid map. For each grid in the grid map, the two-dimensional Euclidean distance of all points in the grid to the current scanning position of the lidar on the horizontal XY plane is calculated to obtain the distance set. Based on the distance set, the mean, covariance and interquartile range are calculated respectively to serve as the multidimensional raster map descriptor of the raster; Compare the differences between the multidimensional raster map descriptors of the corresponding grid cells in the current frame and the keyframe sub-image. When any difference in the mean, covariance, and interquartile range exceeds the corresponding preset threshold, the corresponding grid cell is determined to be a dynamic region. Remove the point cloud from the dynamic region to obtain the static point cloud.

4. The fusion positioning method for the long corridor environment of underground mines according to claim 3, characterized in that: S5 outputs the global pose estimation results, including: ICP point cloud registration is performed between the static point cloud of the current frame and the static point cloud of each key frame in the key frame sub-image to obtain the relative pose estimation between the current frame and each key frame. The relative pose estimation is used as the observation value of the lidar odometry factor. Define state variables, including pose state, velocity state and IMU zero bias state. The pose state includes rotation matrix and translation vector. The velocity state represents the three-dimensional velocity vector of the lidar in the world coordinate system. The IMU zero bias state includes accelerometer zero bias and gyroscope zero bias. A tightly coupled factor graphical model is constructed, which includes initial prior factors, lidar odometry factors, and IMU pre-integration factors. A TrustRegion-based nonlinear optimization algorithm is used to iteratively solve the tightly coupled factor graph model. The optimal linear increment of the state variables is obtained by minimizing the total sum of squared residuals of all factors. The state variables are updated based on the optimal linear increment to obtain the optimized pose state, velocity state, and IMU zero bias state, and the global pose estimation result for the current frame is output.

5. The fusion positioning method for the long corridor environment of underground mines according to claim 4, characterized in that: Initial prior factors The expression is as follows: ； in, This indicates the initial pose state. This indicates the initial velocity state; This represents the initial bias state of the IMU; This represents the prior estimate of the pose state at the initial moment; This represents the prior estimate of the velocity state at the initial moment; This represents the prior estimate of the IMU's zero-bias state at the initial moment.

6. The fusion positioning method for the long corridor environment of underground mines according to claim 4, characterized in that: LiDAR odometry factor The expression is as follows: ； in, This represents the pose state of the i-th keyframe; This represents the pose state of the (i+1)th keyframe. This represents the relative pose observation between the i-th keyframe and the (i+1)-th keyframe obtained through ICP point cloud registration.

7. The fusion positioning method for the long corridor environment of underground mines according to claim 4, characterized in that: IMU pre-integrating factor The expression is as follows: ； in, Let represent the rotation matrix at time i; Let represent the translation vector at time i+1; Let represent the translation vector at time i; This represents the velocity state at time i. This represents the velocity state at time i+1. This represents the time interval between time i and time i+1. represents the IMU zero-bias state at time i; g represents the gravitational acceleration vector; Indicates based on IMU zero bias state The relative rotation increment from time i to time j is obtained through pre-integration calculation; Indicates based on IMU zero bias state The relative velocity increment from time i to time j is obtained through pre-integration calculation.

8. A computer-readable storage medium storing computer instructions that, when executed by a processor, implement the method as described in any one of claims 1 to 7.

Images