A coal mine underground monorail hoist vehicle precise positioning method and system

Abstract

This invention discloses a method and system for precise positioning of a monorail crane locomotive in an underground coal mine, comprising: deploying UWB base stations at a first-level spacing in the underground roadway to form a sparse UWB reference network, and deploying RFID tags at key location nodes; synchronously collecting lidar point cloud data, IMU inertial data, UWB ranging data, and RFID tag trigger data through onboard sensors; generating SLAM odometry factors through tight coupling fusion of lidar point cloud data and IMU inertial data, generating UWB ranging factors with dynamic reliability weights based on UWB ranging data, and generating RFID landmark factors based on RFID tag trigger data; constructing a tightly coupled factor graph optimization model using SLAM odometry factors, UWB ranging factors, and RFID landmark factors; and performing nonlinear least squares solution on the tightly coupled factor graph optimization model to output the globally optimal pose of the locomotive at each moment. The method and system of this application reduce infrastructure costs and improve the robustness of the system in complex underground environments.

Description

Technical Field

[0001] This invention relates to the field of underground moving target positioning technology in coal mines, and in particular to a method and system for precise positioning of a monorail crane in underground coal mines. Background Technology

[0002] Monorail cranes in coal mines are crucial equipment for transporting materials and personnel, and their precise positioning is fundamental to achieving automatic driving, intelligent scheduling, and unmanned transportation. Due to the characteristics of the underground coal mine environment, such as narrow and long tunnels, complex structures, lack of satellite signals, poor lighting conditions, and high dust levels, conventional satellite and visual positioning methods are difficult to apply. Currently, the mainstream positioning technologies in this field and their inherent challenges are as follows: Ultra-wideband (UWB) technology boasts advantages such as high temporal resolution, strong penetration, and superior resistance to multipath interference compared to narrowband systems, making it widely used for locating moving targets in underground coal mines. However, in practical applications, UWB technology faces fundamental challenges: First, there is a contradiction between accuracy and environmental resistance. Coal mine underground roadways can be considered typical unstructured, highly multipath environments. Reflections from the roadway walls, obstructions from support equipment, and interference from passing personnel and vehicles all lead to severe non-line-of-sight errors and multipath effects in UWB signals. Actual measurements show that in a typical coal mine underground environment, the ranging error of UWB can deteriorate from 10-20 cm under line-of-sight conditions to 1-3 meters under non-line-of-sight conditions.

[0003] Secondly, there is a conflict between deployment costs and positioning accuracy. To maintain a positioning accuracy of 20-30 centimeters in long tunnels, existing technologies are forced to adopt a strategy of high-density deployment of UWB base stations, typically requiring one base station every 100-200 meters to ensure line-of-sight communication probability and geometric positioning accuracy. Taking a 5-kilometer-long transport tunnel as an example, a pure UWB solution would require the deployment of 25-50 base stations, resulting in high infrastructure costs, complex engineering implementation, and a large workload for subsequent maintenance.

[0004] Third, there is the limitation of limited functionality. UWB solutions can typically only provide location information and cannot output the vehicle's attitude (orientation) information. They also have difficulty perceiving the surrounding environment, making it difficult to meet the needs of advanced applications such as autonomous driving for six degrees of freedom pose and environmental perception.

[0005] Simultaneous Localization and Mapping (SLAM) technology uses LiDAR or visual sensors to perceive the environment and achieve autonomous localization and map building. It has advantages such as not relying on external infrastructure, providing centimeter-level high-precision relative positioning in a short period of time, being able to output complete six-degree-of-freedom attitude information, and being able to generate environmental maps simultaneously.

[0006] However, SLAM technology has inherent defects in long-term operation in coal mines: First, in long-distance, feature-sparse straight roadways, due to the similar structure on both sides of the roadway and the scarcity of feature points, the inter-frame matching constraints of the lidar are insufficient, causing the odometer error to gradually accumulate over time; Second, this accumulated error cannot be eliminated by its own perception, and the longer the operating distance, the greater the deviation of the positioning result from the true position, which cannot meet the requirements of long-endurance accurate positioning; Third, after the system starts up or tracking is lost, the pure SLAM system needs a long time to complete repositioning, or it cannot recover without external assistance.

[0007] A few studies have attempted to loosely combine inertial navigation and UWB, that is, to calculate the UWB position and the inertial navigation position separately and then fuse them. Although this loose combination method can improve the positioning continuity to some extent, it cannot fundamentally solve the problems of UWB outlier contamination and SLAM cumulative error, and it fails to fully utilize the complementary advantages of each sensor.

[0008] In summary, existing technologies cannot simultaneously meet the comprehensive requirements of high precision, zero cumulative error, low cost, strong robustness, and attitude output for positioning of monorail cranes in coal mines. Therefore, there is an urgent need for a new method that can integrate multi-source information, overcome the limitations of single sensors, and achieve high-precision drift-free positioning under sparse infrastructure conditions. Summary of the Invention

[0009] To address the technical problems existing in the background art, this invention proposes a method and system for precise positioning of monorail cranes in coal mines.

[0010] This invention proposes a method for precise positioning of a monorail crane in an underground coal mine, comprising the following steps: S1. Deploy UWB base stations in the underground roadway at a first-level spacing to form a sparse UWB reference network, and deploy RFID tags at key location nodes. S2. Simultaneously collect lidar point cloud data, IMU inertial data, UWB ranging data, and RFID tag trigger data through vehicle-mounted sensors; S3. Based on the tight coupling fusion of LiDAR point cloud data and IMU inertial data, a SLAM odometry factor is generated. Based on UWB ranging data, a UWB ranging factor with dynamic reliability weight is generated. Based on RFID tag trigger data, an RFID landmark factor is generated. The dynamic reliability weight is calculated based on the signal-to-noise ratio of the UWB signal, multipath index, geometric accuracy factor, and the consistency between the UWB solution position and the SLAM prediction position. Its value is between 0 and 1. The RFID landmark factor is the absolute position observation factor that connects the locomotive pose node at the trigger time with the precise global coordinates of the tag. S4. Construct a tightly coupled factor graph optimization model using SLAM odometry factor, UWB ranging factor and RFID landmark factor. S5. Perform nonlinear least squares solution on the tightly coupled factor graph optimization model to output the global optimal pose of the locomotive at each time step.

[0011] Preferably, the step of generating SLAM odometry factors through tight coupling fusion of lidar point cloud data and IMU inertial data specifically includes: Feature extraction is performed on the current lidar point cloud data to obtain edge feature points and planar feature points; Register the edge feature points and planar feature points at the current moment with the feature points at the previous moment to obtain the relative pose transformation in the lidar coordinate system; Pre-integrate the IMU inertial data between two consecutive time points to obtain the relative rotation, relative velocity, and relative position in the IMU coordinate system, which serve as the IMU relative motion estimate. At the same time, calculate the covariance matrix corresponding to the IMU relative motion estimate. The relative pose transformation of the lidar and the relative motion estimation of the IMU are tightly coupled and fused to construct a relative pose constraint factor connecting adjacent pose nodes, which serves as the SLAM odometry factor. The covariance matrix is ​​used to determine the weight of the IMU observations during the fusion process.

[0012] Preferably, the step of generating a UWB ranging factor with dynamic confidence weights based on UWB ranging data specifically includes: Obtain the raw ranging values ​​between the UWB tag on the locomotive and each visible UWB base station at the current moment. The raw ranging values ​​include signal-to-noise ratio and multipath index. Calculate the signal quality reliability component based on the signal-to-noise ratio and multipath indices attached to the original ranging values; The geometric accuracy factor is calculated based on the estimated pose of the locomotive and the spatial geometric distribution of each visible UWB base station, and the geometric configuration confidence component is calculated based on the geometric accuracy factor. The UWB raw ranging value at the current moment is solved to obtain the UWB solution position. The UWB solution position is compared with the SLAM predicted position based on the SLAM odometry factor prediction, and the Euclidean distance difference between the two is calculated. The consistency confidence component is calculated based on the Euclidean distance difference. The signal quality confidence component, geometric configuration confidence component, and consistency confidence component are fused to calculate a dynamic confidence weight between 0 and 1. The original ranging value is associated with the dynamic confidence weight to construct a ranging constraint factor that connects the locomotive pose node at the current moment with the corresponding UWB base station node, which serves as the UWB ranging factor with dynamic confidence weight.

[0013] Preferably, the formula for calculating the dynamic credibility weight is: ; in, Signal-to-noise ratio; It is a multipath index; Geometric precision factor; To calculate the Euclidean distance difference between the UWB location and the SLAM predicted location, This is a predefined fusion function whose output value is between 0 and 1.

[0014] Preferably, the generation of RFID landmark factors based on RFID tag trigger data specifically includes: By reading passive RFID tags deployed at key locations in the alleyway using an on-board RFID reader, the unique identifier of the tag and the trigger time can be obtained. Based on the tag's unique identifier, the precise global coordinates corresponding to that tag's unique identifier are retrieved from a pre-built RFID tag map database; The locomotive pose node corresponding to the trigger time is associated with the precise global coordinates to construct an absolute position observation factor with high confidence and fixed weight, which serves as the RFID landmark factor.

[0015] Preferably, step S4 specifically includes: The three-dimensional position and three-dimensional attitude of the locomotive at discrete timestamps are defined as pose nodes to be optimized, and the three-dimensional coordinates of each UWB base station deployed in the alley are defined as fixed base station nodes. Two pose nodes at adjacent time points are connected by SLAM odometry factors to form local relative constraint edges describing the continuous motion trajectory of the locomotive; The pose node at the current moment is connected to the visible UWB base station node through a UWB ranging factor with dynamic confidence weight, forming a global absolute constraint edge describing the distance observation between the locomotive and the base station. The pose node at the trigger moment is directly associated with the global coordinates of the corresponding RFID tag through the RFID landmark factor to form a strongly constrained absolute position observation edge describing the locomotive passing through the precise landmark. By combining pose nodes, base station nodes, and various constraint edges consisting of SLAM odometry factors, UWB ranging factors, and RFID landmark factors, a tightly coupled factor graph optimization model is constructed with pose nodes as vertices and multi-source constraint factors as edges.

[0016] Preferably, step S5 specifically includes: All pose nodes in the tightly coupled factor graph optimization model are taken as variables to be optimized, and the constraints provided by all SLAM odometry factors, UWB ranging factors and RFID landmark factors are taken as error terms. A nonlinear least squares objective function with pose nodes as optimization variables is constructed to minimize the weighted sum of squares of all error terms. The objective function is solved using the Levenberg-Marquardt iterative optimization algorithm. In each iteration, the Jacobian matrix of each factor error is calculated based on the current pose estimate, the incremental normal equation is constructed and the pose update amount is solved to update the current pose estimate. The iteration is terminated when the number of iterations reaches the preset maximum value or the pose update amount is less than the preset threshold. The optimized three-dimensional position and three-dimensional attitude of the locomotive at each moment are output as the global optimal pose.

[0017] Preferably, the first spacing is greater than 200 meters.

[0018] Preferably, step S1 specifically includes: UWB base stations are deployed at intervals of 200 to 300 meters along straight sections of underground roadways. The three-dimensional coordinates of each UWB base station are pre-calibrated and stored in the system database, forming a sparse UWB reference network covering the entire transport roadway, which is used to provide a global absolute position reference for locomotives. Passive RFID tags are buried at intersections, loading and unloading stations, and the starting point of curves in the roadway, so that each RFID tag has a unique identifier. The precise global coordinates of the tags are associated with the unique identifier and stored in the RFID tag map database to provide instantaneous absolute position anchor points when locomotives pass by.

[0019] This invention proposes a precise positioning system for a monorail crane in an underground coal mine, comprising: The sparse UWB base station network consists of multiple UWB base stations deployed in underground roadways at intervals greater than 200 meters. Each UWB base station has pre-calibrated three-dimensional coordinates stored in the system database, which are used to provide a global absolute position reference for the locomotive. The RFID tag network consists of multiple passive RFID tags deployed at road intersections, loading and unloading stations, and the starting points of curves. Each RFID tag has a unique identifier, and its precise global coordinates are associated with the unique identifier and stored in the RFID tag map database to provide instantaneous absolute position anchor points when a locomotive passes by. The vehicle-mounted terminal, installed on a monorail crane, includes a lidar, an inertial measurement unit, UWB tags, an RFID reader, and a processing unit. The output end of the lidar is connected to the first input end of the processing unit, and is used to collect point cloud data of the roadway environment around the locomotive and transmit it to the processing unit. The output of the inertial measurement unit is connected to the second input of the processing unit, and is used to collect the three-axis acceleration and three-axis angular velocity data of the locomotive and transmit them to the processing unit. The output end of the UWB tag is connected to the third input end of the processing unit for communicating with the UWB base station, obtaining the original ranging value between the locomotive and each visible base station and transmitting it to the processing unit. The original ranging value includes the signal-to-noise ratio and multipath index. The output of the RFID reader is connected to the fourth input of the processing unit, and is used to read the unique identifier of the RFID tag and record the trigger time when the locomotive passes by the RFID tag, and transmit the unique identifier and trigger time to the processing unit. The processing unit is used to receive and process data transmitted from the LiDAR, inertial measurement unit, UWB tag, and RFID reader, and execute a fusion positioning algorithm, including: generating SLAM odometry factors based on tightly coupled fusion of LiDAR point cloud data and IMU inertial data; generating UWB ranging factors with dynamic reliability weights based on UWB ranging data; and generating RFID landmark factors based on RFID tag trigger data. The dynamic reliability weights are calculated based on the signal-to-noise ratio, multipath index, geometric accuracy factor, and consistency between the UWB calculated position and the SLAM predicted position of the UWB signal, and their values ​​are between 0 and 1. The RFID landmark factors are absolute position observation factors that connect the locomotive pose node at the trigger time with the precise global coordinates of the tag. A tightly coupled factor graph optimization model is constructed using the SLAM odometry factors, UWB ranging factors, and RFID landmark factors. The tightly coupled factor graph optimization model is solved using nonlinear least squares to output the global optimal pose of the locomotive at each moment.

[0020] This invention presents a precise positioning method and system for monorail cranes in coal mines. By constructing a tightly coupled factor graph optimization model with SLAM odometry factors as the main body and UWB ranging factors and RFID landmark factors with dynamic reliability weights as global correction sources, it achieves deep fusion of high-precision local relative positioning and drift-free global absolute reference. Under conditions of sparsely deployed UWB base stations and RFID tags at key locations, it suppresses the cumulative error of SLAM during long-distance operation and intelligently filters out abnormal observations when UWB is interfered with through a multi-dimensional dynamic weight mechanism. This results in accurate six-DOF pose output with no cumulative error across the entire operating range, while simultaneously generating a high-precision environmental map. This significantly reduces infrastructure deployment costs while improving the system's robustness and reliability in complex underground environments, and also exhibits good sensor scalability and scene generalization capabilities. Attached Figure Description

[0021] Figure 1This is a schematic diagram illustrating the workflow of a precise positioning method for a monorail crane in a coal mine, as proposed in this invention. Figure 2 This is a schematic diagram of the processing flow of one embodiment of the precise positioning method for a monorail crane locomotive in a coal mine proposed in this invention; Figure 3 This is a schematic diagram of a tightly coupled factor graph optimization model for one embodiment of a precise positioning method for a monorail crane locomotive in an underground coal mine proposed in this invention. Figure 4 This is a UWB dynamic weight adjustment logic diagram for one embodiment of the precise positioning method for a monorail crane in an underground coal mine proposed in this invention. Detailed Implementation

[0022] Reference Figures 1-4 The present invention proposes a method for precise positioning of a monorail crane in an underground coal mine, comprising the following steps: S1. Deploy UWB base stations at a first-level spacing in the underground roadway to form a sparse UWB reference network, and deploy RFID tags at key location nodes.

[0023] In this embodiment, the first spacing is greater than 200 meters.

[0024] In this embodiment, step S1 specifically includes: UWB base stations are deployed at intervals of 200 to 300 meters along straight sections of underground roadways. The three-dimensional coordinates of each UWB base station are pre-calibrated and stored in the system database, forming a sparse UWB reference network covering the entire transport roadway, which is used to provide a global absolute position reference for locomotives. Passive RFID tags are buried at intersections, loading and unloading stations, and the starting points of curves in the tunnels, so that each RFID tag has a unique identifier. The precise global coordinates of the tags are associated with the unique identifier and stored in the RFID tag map database to provide instantaneous absolute position anchor points when locomotives pass by.

[0025] Specifically, UWB base stations are deployed at 250-meter intervals along straight sections of the underground roadway. The three-dimensional coordinates of each UWB base station are pre-calibrated and stored in the system database, forming a sparse UWB reference network covering the entire transportation roadway. Simultaneously, passive RFID tags are embedded at roadway intersections, loading / unloading stations, and the starting points of curves. Each RFID tag has a unique identifier, and its precise global coordinates are associated with and stored in the RFID tag map database.

[0026] S2. Simultaneously collect LiDAR point cloud data, IMU inertial data, UWB ranging data, and RFID tag trigger data through vehicle-mounted sensors.

[0027] Specifically, after the locomotive starts, the onboard terminal synchronously collects data from various sensors at a frequency of 10Hz: The lidar collects point cloud data of the tunnel environment at the current moment; The inertial measurement unit acquires inertial data from the IMU, including triaxial acceleration and triaxial angular velocity; UWB tags communicate with visible UWB base stations to obtain raw ranging values ​​and their associated signal-to-noise ratio and multipath metrics; The RFID reader is triggered when the locomotive passes the RFID tag, and reads the tag's unique identifier and the trigger time; All data is accompanied by a unified timestamp and transmitted to the processing unit for further processing.

[0028] S3. Based on the tight coupling fusion of LiDAR point cloud data and IMU inertial data, SLAM odometry factors are generated. Based on UWB ranging data, UWB ranging factors with dynamic reliability weights are generated. Based on RFID tag trigger data, RFID landmark factors are generated. The dynamic reliability weights are calculated based on the signal-to-noise ratio of the UWB signal, multipath index, geometric accuracy factor, and the consistency between the UWB solution position and the SLAM prediction position. Their values ​​are between 0 and 1. The RFID landmark factor is the absolute position observation factor that connects the locomotive pose node at the trigger time with the precise global coordinates of the tag.

[0029] In this embodiment, SLAM odometry factors are generated by tightly coupled fusion of lidar point cloud data and IMU inertial data, specifically including: Feature extraction is performed on the current lidar point cloud data to obtain edge feature points and planar feature points; Register the edge feature points and planar feature points at the current moment with the feature points at the previous moment to obtain the relative pose transformation in the lidar coordinate system; Pre-integrate the IMU inertial data between two consecutive time points to obtain the relative rotation, relative velocity, and relative position in the IMU coordinate system, which serve as the IMU relative motion estimate. At the same time, calculate the covariance matrix corresponding to the IMU relative motion estimate. The relative pose transformation of the lidar and the relative motion estimation of the IMU are tightly coupled and fused to construct a relative pose constraint factor connecting adjacent pose nodes, which serves as the SLAM odometry factor. The covariance matrix is ​​used to determine the weight of the IMU observations during the fusion process.

[0030] Specifically, generating SLAM odometry factors includes the following sub-steps: First, feature extraction is performed on the current lidar point cloud data. By calculating the curvature of the point cloud, point clouds with curvature greater than a preset threshold are classified as edge feature points, and point clouds with curvature less than a preset threshold are classified as planar feature points.

[0031] Secondly, the edge feature points and planar feature points at the current moment are registered with the feature points at the previous moment: the edge feature points are matched by the Iterative Closest Point (ICP) algorithm, and the planar feature points are matched by the point-to-plane matching algorithm, so as to obtain the relative pose transformation in the lidar coordinate system, including the relative rotation matrix and the relative translation vector.

[0032] Next, pre-integration is performed on the IMU inertial data between two consecutive time points. Assuming the IMU measurements between time i and time i+1 include acceleration and angular velocity, integration yields the relative rotation, relative velocity, and relative position in the IMU coordinate system, serving as the IMU relative motion estimate. Simultaneously, based on the IMU's noise model, the covariance matrix corresponding to the IMU relative motion estimate is calculated using the covariance propagation law. This covariance matrix characterizes the uncertainty of the IMU relative motion estimate.

[0033] Finally, the relative pose transformation of the lidar and the relative motion estimation of the IMU are tightly coupled and fused. Specifically, a graph optimization framework is used to construct a relative pose constraint factor connecting adjacent pose nodes Xi and Xi+1, which serves as the SLAM odometry factor. The error function of this SLAM odometry factor is defined as the residual between the lidar observation and the IMU prediction, where the covariance matrix is ​​used to determine the weight of the IMU observations during the fusion process.

[0034] In this embodiment, generating a UWB ranging factor with dynamic confidence weights based on UWB ranging data specifically includes: Obtain the raw ranging values ​​between the UWB tag on the locomotive and each visible UWB base station at the current moment. The raw ranging values ​​include the signal-to-noise ratio and multipath index. Calculate the signal quality reliability component based on the signal-to-noise ratio and multipath indices attached to the original ranging values; The geometric accuracy factor is calculated based on the current locomotive's estimated pose and the spatial geometric distribution of each visible UWB base station, and the geometric configuration confidence component is calculated based on the geometric accuracy factor. The UWB raw ranging value at the current moment is solved to obtain the UWB solution position. The UWB solution position is compared with the SLAM predicted position based on SLAM odometry factor prediction, and the Euclidean distance difference between the two is calculated. The consistency confidence component is calculated based on the Euclidean distance difference. The signal quality confidence component, geometric configuration confidence component, and consistency confidence component are fused to calculate a dynamic confidence weight between 0 and 1. The original ranging value is associated with the dynamic confidence weight to construct a ranging constraint factor that connects the locomotive pose node at the current moment with the corresponding UWB base station node, which serves as the UWB ranging factor with dynamic confidence weight.

[0035] Specifically, the formula for calculating the dynamic credibility weight is as follows: ; in, Signal-to-noise ratio; It is a multipath index; Geometric precision factor; To calculate the Euclidean distance difference between the UWB location and the SLAM predicted location, This is a predefined fusion function whose output value is between 0 and 1.

[0036] Specifically, such as Figure 4 As shown, generating a UWB ranging factor with dynamic confidence weights specifically includes the following sub-steps: First, the raw ranging values ​​between the UWB tag on the locomotive and each visible UWB base station at the current time are obtained, and the raw ranging values ​​include the signal-to-noise ratio and multipath index.

[0037] Secondly, the signal quality reliability component is calculated based on the signal-to-noise ratio (SNR) and multipath index attached to the original ranging value. In this embodiment, a piecewise linear function is used: when the SNR is higher than the first threshold (set to 30dB), the signal quality reliability component = 1; when the SNR is lower than the first threshold, the signal quality reliability component decreases linearly with the SNR; and when the multipath index exceeds the second threshold (set to 0.3), the signal quality reliability component is multiplied by an attenuation coefficient of 0.5.

[0038] Next, the geometric precision factor (DOP) is calculated based on the estimated pose of the locomotive and the spatial geometric distribution of each visible UWB base station, and the geometric configuration confidence component is calculated based on the DOP value. This embodiment uses ,in, This is an adjustment factor; the larger the DOP value, the better. The smaller.

[0039] Then, the UWB raw ranging value at the current time is used to perform least squares calculation to obtain the UWB calculated position. The UWB-derived location is compared with the SLAM-predicted location based on SLAM odometry factor prediction. Compare the two and calculate the Euclidean distance difference. .according to Calculate the consistency confidence component :when Less than the third threshold (Assuming a height of 1.0 meter) ;when Greater than hour, .

[0040] Finally, the signal quality confidence component, geometric configuration confidence component, and consistency confidence component are fused. In this embodiment, the fusion function f uses a weighted geometric average to obtain a dynamic confidence weight between 0 and 1. The original ranging value is associated with the dynamic confidence weight to construct a connection between the current locomotive pose node Xk and the corresponding UWB base station node. The ranging constraint factor is used as a UWB ranging factor with dynamic confidence weight.

[0041] In this embodiment, generating RFID landmark factors based on RFID tag trigger data specifically includes: By reading passive RFID tags deployed at key locations in the alleyway using an on-board RFID reader, the unique identifier of the tag and the trigger time can be obtained. Based on the tag's unique identifier, the precise global coordinates corresponding to that tag's unique identifier are retrieved from a pre-built RFID tag map database; The locomotive pose node corresponding to the trigger time is associated with the precise global coordinates to construct an absolute position observation factor with high confidence and fixed weight, which serves as the RFID landmark factor.

[0042] Specifically, when a locomotive passes by passive RFID tags deployed at key location nodes, the RFID reader reads the tag's unique identifier ID and records the trigger time t. The processing unit queries the precise global coordinates corresponding to the tag from a pre-built RFID tag map database based on the tag's unique identifier ID. The locomotive pose node Xt corresponding to the trigger time t is associated with the precise global coordinates to construct an absolute position observation factor with high confidence and fixed weight, which serves as the RFID landmark factor. In this embodiment, the fixed weight is set to 100.

[0043] S4. Construct a tightly coupled factor graph optimization model using SLAM odometer factor, UWB ranging factor, and RFID landmark factor.

[0044] In this embodiment, step S4 specifically includes: The three-dimensional position and three-dimensional attitude of the locomotive at discrete timestamps are defined as pose nodes to be optimized, and the three-dimensional coordinates of each UWB base station deployed in the alley are defined as fixed base station nodes. Two pose nodes at adjacent time points are connected by SLAM odometry factors to form local relative constraint edges describing the continuous motion trajectory of the locomotive; The pose node at the current moment is connected to the visible UWB base station node through a UWB ranging factor with dynamic confidence weight, forming a global absolute constraint edge describing the distance observation between the locomotive and the base station. The pose node at the trigger moment is directly associated with the global coordinates of the corresponding RFID tag through the RFID landmark factor to form a strongly constrained absolute position observation edge describing the locomotive passing through the precise landmark. By combining pose nodes, base station nodes, and various constraint edges consisting of SLAM odometry factors, UWB ranging factors, and RFID landmark factors, a tightly coupled factor graph optimization model is constructed with pose nodes as vertices and multi-source constraint factors as edges.

[0045] Specifically, such as Figure 3 As shown, the construction of the tightly coupled factor graph optimization model includes the following sub-steps: First, the three-dimensional position (x, y, z) and three-dimensional pose (roll, pitch, yaw) of the locomotive at discrete timestamps are defined as pose nodes X1, X2, ..., Xn to be optimized. The three-dimensional coordinates of each UWB base station deployed in the tunnel are defined as fixed base station nodes B1, B2, ..., Bn. m .

[0046] Secondly, two pose nodes Xi and Xi+1 at adjacent times are connected by SLAM odometry factors to form local relative constraint edges describing the continuous motion trajectory of the locomotive.

[0047] Next, the pose node Xk at the current moment is compared with the visible UWB base station node B. j By connecting UWB ranging factors with dynamic confidence weights, a global absolute constraint edge describing the distance observation between the locomotive and the base station is formed.

[0048] Then, the pose node Xt at the trigger moment is directly associated with the corresponding global coordinates of the RFID tag through the RFID landmark factor to form a strongly constrained absolute position observation edge describing the locomotive passing through the precise landmark.

[0049] Finally, the pose nodes, base station nodes, and various constraint edges consisting of SLAM odometry factors, UWB ranging factors, and RFID landmark factors are combined to construct a tightly coupled factor graph optimization model with pose nodes as vertices and multi-source constraint factors as edges. The total error function of this model is the weighted sum of squares of all factor error terms.

[0050] It should be noted that during the construction of the tightly coupled factor graph optimization model, when the locomotive travels to a previously traversed alleyway area, the SLAM front-end detects the similarity between the current scene and the historical keyframe scene by scanning the context or using a bag-of-words model. This automatically triggers a loop closure detection event and generates a loop closure detection factor. The loop closure detection factor directly connects the pose node at the current moment with the corresponding pose node at the historical moment, forming a spatial consistency constraint edge describing when the locomotive revisits the same physical location. The error function of this constraint edge is calculated based on the relative transformation between the current pose and the historical pose, and is added to the factor graph optimization model with a high-confidence fixed weight. This is used to pull the current trajectory toward the historical trajectory during the global optimization process, thereby effectively correcting the drift error accumulated over a long period of operation and eliminating trajectory inconsistencies before and after loop closure.

[0051] S5. Perform nonlinear least squares solution on the tightly coupled factor graph optimization model to output the global optimal pose of the locomotive at each time step.

[0052] In this embodiment, step S5 specifically includes: All pose nodes in the tightly coupled factor graph optimization model are taken as variables to be optimized, and the constraints provided by all SLAM odometry factors, UWB ranging factors and RFID landmark factors are taken as error terms. A nonlinear least squares objective function with pose nodes as optimization variables is constructed to minimize the weighted sum of squares of all error terms. The Levenberg-Marquardt iterative optimization algorithm is used to solve the objective function. In each iteration, the Jacobian matrix of each factor error is calculated based on the current pose estimate, the incremental normal equation is constructed and the pose update amount is solved to update the current pose estimate. The iteration is terminated when the number of iterations reaches the preset maximum value or the pose update amount is less than the preset threshold. The optimized three-dimensional position and three-dimensional attitude of the locomotive at each moment are output as the global optimal pose.

[0053] Reference Figures 1-4 The present invention proposes a precise positioning system for a monorail crane in an underground coal mine, comprising: The sparse UWB base station network consists of multiple UWB base stations deployed in underground roadways at intervals greater than 200 meters. Each UWB base station has pre-calibrated three-dimensional coordinates stored in the system database, which are used to provide a global absolute position reference for the locomotive. The RFID tag network consists of multiple passive RFID tags deployed at road intersections, loading and unloading stations, and the starting points of curves. Each RFID tag has a unique identifier, and its precise global coordinates are associated with the unique identifier and stored in the RFID tag map database to provide instantaneous absolute position anchor points when a locomotive passes by. The vehicle-mounted terminal, installed on a monorail crane, includes a lidar, an inertial measurement unit, UWB tags, an RFID reader, and a processing unit. The output end of the lidar is connected to the first input end of the processing unit to collect point cloud data of the roadway environment around the locomotive and transmit it to the processing unit. The output of the inertial measurement unit is connected to the second input of the processing unit to collect the three-axis acceleration and three-axis angular velocity data of the locomotive and transmit them to the processing unit. The output of the UWB tag is connected to the third input of the processing unit to communicate with the UWB base station, obtain the raw ranging value between the locomotive and each visible base station, and transmit it to the processing unit. The raw ranging value includes the signal-to-noise ratio and multipath index. The output of the RFID reader is connected to the fourth input of the processing unit. It is used to read the unique identifier of the RFID tag and record the trigger time when the locomotive passes by the RFID tag, and transmit the unique identifier and trigger time to the processing unit. The processing unit receives and processes data transmitted from the LiDAR, inertial measurement unit, UWB tags, and RFID readers, and executes a fusion positioning algorithm, including: generating SLAM odometry factors through tight coupling fusion based on LiDAR point cloud data and IMU inertial data; generating UWB ranging factors with dynamic reliability weights based on UWB ranging data; and generating RFID landmark factors based on RFID tag trigger data. The dynamic reliability weights are calculated based on the signal-to-noise ratio, multipath index, geometric accuracy factor, and consistency between the UWB calculated position and the SLAM predicted position of the UWB signal, and their values ​​are between 0 and 1. The RFID landmark factor is an absolute position observation factor that connects the locomotive pose node at the trigger time with the precise global coordinates of the tag. A tight coupling factor graph optimization model is constructed using the SLAM odometry factor, UWB ranging factor, and RFID landmark factor. The tight coupling factor graph optimization model is solved using nonlinear least squares to output the global optimal pose of the locomotive at each moment.

[0054] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for precise positioning of a monorail crane in an underground coal mine, characterized in that, Includes the following steps: S1. Deploy UWB base stations in the underground roadway at a first-level spacing to form a sparse UWB reference network, and deploy RFID tags at key location nodes. S2. Simultaneously collect lidar point cloud data, IMU inertial data, UWB ranging data, and RFID tag trigger data through vehicle-mounted sensors; S3. Based on the tight coupling fusion of LiDAR point cloud data and IMU inertial data, a SLAM odometry factor is generated. Based on UWB ranging data, a UWB ranging factor with dynamic reliability weight is generated. Based on RFID tag trigger data, an RFID landmark factor is generated. The dynamic reliability weight is calculated based on the signal-to-noise ratio of the UWB signal, multipath index, geometric accuracy factor, and the consistency between the UWB solution position and the SLAM prediction position. Its value is between 0 and 1. The RFID landmark factor is the absolute position observation factor that connects the locomotive pose node at the trigger time with the precise global coordinates of the tag. S4. Construct a tightly coupled factor graph optimization model using SLAM odometry factor, UWB ranging factor and RFID landmark factor. S5. Perform nonlinear least squares solution on the tightly coupled factor graph optimization model to output the global optimal pose of the locomotive at each time step.

2. The method for precise positioning of a monorail crane in an underground coal mine according to claim 1, characterized in that, The process of generating SLAM odometry factors through tight coupling fusion of lidar point cloud data and IMU inertial data specifically includes: Feature extraction is performed on the current lidar point cloud data to obtain edge feature points and planar feature points; By registering the edge feature points and planar feature points at the current moment with the feature points at the previous moment, the relative pose transformation in the lidar coordinate system is obtained. Pre-integrate the IMU inertial data between two consecutive time points to obtain the relative rotation, relative velocity, and relative position in the IMU coordinate system, which serve as the IMU relative motion estimate. At the same time, calculate the covariance matrix corresponding to the IMU relative motion estimate. The relative pose transformation of the lidar and the relative motion estimation of the IMU are tightly coupled and fused to construct a relative pose constraint factor connecting adjacent pose nodes, which serves as the SLAM odometry factor. The covariance matrix is ​​used to determine the weight of the IMU observations during the fusion process.

3. The method for precise positioning of a monorail crane in an underground coal mine according to claim 1, characterized in that, The generation of UWB ranging factors with dynamic confidence weights based on UWB ranging data specifically includes: Obtain the raw ranging values ​​between the UWB tag on the locomotive and each visible UWB base station at the current moment. The raw ranging values ​​include signal-to-noise ratio and multipath index. Calculate the signal quality reliability component based on the signal-to-noise ratio and multipath indices attached to the original ranging values; The geometric accuracy factor is calculated based on the estimated pose of the locomotive and the spatial geometric distribution of each visible UWB base station, and the geometric configuration confidence component is calculated based on the geometric accuracy factor. The UWB raw ranging value at the current moment is solved to obtain the UWB solution position. The UWB solution position is compared with the SLAM predicted position based on the SLAM odometry factor prediction, and the Euclidean distance difference between the two is calculated. The consistency confidence component is calculated based on the Euclidean distance difference. The signal quality confidence component, geometric configuration confidence component, and consistency confidence component are fused to calculate a dynamic confidence weight between 0 and 1. The original ranging value is associated with the dynamic confidence weight to construct a ranging constraint factor that connects the locomotive pose node at the current moment with the corresponding UWB base station node, which serves as the UWB ranging factor with dynamic confidence weight.

4. The method for precise positioning of a monorail crane in a coal mine according to claim 3, characterized in that, The formula for calculating the dynamic credibility weight is as follows: ； in, Signal-to-noise ratio; It is a multipath index; Geometric precision factor; To calculate the Euclidean distance difference between the UWB location and the SLAM predicted location, This is a predefined fusion function whose output value is between 0 and 1.

5. The method for precise positioning of a monorail crane in an underground coal mine according to claim 1, characterized in that, The generation of RFID landmark factors based on RFID tag-triggered data specifically includes: By reading passive RFID tags deployed at key locations in the alleyway using an on-board RFID reader, the unique identifier of the tag and the trigger time can be obtained. Based on the tag's unique identifier, the precise global coordinates corresponding to that tag's unique identifier are retrieved from a pre-built RFID tag map database; The locomotive pose node corresponding to the trigger time is associated with the precise global coordinates to construct an absolute position observation factor with high confidence and fixed weight, which serves as the RFID landmark factor.

6. The method for precise positioning of a monorail crane in an underground coal mine according to claim 1, characterized in that, Step S4 specifically includes: The three-dimensional position and three-dimensional attitude of the locomotive at discrete timestamps are defined as pose nodes to be optimized, and the three-dimensional coordinates of each UWB base station deployed in the alley are defined as fixed base station nodes. Two pose nodes at adjacent time points are connected by SLAM odometry factors to form local relative constraint edges describing the continuous motion trajectory of the locomotive; The pose node at the current moment is connected to the visible UWB base station node through a UWB ranging factor with dynamic confidence weight, forming a global absolute constraint edge describing the distance observation between the locomotive and the base station. The pose node at the trigger moment is directly associated with the global coordinates of the corresponding RFID tag through the RFID landmark factor to form a strongly constrained absolute position observation edge describing the locomotive passing through the precise landmark. By combining pose nodes, base station nodes, and various constraint edges consisting of SLAM odometry factors, UWB ranging factors, and RFID landmark factors, a tightly coupled factor graph optimization model is constructed with pose nodes as vertices and multi-source constraint factors as edges.

7. The method for precise positioning of a monorail crane in an underground coal mine according to claim 1, characterized in that, Step S5 specifically includes: All pose nodes in the tightly coupled factor graph optimization model are taken as variables to be optimized, and the constraints provided by all SLAM odometry factors, UWB ranging factors and RFID landmark factors are taken as error terms. A nonlinear least squares objective function with pose nodes as optimization variables is constructed to minimize the weighted sum of squares of all error terms. The objective function is solved using the Levenberg-Marquardt iterative optimization algorithm. In each iteration, the Jacobian matrix of each factor error is calculated based on the current pose estimate, the incremental normal equation is constructed and the pose update amount is solved to update the current pose estimate. The iteration is terminated when the number of iterations reaches the preset maximum value or the pose update amount is less than the preset threshold. The optimized three-dimensional position and three-dimensional attitude of the locomotive at each moment are output as the global optimal pose.

8. The method for precise positioning of a monorail crane in an underground coal mine according to claim 1, characterized in that, The first spacing is greater than 200 meters.

9. The method for precise positioning of a monorail crane in a coal mine according to claim 8, characterized in that, Step S1 specifically includes: UWB base stations are deployed at intervals of 200 to 300 meters along straight sections of underground roadways. The three-dimensional coordinates of each UWB base station are pre-calibrated and stored in the system database, forming a sparse UWB reference network covering the entire transport roadway, which is used to provide a global absolute position reference for locomotives. Passive RFID tags are buried at intersections, loading and unloading stations, and the starting point of curves in the roadway, so that each RFID tag has a unique identifier. The precise global coordinates of the tags are associated with the unique identifier and stored in the RFID tag map database to provide instantaneous absolute position anchor points when locomotives pass by.

10. A precise positioning system for a monorail crane in an underground coal mine, characterized in that, include: The sparse UWB base station network consists of multiple UWB base stations deployed in underground roadways at intervals greater than 200 meters. Each UWB base station has pre-calibrated three-dimensional coordinates stored in the system database, which are used to provide a global absolute position reference for the locomotive. The RFID tag network consists of multiple passive RFID tags deployed at road intersections, loading and unloading stations, and the starting points of curves. Each RFID tag has a unique identifier, and its precise global coordinates are associated with the unique identifier and stored in the RFID tag map database to provide instantaneous absolute position anchor points when a locomotive passes by. The vehicle-mounted terminal, installed on a monorail crane, includes a lidar, an inertial measurement unit, UWB tags, an RFID reader, and a processing unit. The output end of the lidar is connected to the first input end of the processing unit, and is used to collect point cloud data of the roadway environment around the locomotive and transmit it to the processing unit. The output of the inertial measurement unit is connected to the second input of the processing unit, and is used to collect the three-axis acceleration and three-axis angular velocity data of the locomotive and transmit them to the processing unit. The output end of the UWB tag is connected to the third input end of the processing unit for communicating with the UWB base station, obtaining the original ranging value between the locomotive and each visible base station and transmitting it to the processing unit. The original ranging value includes the signal-to-noise ratio and multipath index. The output of the RFID reader is connected to the fourth input of the processing unit, and is used to read the unique identifier of the RFID tag and record the trigger time when the locomotive passes by the RFID tag, and transmit the unique identifier and trigger time to the processing unit. The processing unit is used to receive and process data transmitted from the LiDAR, inertial measurement unit, UWB tag, and RFID reader, and execute a fusion positioning algorithm, including: generating SLAM odometry factors based on tightly coupled fusion of LiDAR point cloud data and IMU inertial data; generating UWB ranging factors with dynamic reliability weights based on UWB ranging data; and generating RFID landmark factors based on RFID tag trigger data. The dynamic reliability weights are calculated based on the signal-to-noise ratio, multipath index, geometric accuracy factor, and consistency between the UWB calculated position and the SLAM predicted position of the UWB signal, and their values ​​are between 0 and 1. The RFID landmark factors are absolute position observation factors that connect the locomotive pose node at the trigger time with the precise global coordinates of the tag. A tightly coupled factor graph optimization model is constructed using the SLAM odometry factors, UWB ranging factors, and RFID landmark factors. The tightly coupled factor graph optimization model is solved using nonlinear least squares to output the global optimal pose of the locomotive at each moment.

Images