Coal mine tunnel global consistent positioning method fusing laser inertial navigation and UWB target

Abstract

The invention discloses a coal mine tunnel global consistent positioning method fusing laser inertial navigation and a UWB target, which comprises the following steps: S1, collecting multi-source sensor data, calculating the weight of the sensor data, and generating weighted sensor data; s2, based on weighted sensor data, performing data fusion through an error state filter to generate a pose estimation result; s3, establishing a pose map based on the pose estimation result, adding various constraint factors, executing nonlinear optimization, and generating an optimized pose result and an environment map; and S4, outputting coal mine tunnel positioning and map data based on the optimized pose result and the environment map. And the problem of performance degradation of a single sensor in an underground environment is effectively solved through a dynamic weight distribution mechanism.

Description

Technical Field

[0001] This invention relates to the field of positioning technology, and in particular to a globally consistent positioning method for coal mine roadways that integrates laser inertial navigation and UWB targets. Background Technology

[0002] With the deepening of intelligent coal mine construction, the application of underground automatic driving equipment and unmanned operation systems is becoming increasingly widespread. Precise and reliable positioning and navigation technology has become a key technological foundation for ensuring safe equipment operation and improving operational efficiency. The unique characteristics of underground coal mine roadways, such as enclosed spaces, complex structures, and harsh environments, place extremely high demands on the accuracy, stability, and adaptability of positioning systems. Traditional ground positioning technologies are often difficult to apply directly in the underground environment. Therefore, there is an urgent need to develop high-precision positioning and navigation technologies suitable for the special environment of underground coal mines, providing reliable position benchmarks and navigation support for intelligent mining equipment.

[0003] However, existing underground positioning technologies have significant defects and shortcomings. First, most existing methods employ single-sensor positioning schemes, which are prone to perception degradation and positioning failure when faced with complex environments such as dust interference, insufficient lighting, and limited feature diversity in coal mines. Second, traditional multi-sensor fusion methods lack adaptive weight adjustment mechanisms for different environmental conditions and cannot dynamically optimize the fusion strategy based on sensor data quality, resulting in poor fusion performance. Third, existing positioning methods generally lack global consistency constraints and optimization mechanisms, easily leading to accumulated drift errors and failing to meet the accuracy requirements of long-term, large-scale operations. Finally, existing technologies do not adequately consider the structural symmetry and feature sparsity of underground coal mine roadways, lacking effective environmental adaptability design. These technological limitations severely restrict the reliable application of automated driving and intelligent operation equipment in underground mines.

[0004] Therefore, there is an urgent need for a globally consistent positioning method for coal mine roadways that integrates laser inertial navigation and UWB targets. Summary of the Invention

[0005] This invention provides a globally consistent positioning method for coal mine roadways that integrates laser inertial navigation and UWB targets, in order to solve the aforementioned problems existing in the prior art.

[0006] To achieve the above objectives, the present invention provides the following technical solution: A globally consistent localization method for coal mine roadways integrating laser inertial navigation and UWB targets includes: S1: Collect multi-source sensor data and calculate the sensor data weights to generate weighted sensor data; S2: Based on weighted sensor data, data fusion is performed through an error state filter to generate pose estimation results; S3: Based on the pose estimation results, a pose graph is built, various constraint factors are added, nonlinear optimization is performed, and optimized pose results and environment map are generated. S4: Based on the optimized pose results and environmental map, output coal mine roadway positioning and map data.

[0007] Step S1 includes: S11: Collect lidar point cloud data, filter and downsample the point cloud data, and perform motion distortion compensation on the processed point cloud based on inertial navigation data to generate preprocessed point cloud data. S12: Acquire image data from the vision sensor, enhance the contrast of the image data, extract corner and edge feature points, and generate visual feature data; S13: Collect wheel speed meter pulse signals, calculate the correlation coefficient between the wheel speed change rate and the vehicle acceleration measured by the inertial navigation system, detect wheel slippage state based on the correlation coefficient, and generate wheel speed measurement data with slippage state markers; S14: Collect inertial navigation triaxial acceleration and triaxial angular velocity data, perform zero bias estimation on the data and subtract the zero bias from the original data to generate inertial navigation data after zero bias compensation; S15: Calculate the temporal stability variance and signal-to-noise ratio of each sensor data, determine the weighting coefficient of each sensor data based on the index, and multiply the weighting coefficient with the corresponding sensor data to generate weighted sensor data.

[0008] Step S2 includes: S21: Construct the motion equations for the nominal state vector and error state vector of the system, substitute the compensated inertial navigation data into the motion equations to perform state prediction, and generate state prediction values. S22: Construct the system observation equation, combine the coordinates of the lidar feature points, the coordinates of the visual feature pixels, and the linear velocity calculated by the wheel speed meter to form the observation vector, and calculate the observation prediction value through the observation equation; S23: The error state vector is iteratively calculated using an error state filter to solve for the optimal estimate of the error state. S24: Add the optimal estimate of the error state to the nominal state vector to generate the vehicle's six-degree-of-freedom pose estimation result.

[0009] Step S3 includes: S31: Calculate the displacement and rotation changes between adjacent frame poses. When the displacement or rotation exceeds the dynamic threshold calculated based on feature point density, set the current frame as a key frame node and construct a pose graph using the node and relative pose transformation. S32: Calculate the global coordinates of the tag based on the ranging signals from at least three UWB base stations, calculate the pose of the target relative to the vehicle by using the feature point positions in the target image and the known geometric dimensions of the target, and add the global coordinates of the tag as position constraints to the corresponding keyframe node; S33: The RANSAC method is used to fit the plane equation of the tunnel wall from the point cloud. The principal direction features of the point cloud are extracted by principal component analysis to identify the tunnel boundary. The plane equation parameters and boundary direction vectors are added as structural constraints to the pose graph. S34: Calculate the cosine similarity between the bag-of-words vectors of keyframes, perform point cloud registration on frame pairs with similarity greater than a preset threshold, calculate the consistency between the registration result and the Euclidean distance of UWB coordinates, and add the relative pose transformation between the verified frame pairs as closure constraint edges to the pose graph. S35: Transform the constrained pose graph into a nonlinear least squares problem to solve for the optimal pose of the nodes, and generate a 3D environment map based on the optimized pose fusion point cloud.

[0010] This also includes system status monitoring and multi-vehicle collaborative positioning steps: S5: Calculate the variance of each sensor data within the sliding time window to evaluate its stability, calculate the trace of the pose estimation covariance matrix to evaluate the positioning uncertainty, and reduce the weight coefficient of the corresponding sensor in the fusion when the stability index is lower than the first preset threshold or the uncertainty is higher than the second preset threshold. S6: Establish communication connections between adjacent vehicles, transmit compressed keyframe feature descriptors and pose graph node information, identify common areas by matching node features and perform map stitching.

[0011] The point cloud filtering and downsampling processing in step S11 includes: S111: Calculate the k-nearest neighbor density of each point in the point cloud and filter out points with a density lower than a preset threshold. S112: Divide the point cloud into a preset distance range region based on the distance from the point to the lidar, and apply voxel grids of different sizes to downsample in different regions; S113: Calculate the nearest neighbor distance between the current frame point cloud and the previous frame point cloud in the overlapping area, and filter out points whose distance is greater than a preset threshold. S114: Identify planar point clouds and edge point clouds in point clouds, and retain planar point clouds and edge point clouds as structural feature points.

[0012] Step S23 includes: S231: Substitute the state estimate and error covariance at time t into the discretized state transition equation, and obtain the prior estimate at time t+1 by the fourth-order Runge-Kutta numerical integration method; S232: Calculate the predicted observations based on the prior state estimation and compare them with the actual sensor measurements to obtain the innovation vector; S233: Multiply the sensor weighting coefficients by the corresponding elements of the observation noise covariance matrix to obtain the weighted observation noise matrix; S234: Calculate the Kalman gain based on the Jacobian matrix of the state transition and observation equations, and update the state estimate and error covariance; S235: Terminate the iteration when the Euclidean norm of the new information vector is less than the preset threshold or the number of iterations reaches the preset upper limit.

[0013] Step S31 includes: S311: Calculate the three-dimensional translation distance between adjacent frames. When the distance is greater than the displacement threshold calculated based on the number of feature points in the current frame, mark the current frame as a key frame. S312: Calculate the rotation angle between adjacent frames using quaternions. When the angle is greater than the rotation threshold calculated based on the number of feature points in the current frame, mark the current frame as a keyframe. S313: When the largest eigenvalue of the pose covariance matrix is ​​greater than the preset uncertainty threshold, the current frame is set as a key frame.

[0014] Step S32 includes: S321: Calculate the global coordinates of the tag based on the ranging signals from at least three UWB base stations, and calculate the pose of the target relative to the vehicle by using the feature point positions in the target image and the known geometric dimensions of the target. S322: Calculate the standard deviation of multiple UWB positioning results. When the standard deviation is less than the preset consistency threshold, the positioning result is confirmed to be valid. S323: Establish a kinematic model based on the vehicle wheelbase and maximum steering angle, and filter out UWB positioning data that exceeds the model's prediction range; S324: Calculate a reliability score based on the UWB signal reception strength and historical positioning accuracy, and set the weight of the location constraint according to the score.

[0015] Step S34 includes: S341: Extract ORB feature points from keyframes and construct bag-of-words vectors, then calculate the cosine similarity between the bag-of-words vectors of the current frame and historical frames. S342: Select frame pairs with a similarity greater than a preset similarity threshold, perform point cloud registration, and calculate the point cloud registration residual; S343: Transform the global coordinates of the label calculated by UWB to the lidar coordinate system, and calculate the spatial distance between the coordinates and the point cloud registration result; S344: When the point cloud registration residual is less than the preset residual threshold and the spatial distance is less than the preset distance threshold, add a closure constraint edge based on the relative pose transformation between the two frames.

[0016] Compared with the prior art, the present invention has the following advantages: This invention employs a multi-source sensor data fusion architecture and establishes a sensor data quality assessment system to achieve a dynamic weight allocation mechanism, effectively solving the problems of single sensor performance degradation and data unreliability in underground environments. The system can monitor the data quality of explosion-proof lidar, mining vision sensors, intrinsically safe wheel speedometers, and fiber-optic strapdown inertial navigation systems in real time, adaptively adjusting the weight contribution of each sensor according to environmental conditions to ensure high-quality fused data even under complex conditions such as dust interference, insufficient lighting, and slippery road surfaces. The front-end error state filter adopts an iterative optimization strategy, fully leveraging the complementary advantages and synergistic effects of multiple sensors. Through the geometric constraints of lidar, the texture features of vision sensors, the motion information of wheel speedometers, and the high-frequency dynamic response of the inertial navigation system, robust pose estimation is achieved. This filtering framework effectively handles technical challenges such as temporal asynchrony between sensors, coordinate system deviation, and nonlinear coupling, maintaining stable positioning performance in the unique structural symmetry and characteristic uniformity environment of underground coal mines. The backend pose map optimization module establishes a complete globally consistent optimization framework by constructing a multi-layered constraint network. This framework integrates the global position reference provided by the UWB target, the natural constraints formed by the tunnel structure features, and the topological constraints established by trajectory loop detection. This optimization mechanism effectively eliminates the cumulative drift error generated by the front-end odometer during long-term operation, significantly improving the positioning accuracy and global consistency of the environmental map in a wide range of tunnel environments, thus providing a reliable navigation reference and safety guarantee for underground autonomous driving equipment.

[0017] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention.

[0018] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0019] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart of a global consistent positioning method for coal mine roadways that integrates laser inertial navigation and UWB targets, as described in an embodiment of the present invention. Figure 2 This is a flowchart illustrating the generation of pose estimation results in an embodiment of the present invention. Detailed Implementation

[0020] The preferred embodiments of the present invention will be described below with reference to the accompanying drawings. It should be understood that the preferred embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0021] The embodiments of the present invention provide, as follows Figure 1 As shown, a globally consistent localization method for coal mine roadways that integrates laser inertial navigation and UWB targets includes: S1: Collect multi-source sensor data and calculate the sensor data weights to generate weighted sensor data; S2: Based on weighted sensor data, data fusion is performed through an error state filter to generate pose estimation results; S3: Based on the pose estimation results, a pose graph is built, various constraint factors are added, nonlinear optimization is performed, and optimized pose results and environment map are generated. S4: Based on the optimized pose results and environmental map, output coal mine roadway positioning and map data.

[0022] The working principle and beneficial effects of the above technical solution are as follows: Step S1 achieves dynamic weight allocation of data sources by establishing an adaptive evaluation system for sensor data quality. This step simultaneously connects four types of mine-specific equipment: explosion-proof lidar, mine explosion-proof vision sensors, intrinsically safe wheel speed gauges, and mine fiber optic strapdown inertial navigation systems, to obtain three-dimensional point cloud information of the roadway, low-light image data, wheel speed measurements, and inertial motion parameters. The system establishes a sensor quality evaluation mechanism, comprehensively considering factors such as the signal-to-noise ratio characteristics of the data, information entropy index, prediction residual size, and time series stability, and determines the contribution coefficient of each sensor through normalization processing and weighted algorithms. When environmental conditions change, such as an increase in dust concentration leading to a decrease in laser ranging accuracy, the system can automatically adjust the weight distribution to increase the influence of the vision sensors.

[0023] Step S2 employs an improved error-state Kalman filter framework to handle the heterogeneous sensor data fusion task. This process constructs a complete state description including position, velocity, attitude, and bias terms, avoiding singularity issues in attitude representation by separating the nominal state and the error state. Using acceleration and angular velocity provided by the inertial navigation system as system dynamics inputs, a state prediction model is established; simultaneously, LiDAR geometric features, visual image features, and wheel speed measurements are used as multi-source observation inputs to construct measurement update equations. The filtering process uses an iterative strategy to handle the system's strong nonlinear characteristics, obtaining the optimal state estimate through multiple linearizations and gain calculations, and outputting a six-DOF vehicle localization result including position and attitude.

[0024] Step S3 achieves globally consistent localization optimization by constructing a multi-constraint pose graph network. Key reference frames are selected based on the pose change amplitude, and a graph topology is established to represent the vehicle's trajectory. The system integrates three types of constraint information: global localization constraints are constructed using absolute position benchmarks provided by GNSS, UWB positioning networks, and pre-set targets; local feature constraints are constructed by identifying fixed landmarks such as lane walls and support structures from point clouds and images; and closed-loop constraints are constructed by identifying trajectory loop locations through feature matching and geometric verification. The graph structure and constraint information are transformed into a nonlinear optimization problem, and the Levenberg-Marquardt iterative algorithm is used to solve for the optimal pose estimation of each node, eliminating cumulative localization drift and generating a globally consistent environmental map.

[0025] Step S4 standardizes the output and format conversion of the positioning results. The optimized vehicle pose information is converted into a standard coordinate system representation, and the data is integrated and format unified with the constructed point cloud environment map. The output includes real-time vehicle spatial position and attitude information, as well as environmental map data containing elements such as tunnel geometry and equipment layout, supporting precise navigation, path planning, and safety monitoring applications for underground autonomous vehicles.

[0026] In another embodiment, step S1 includes: S11: Collect lidar point cloud data, filter and downsample the point cloud data, and perform motion distortion compensation on the processed point cloud based on inertial navigation data to generate preprocessed point cloud data. S12: Acquire image data from the vision sensor, enhance the contrast of the image data, extract corner and edge feature points, and generate visual feature data; S13: Collect wheel speed meter pulse signals, calculate the correlation coefficient between the wheel speed change rate and the vehicle acceleration measured by the inertial navigation system, detect wheel slippage state based on the correlation coefficient, and generate wheel speed measurement data with slippage state markers; S14: Collect inertial navigation triaxial acceleration and triaxial angular velocity data, perform zero bias estimation on the data and subtract the zero bias from the original data to generate inertial navigation data after zero bias compensation; S15: Calculate the temporal stability variance and signal-to-noise ratio of each sensor data, determine the weighting coefficient of each sensor data based on the index, and multiply the weighting coefficient with the corresponding sensor data to generate weighted sensor data.

[0027] The working principle and beneficial effects of the above technical solution are as follows: Step S11 realizes the preprocessing and motion distortion correction of the raw lidar data. For harsh environments such as dust and water mist in underground mines, statistical filtering methods are used to analyze the local density distribution of the point cloud, identify and remove abnormal outliers; the space is discretized into fixed-size cubic units using voxel mesh downsampling technology, with the point set within each unit represented by the center point, significantly reducing the data volume while maintaining geometric structural features. Based on the motion state information provided by the inertial navigation system, the pose change during laser scanning is calculated, and motion compensation is performed on the point cloud using a coordinate transformation matrix to eliminate geometric distortion caused by equipment movement, ensuring the geometric consistency of the point cloud data.

[0028] Step S12 focuses on image quality enhancement and feature extraction in low-light environments like mines. After acquiring tunnel images from explosion-proof vision sensors, histogram equalization is used to redistribute pixel grayscale, improving overall image contrast. An adaptive local enhancement algorithm is applied to improve the visibility of details in dark areas. The pre-processed images are then processed by a pre-trained feature detection network to extract geometric features, including corners and edges, as well as semantic features such as support structures and pipeline equipment. When the system detects a decrease in lidar measurement accuracy under high dust conditions, the weight contribution of visual data is automatically increased, serving as an auxiliary observation source for positioning estimation.

[0029] Step S13 establishes a wheel speed sensor data reliability detection mechanism. Pulse signals from the left and right wheel encoders are read, and the wheel rotation angular velocity and corresponding linear traverse speed are calculated. Wheel slippage is detected by analyzing acceleration change characteristics over a short time period. Specifically, the time derivative of the speed change is calculated; when the derivative exceeds a set threshold or the speed difference between the two wheels is too large, a slippage condition is identified. The confidence level of the wheel speed data is quantified based on the slippage detection results. During normal driving, the confidence level is close to full value; during severe slippage, the confidence level is close to zero value. This indicator is used for dynamic weight adjustment in subsequent data fusion.

[0030] Step S14 performs systematic error compensation and preprocessing on the inertial navigation data. Raw measurement data from the three-axis accelerometer and gyroscope are acquired. Considering the effects of Earth's rotation and the magnetic field environment of the mining area, an Earth parameter compensation algorithm is applied for data correction. The compensation calculation incorporates latitude information, magnetic declination data, and Earth rotation rate parameters from the current geographical location. A correction transformation matrix is ​​constructed to process the raw data, removing systematic biases, improving the absolute accuracy of the inertial navigation measurement, and providing high-quality dynamic input for state estimation.

[0031] Step S15 constructs a comprehensive evaluation index system for sensor data quality. Four core indicators are calculated for each sensor's data: signal-to-noise ratio (SNR), reflecting the energy ratio of effective signal to interference noise; information entropy, quantifying the richness of information carried by the data; observation residual, measuring the deviation between measured and theoretically predicted values; and temporal consistency, assessing the continuity of data over time and the stability of its changing patterns. Through standardization, each indicator is mapped to a unified dimension, and a linear combination using preset weights is performed to obtain the comprehensive sensor score. The system adjusts the weight coefficients of each sensor in the data fusion process in real time based on the score results, ensuring that high-quality data receives greater influence and improving the overall performance of the fusion algorithm.

[0032] In another embodiment, such as Figure 2 As shown, step S2 includes: S21: Construct the motion equations for the nominal state vector and error state vector of the system, substitute the compensated inertial navigation data into the motion equations to perform state prediction, and generate state prediction values. S22: Construct the system observation equation, combine the coordinates of the lidar feature points, the coordinates of the visual feature pixels, and the linear velocity calculated by the wheel speed meter to form the observation vector, and calculate the observation prediction value through the observation equation; S23: The error state vector is iteratively calculated using an error state filter to solve for the optimal estimate of the error state. S24: Add the optimal estimate of the error state to the nominal state vector to generate the vehicle's six-degree-of-freedom pose estimation result.

[0033] The working principle and beneficial effects of the above technical solution are as follows: Step S21 establishes a system dynamics model based on error state representation. A complete nominal state vector is defined, containing the vehicle's three-dimensional position, three-dimensional velocity, attitude quaternions, and sensor biases. Simultaneously, an error state vector representing the deviation between the real state and the nominal state is constructed. Using the three-axis acceleration and angular velocity data provided by the inertial navigation system as system input, a state transition equation is established based on the principles of rigid body kinematics: the velocity state is updated by time integration of acceleration, the position state is updated by velocity integration, and the attitude matrix representation is updated using angular velocity information. This modeling method effectively handles the nonlinear characteristics of attitude and avoids the singularity problem of traditional Euler angle representation.

[0034] Step S22 constructs a unified observation model for the multimodal sensors. The geometric feature points identified by the lidar, the image feature points detected by the vision sensor, and the motion velocity calculated by the wheel speedometer are combined to form a multidimensional observation vector. For each type of observation, a corresponding measurement equation is established, creating a mathematical mapping relationship between sensor observations and system state variables. The observation model considers the installation position of each sensor relative to the vehicle body and the coordinate system transformation relationship, ensuring data consistency and alignment across different coordinate systems through a homogeneous transformation matrix. Multiple observation equations form a complete measurement model, providing observation constraint information for the update steps of the filtering algorithm.

[0035] Step S23 employs an iterative error Kalman filter algorithm to handle the nonlinear estimation problem. The prior state estimate and corresponding covariance matrix are calculated during the prediction phase to assess the uncertainty of the state prediction. The relative reliability of the Kalman gain-balanced prediction and observation information is calculated, and the state is corrected and updated in conjunction with the observation residuals. For the strongly nonlinear characteristics of the system, an iterative calculation strategy is adopted: in each iteration, the Jacobian matrix of the observation equation is recalculated, and the linearization parameters are updated until the convergence criterion is met or the maximum number of iterations is reached. This method effectively handles the state estimation problem of complex nonlinear systems and obtains the optimal estimate of the error state.

[0036] Step S24 generates the complete vehicle pose estimation output. The optimal error state obtained through iterative calculation is combined with the nominal state to obtain the final state estimation result of the system. The three-dimensional position coordinates and three-dimensional attitude angles are separated from the complete state vector to constitute the vehicle's six-degree-of-freedom pose description. The pose result adopts the standard representation of a homogeneous transformation matrix, where the rotation submatrix describes the vehicle's attitude and the translation vector describes the vehicle's position. Simultaneously, the uncertainty covariance matrix of the pose estimation is calculated to provide a confidence assessment basis for subsequent optimization processing and system health monitoring.

[0037] In another embodiment, step S3 includes: S31: Calculate the displacement and rotation changes between adjacent frame poses. When the displacement or rotation exceeds the dynamic threshold calculated based on feature point density, set the current frame as a key frame node and construct a pose graph using the node and relative pose transformation. S32: Calculate the global coordinates of the tag based on the ranging signals from at least three UWB base stations, calculate the pose of the target relative to the vehicle by using the feature point positions in the target image and the known geometric dimensions of the target, and add the global coordinates of the tag as position constraints to the corresponding keyframe node; S33: The RANSAC method is used to fit the plane equation of the tunnel wall from the point cloud. The principal direction features of the point cloud are extracted by principal component analysis to identify the tunnel boundary. The plane equation parameters and boundary direction vectors are added as structural constraints to the pose graph. S34: Calculate the cosine similarity between the bag-of-words vectors of keyframes, perform point cloud registration on frame pairs with similarity greater than a preset threshold, calculate the consistency between the registration result and the Euclidean distance of UWB coordinates, and add the relative pose transformation between the verified frame pairs as closure constraint edges to the pose graph. S35: Transform the constrained pose graph into a nonlinear least squares problem to solve for the optimal pose of the nodes, and generate a 3D environment map based on the optimized pose fusion point cloud.

[0038] The working principle and beneficial effects of the above technical solution are as follows: Step S31 designs an adaptive keyframe selection strategy to construct a pose graph topology. By calculating the spatial displacement and angle changes between consecutive pose estimates, keyframe selection is triggered when the change exceeds a dynamically set threshold. The threshold is adaptively adjusted according to the feature point density of the current environment: a larger threshold is set in feature-rich regions to reduce redundant nodes, and a smaller threshold is set in feature-sparse regions to ensure trajectory representation accuracy. Each keyframe node stores the corresponding six-degree-of-freedom pose information and timestamp. Adjacent keyframes are connected by constraint edges to form an incremental graph structure representing the vehicle's motion history, establishing a basic network framework for subsequent global optimization.

[0039] Step S32 establishes a multi-source global positioning constraint mechanism. It integrates multiple absolute positioning information sources, including ground GNSS signals, downhole UWB positioning networks, and pre-set targets, and transforms this global position data to a unified coordinate reference system. The absolute coordinates of the tags are calculated using the UWB trilateration principle. Where W is the tag position, Bi is the coordinate of the i-th base station, and di is the corresponding ranging value. The relative pose relationship is calculated using the known geometric dimensions of the target and the positions of image feature points. The calculated global position information is then linked to the corresponding keyframe nodes to establish constraints. Weight coefficients are assigned to each constraint based on signal quality and historical consistency to ensure that high-quality constraints play a dominant role in the optimization.

[0040] Step S33 extracts tunnel structural features to construct environmental constraints. The RANSAC random sampling consensus algorithm is used to fit the planar structures of tunnel walls, roof, etc., from the point cloud. The optimal planar parameters are iteratively calculated, and the number of interior points is counted to evaluate model quality. Principal component analysis is used to calculate the dominant directional features of the point cloud, identifying the tunnel direction and boundary information. Deep learning methods are used on the image data to identify fixed facilities such as support components and ventilation ducts, establishing feature descriptors for these natural landmarks. The system evaluates the spatiotemporal stability of the extracted features, selecting highly stable features as localization benchmarks, and converting the feature parameters and direction vectors into structural constraint factors for the pose graph.

[0041] Step S34 implements the loop closure detection and consistency verification mechanism. The bag-of-words vector representation of the keyframe feature descriptors is calculated, and potential loop closure candidates are identified using cosine similarity measurement. ,in The vectors are bag-of-words. For frame pairs with similarity exceeding a threshold, precise point cloud registration is performed to calculate the relative transformation relationship, and geometric consistency is verified with the UWB localization results. The spatial deviation between the registration results and UWB coordinates is compared using Euclidean distance to verify the authenticity of loop closure detection and avoid misjudgments due to environmental similarity. Valid loop closure constraints are confirmed to be added to the pose graph, providing loop closure information for global consistency optimization.

[0042] Step S35 executes the graph optimization algorithm to generate globally consistent localization results. The constructed pose graph and its constraint factors are transformed into a nonlinear least squares optimization problem, with the objective function being the weighted sum of squared errors of each constraint term. The Levenberg-Marquardt iterative optimization algorithm is used to solve for the optimal estimate of the pose of each node. This method combines the fast convergence characteristics of the Gauss-Newton method with the global stability of the gradient descent method. The optimization process dynamically adjusts the allocation of computational resources to ensure the real-time performance of the algorithm, and outputs the optimized keyframe pose sequence and the corresponding uncertainty covariance. Based on the optimized pose, the local point clouds of each keyframe are fused to generate a global environment map, and the map quality is ensured through spatial indexing and duplicate point removal.

[0043] In another embodiment, system status monitoring and multi-vehicle cooperative positioning steps are also included: S5: Calculate the variance of each sensor data within the sliding time window to evaluate its stability, calculate the trace of the pose estimation covariance matrix to evaluate the positioning uncertainty, and reduce the weight coefficient of the corresponding sensor in the fusion when the stability index is lower than the first preset threshold or the uncertainty is higher than the second preset threshold. S6: Establish communication connections between adjacent vehicles, transmit compressed keyframe feature descriptors and pose graph node information, identify common areas by matching node features and perform map stitching.

[0044] The working principle and beneficial effects of the above technical solution are as follows: Step S5 establishes a sensor health status monitoring and adaptive weight adjustment mechanism. A sliding time window is used to calculate the statistical variance of each sensor's data to assess the temporal stability of the data; the trace value of the covariance matrix is ​​used to quantify the overall uncertainty level of pose estimation. When the stability index drops to a preset lower limit or the uncertainty exceeds a warning upper limit, the system activates a sensor weight decay strategy, reducing the influence weight of abnormal sensors according to an exponential decay function. This mechanism achieves automatic detection and isolation of sensor faults, maintains the stable operation of the data fusion system through weight redistribution, and ensures that the system still has basic positioning capabilities even when some sensors fail.

[0045] Step S6 implements multi-vehicle cooperative localization and distributed map fusion. Wireless communication links are established between adjacent autonomous vehicles, transmitting compressed keyframe feature descriptors and pose map node information. Overlapping areas between vehicle trajectories are identified using a feature matching algorithm, establishing relative pose constraints between vehicles. The map fusion algorithm employs a confidence-weighted strategy, assigning fusion weights based on the sensor quality and localization uncertainty of each vehicle. This cooperative mechanism expands map coverage, improves localization robustness, and supports coordinated navigation and operation scheduling for large-scale vehicle fleets in mining areas.

[0046] In another embodiment, the point cloud filtering and downsampling processing in step S11 includes: S111: Calculate the k-nearest neighbor density of each point in the point cloud and filter out points with a density lower than a preset threshold. S112: Divide the point cloud into a preset distance range region based on the distance from the point to the lidar, and apply voxel grids of different sizes to downsample in different regions; S113: Calculate the nearest neighbor distance between the current frame point cloud and the previous frame point cloud in the overlapping area, and filter out points whose distance is greater than a preset threshold. S114: Identify planar point clouds and edge point clouds in point clouds, and retain planar point clouds and edge point clouds as structural feature points.

[0047] The working principle and beneficial effects of the above technical solution are as follows: Step S111 uses density analysis to detect and remove outliers. The system performs neighborhood density statistical analysis on the raw point cloud data collected by the lidar, calculating the spatial distribution density of the k nearest neighbors around each sampling point. By constructing a density histogram, the distribution pattern of point clouds in each region is statistically analyzed, identifying outlier points whose density significantly deviates from the normal distribution. The system sets the density filtering range to 0.3 to 3.0 times the statistical mean. Data points exceeding this range are marked as noise points and excluded from subsequent processing, ensuring the spatial continuity and geometric consistency of the point cloud data.

[0048] Step S112 employs a hierarchical voxelization strategy to achieve adaptive downsampling. The scanning space is divided into multiple concentric ring regions based on the radial distance from the point cloud to the LiDAR center. Each region is downsampled using a voxel grid of different resolutions. The near-range region (0-10 meters) uses a 0.05-meter voxel size to preserve detailed features; the mid-range region (10-30 meters) uses a 0.1-meter voxel size to balance accuracy and efficiency; and the far-range region (above 30 meters) uses a 0.2-meter voxel size to reduce data redundancy. This hierarchical processing strategy adjusts the sampling density according to distance, significantly reducing the amount of point cloud data while preserving key structural features.

[0049] Step S113 establishes an inter-frame geometric consistency constraint mechanism for temporal filtering. The system calculates the geometric correspondence between the point clouds of the current frame and the previous frame in the spatially overlapping region, and establishes point pair matching using nearest neighbor search. The spatial deviation of the matched point pairs is evaluated using Euclidean distance metric, and a statistical distribution of the distance deviation is constructed. When the distance deviation between point pairs exceeds twice the point cloud sampling resolution, it is determined to be a temporally inconsistent point, and the system automatically removes such unstable data points to ensure the geometric consistency and temporal continuity of the point clouds between consecutive frames.

[0050] Step S114 uses a geometric model fitting method to extract structural feature points of the tunnel. The system applies a random sample consensus algorithm to perform plane fitting on the preprocessed point cloud, and determines the optimal plane parameters through iterative calculation. The algorithm randomly selects three points to construct candidate planes, counts the number of interior points with a plane distance of less than 0.05 meters, and selects the plane with the most interior points as the geometric representation of the tunnel wall or roof. The system extracts point clouds that meet the plane constraints as structural feature points, and simultaneously identifies edge feature points at the plane boundaries and intersections, providing a stable geometric constraint benchmark for subsequent fusion positioning algorithms.

[0051] In another embodiment, step S23 includes: S231: Substitute the state estimate and error covariance at time t into the discretized state transition equation, and obtain the prior estimate at time t+1 by the fourth-order Runge-Kutta numerical integration method; S232: Calculate the predicted observations based on the prior state estimation and compare them with the actual sensor measurements to obtain the innovation vector; S233: Multiply the sensor weighting coefficients by the corresponding elements of the observation noise covariance matrix to obtain the weighted observation noise matrix; S234: Calculate the Kalman gain based on the Jacobian matrix of the state transition and observation equations, and update the state estimate and error covariance; S235: Terminate the iteration when the Euclidean norm of the new information vector is less than the preset threshold or the number of iterations reaches the preset upper limit.

[0052] The working principle and beneficial effects of the above technical solution are as follows: Step S231 employs a high-order numerical integration algorithm to achieve state prediction propagation. The system reads the current system state estimate and the corresponding uncertainty covariance matrix, and substitutes the triaxial acceleration and angular acceleration measured by the inertial navigation device into the pre-established state dynamics equations. The time evolution of the state variables is calculated using the fourth-order Runge-Kutta numerical integration method, which has higher numerical accuracy and stability compared to Euler integration. The integration process also considers the influence of system process noise, updating the state covariance matrix to characterize the prediction uncertainty, and providing a high-quality prior estimate basis for subsequent observation update steps.

[0053] Step S232 constructs a multi-source observation residual vector for state correction preparation. Based on prior state estimation, the system calculates theoretical measurements using the observation models of each sensor, including the spatial coordinates of the lidar's geometric features in the predicted pose, the image projection coordinates of visual feature points, and the theoretical wheel speed calculated based on the kinematic model. The actual measurement data from each sensor are then compared with their corresponding theoretical values ​​to form a multi-dimensional observation residual vector. This residual vector quantitatively describes the degree of mismatch between the state prediction and the actual observation, and is the core driving information for the filter to perform state correction.

[0054] Step S233 implements dynamic observation noise modeling for sensor weights. The system applies the sensor reliability weight coefficients calculated in step S1 to the modulation process of the observation noise covariance matrix. High-weight sensors correspond to smaller observation noise variances and gain greater influence in state updates; low-weight sensors correspond to larger observation noise variances, and their influence is appropriately suppressed. By constructing a dynamic weighted observation equation through weight modulation, the fusion process can adaptively adjust the contribution of each data source according to environmental conditions and sensor states, improving the robustness and accuracy of the overall estimation.

[0055] Step S234 performs Kalman filter gain calculation and state update operations. The system calculates the optimal Kalman gain based on the prediction covariance matrix and the weighted observation noise matrix. The calculated gain matrix and observation residuals are used to update the error state estimate and covariance matrix, completing one iterative filtering cycle.

[0056] Step S235 establishes a convergence judgment mechanism to control the iteration termination condition. The system calculates the Euclidean norm of the observed residual vector as a convergence index. When the norm value is less than a preset threshold of 0.05 m, the filtering is considered to have reached convergence. Simultaneously, the maximum number of iterations is limited to 8 to prevent infinite loops in ill-conditioned cases. When any termination condition is met, the system stops iterating and outputs the final state estimation result, achieving a balance between convergence and computational efficiency.

[0057] In another embodiment, step S31 includes: S311: Calculate the three-dimensional translation distance between adjacent frames. When the distance is greater than the displacement threshold calculated based on the number of feature points in the current frame, mark the current frame as a key frame. S312: Calculate the rotation angle between adjacent frames using quaternions. When the angle is greater than the rotation threshold calculated based on the number of feature points in the current frame, mark the current frame as a keyframe. S313: When the largest eigenvalue of the pose covariance matrix is ​​greater than the preset uncertainty threshold, the current frame is set as a key frame.

[0058] The working principle and beneficial effects of the above technical solution are as follows: Step S311 establishes an adaptive keyframe triggering mechanism based on translational motion. The system extracts the three-dimensional spatial coordinates of continuous pose estimation and calculates the Euclidean distance magnitude of the inter-frame translational displacement. The displacement threshold is dynamically adjusted according to the feature point extraction density in the current environment: a threshold of 1.5 meters is set when the number of feature points exceeds 500, a threshold of 1.0 meter is set when the number of feature points is between 200 and 500, and a threshold of 0.5 meters is set when the number of feature points is less than 200. This adaptive strategy appropriately sparses the keyframe distribution in feature-rich environments to improve computational efficiency, and densifies the keyframe distribution in feature-scarce environments to maintain positioning accuracy.

[0059] Step S312 uses quaternion angle measurement to determine the keyframes for rotational motion. The system converts the attitude representation of consecutive frames into unit quaternion form and calculates the relative rotation quaternion through quaternion multiplication. The rotation angle is extracted from the scalar part of the quaternion using the inverse cosine function. ,in The real part of the quaternion is defined. Angle thresholds are set based on feature density: 6 degrees for feature-rich regions, 4 degrees for feature-moderate regions, and 3 degrees for feature-sparse regions. Keyframe selection is triggered when the rotation angle exceeds the corresponding threshold to ensure sufficient attitude constraint information during vehicle steering.

[0060] Step S313 implements keyframe selection based on covariance through uncertainty analysis. The system calculates the largest eigenvalue of the pose estimation covariance matrix as a measure of system uncertainty, reflecting the overall confidence level of the current state estimate. When the largest eigenvalue exceeds a preset threshold of 0.1 (unit: m²·radians²), it indicates that system uncertainty is growing rapidly, requiring the addition of keyframes to improve estimation quality. Even if the translation and rotation changes do not meet the standard triggering conditions, the system will still force the selection of the current frame as a keyframe to ensure the stability and reliability of the positioning system.

[0061] In another embodiment, step S32 includes: S321: Calculate the global coordinates of the tag based on the ranging signals from at least three UWB base stations, and calculate the pose of the target relative to the vehicle by using the feature point positions in the target image and the known geometric dimensions of the target. S322: Calculate the standard deviation of multiple UWB positioning results. When the standard deviation is less than the preset consistency threshold, the positioning result is confirmed to be valid. S323: Establish a kinematic model based on the vehicle wheelbase and maximum steering angle, and filter out UWB positioning data that exceeds the model's prediction range; S324: Calculate a reliability score based on the UWB signal reception strength and historical positioning accuracy, and set the weight of the location constraint according to the score.

[0062] The working principle and beneficial effects of the above technical solution are as follows: Step S321 integrates multimodal global positioning information sources to establish an absolute position benchmark. The system simultaneously receives three types of global positioning data: GNSS satellite navigation signals, UWB ultra-wideband positioning network signals, and visual target recognition information. For GNSS signals, the weighted least squares method is used to calculate the three-dimensional coordinates; for UWB signals, the trilateration principle is used to calculate the spatial position; and for visual targets, the perspective geometric model is used to deduce the relative pose relationship. All positioning results are uniformly converted to the mine engineering coordinate system, establishing a multi-source fusion framework for global position information, providing a data foundation for subsequent consistency analysis and constraint construction.

[0063] Step S322 establishes a consistency verification and reliability screening mechanism for global position information. The system calculates the spatial position deviation between different positioning sources and constructs a position difference matrix to evaluate the mutual consistency of each signal source. A consistency judgment threshold of 2.0 meters is set; when the position deviation between any two signal sources is less than this threshold, they are considered to have good consistency. Statistical analysis identifies the signal combination with the best consistency, and abnormal signals with excessive deviations are removed from the global constraint construction. This screening mechanism effectively avoids the negative impact of erroneous global information on pose graph optimization.

[0064] Step S323 constructs a vehicle kinematic constraint model to verify the physical rationality of the global positioning. A kinematic boundary model is established based on physical parameters such as vehicle wheelbase and maximum steering angle, setting a reasonable speed range of 0-15 m / s and an acceleration range of -5 to +3 m / s². Motion state parameters are calculated using temporally adjacent global position information. When the speed or acceleration exceeds the physical constraint range, it is identified as abnormal data and removed. This verification process ensures that the global constraints conform to the vehicle's dynamic characteristics and avoids unreasonable constraint conditions caused by signal anomalies.

[0065] Step S324 establishes a method for constructing weighted global constraint factors based on signal quality. The system calculates a reliability score based on the historical accuracy statistics of each global positioning source and the current signal strength, with the score ranging from 0 to 1. Signal sources with high scores are given greater weight in pose graph optimization, while the constraint strength of signal sources with low scores is correspondingly weakened. Adaptive global constraint fusion is achieved through dynamic weight allocation, allowing high-quality global information to play a dominant role, while low-quality information provides auxiliary support, thereby improving the accuracy and robustness of the overall positioning system.

[0066] In another embodiment, step S34 includes: S341: Extract ORB feature points from keyframes and construct bag-of-words vectors, then calculate the cosine similarity between the bag-of-words vectors of the current frame and historical frames. S342: Select frame pairs with a similarity greater than a preset similarity threshold, perform point cloud registration, and calculate the point cloud registration residual; S343: Transform the global coordinates of the label calculated by UWB to the lidar coordinate system, and calculate the spatial distance between the coordinates and the point cloud registration result; S344: When the point cloud registration residual is less than the preset residual threshold and the spatial distance is less than the preset distance threshold, add a closure constraint edge based on the relative pose transformation between the two frames.

[0067] The working principle and beneficial effects of the above technical solution are as follows: Step S341 uses a multi-dimensional feature descriptor to construct a loop closure candidate detection mechanism. The system extracts the ORB visual features, LiDAR geometric features, and environmental semantic features of keyframes to construct a high-dimensional feature vector representation. The feature vector is converted into a sparse representation using a bag-of-words model, and the cosine similarity between the bag-of-words vectors of the current frame and historical keyframes is calculated. , where a and b are the bag-of-words vectors. A similarity value close to 1 indicates a high degree of feature matching, while a value close to 0 indicates a large feature difference. A similarity threshold of 0.7 is set to filter loop closure candidate frame pairs, providing a candidate set for subsequent accurate verification.

[0068] Step S342 performs precise geometric registration to verify the authenticity of the loop closure relationship. For candidate frame pairs that pass feature screening, the system extracts the corresponding point cloud data and executes an iterative nearest-point registration algorithm. The registration process optimizes the spatial transformation parameters by minimizing the sum of squared distances between corresponding point pairs, setting a convergence threshold of 0.03 meters and a maximum number of iterations of 100. The criteria for successful registration are a final residual of less than 0.1 meters and an effective matching point pair ratio exceeding 75%. Frame pairs that meet the conditions are confirmed as true loop closures, and the registration transformation matrix serves as the geometric basis for the loop closure constraint.

[0069] Step S343 establishes a geometric consistency verification mechanism for the UWB target position. The system acquires the absolute position of the UWB target in the mine's global coordinate system and transforms it to the lidar coordinate system for comparative analysis. The transformation process considers the vehicle's current pose and sensor installation offset to calculate the target's theoretical spatial position. Simultaneously, the geometric position information of the target is extracted from the point cloud registration results, and the spatial distance deviation between the two position estimates is calculated. When the deviation is less than 0.5 meters, the UWB information is confirmed to be consistent with the geometric registration results, further verifying the reliability of loop closure detection.

[0070] Step S344 constructs robust loop closure constraint factors and integrates them into the pose graph optimization framework. Based on the validated loop closure relationships, the system calculates the relative pose transformation between loop closure frame pairs, including rotation matrices and translation vectors. An error function model is established to describe the constraint relationships, and an information matrix is ​​set to reflect the confidence level of the constraints.

[0071] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from the spirit and scope of this invention.

Claims

1. A coal mine tunnel global consistent positioning method fusing laser inertial navigation and UWB target, characterized in that, The method comprises the following steps: S1: Collecting multi-source sensor data and calculating sensor data weight to generate weighted sensor data; S2: Based on the weighted sensor data, data fusion is carried out through an error state filter to generate a pose estimation result; S3: Based on the pose estimation result, a pose graph is established, a plurality of constraint factors are added, nonlinear optimization is performed, and an optimized pose result and an environment map are generated; S4: Based on the optimized pose result and the environment map, coal mine tunnel positioning and map data are output.

2. The method according to claim 1, wherein, The step S1 comprises: S11: Collecting laser radar point cloud data, filtering and downsampling the point cloud data, and compensating the motion distortion of the processed point cloud based on inertial navigation data to generate preprocessed point cloud data; S12: Collecting visual sensor image data, enhancing the contrast of the image data, and extracting corner and edge feature points to generate visual feature data; S13: Collecting wheel speed pulse signals, calculating the correlation coefficient between the wheel speed change rate and the vehicle acceleration measured by the inertial navigation, detecting the wheel slip state based on the correlation coefficient, and generating wheel speed measurement data with a slip state marker; S14: Collecting inertial navigation three-axis acceleration and three-axis angular velocity data, estimating the zero offset of the data and subtracting the zero offset from the original data to generate zero offset compensated inertial navigation data; S15: Calculate the time sequence stability variance and signal-to-noise ratio index of each sensor data, determine the weight coefficient of each sensor data based on the index, multiply the weight coefficient with the corresponding sensor data, and generate weighted sensor data.

3. The method according to claim 1, wherein, The step S2 comprises: S21: Constructing a system nominal state vector and an error state vector motion equation, substituting the compensated inertial navigation data into the motion equation to perform state prediction, and generating state prediction values; S22: Constructing a system observation equation, forming an observation vector with laser radar feature point coordinates, visual feature pixel coordinates and wheel speed calculated by a wheel speed meter, and calculating observation prediction values through the observation equation; S23: Iteratively calculating the error state vector using an error state filter to solve the optimal estimation value of the error state; S24: Adding the optimal estimation value of the error state to the nominal state vector to generate a six-degree-of-freedom vehicle pose estimation result.

4. The method according to claim 1, wherein, The step S3 comprises: S31: Calculate the displacement and rotation change between adjacent frames of pose, when the displacement or rotation exceeds the dynamic threshold calculated based on the feature point density, set the current frame as a key frame node, and construct a pose graph with nodes and relative pose transformation; S32: Solving the global coordinates of the tag based on the ranging signals of at least three UWB base stations, solving the pose of the target relative to the vehicle through the feature point position in the target image and the known geometric size of the target, and adding the global coordinates of the tag as a position constraint to the corresponding key frame node; S33: Fitting the roadway wall plane equation from the point cloud using the RANSAC method, identifying the roadway boundary by extracting the main direction feature of the point cloud through principal component analysis, and adding the plane equation parameters and boundary direction vector as a structure constraint to the pose graph; S34: Calculate the cosine similarity between the key frame bag-of-words vectors, perform point cloud registration on the frame pairs with similarity greater than a preset threshold, and calculate the Euclidean distance of the registration result and the UWB coordinates to verify the consistency, and add the relative pose transformation between the frame pairs that pass the verification to the pose graph as loop constraint edges; S35: Convert the pose graph with constraints into a nonlinear least squares problem to solve the optimal node pose, and generate a three-dimensional environment map based on the optimized pose fusion point cloud.

5. The method according to claim 1, wherein, It also includes system state monitoring and multi-vehicle cooperative positioning steps: S5: Calculate the variance of each sensor data in the sliding time window to evaluate its stability, calculate the trace of the pose estimation covariance matrix to evaluate the positioning uncertainty, and reduce the weight coefficient of the corresponding sensor in the fusion when the stability index is lower than the first preset threshold or the uncertainty is higher than the second preset threshold; S6: Establish a communication connection between adjacent vehicles, transmit compressed key frame feature descriptors and pose graph node information, identify common areas by matching node features, and perform map stitching.

6. The method according to claim 2, wherein, The point cloud filtering and downsampling processing of S11 step includes: S111: Calculate the k-nearest neighbor density of each point in the point cloud, and filter out points with a density lower than a preset threshold; S112: Divide the point cloud into preset distance range areas based on the distance from the point to the lidar, and apply different size voxel grids for downsampling in different areas; S113: Calculate the nearest neighbor distance of the current frame point cloud and the previous frame point cloud in the overlapping area, and filter out points with a distance greater than a preset threshold; S114: Identify the planar point cloud and edge point cloud in the point cloud, and retain the planar point cloud and edge point cloud as structural feature points.

7. The method according to claim 3, wherein, S23 step includes: S231: Substitute the state estimation and error covariance at time t into the discretized state transition equation, and obtain the prior estimation at time t+1 by the fourth-order Runge-Kutta numerical integration method; S232: Calculate the predicted observation value based on the prior state estimation, and compare it with the actual sensor measurement value to obtain the innovation vector; S233: Multiply the sensor weight coefficient by the corresponding elements of the observation noise covariance matrix to obtain the weighted observation noise matrix; S234: Calculate the Kalman gain based on the Jacobian matrix of the state transition and observation equations, and update the state estimation and error covariance; S235: Terminate the iteration when the Euclidean norm of the innovation vector is less than a preset threshold or the iteration count reaches a preset upper limit.

8. The method according to claim 4, wherein, S31 step includes: S311: Calculate the three-dimensional translation distance of adjacent frames, and mark the current frame as a key frame when the distance is greater than the displacement threshold calculated based on the number of feature points in the current frame; S312: Calculate the rotation angle between adjacent frames by quaternion, and mark the current frame as a key frame when the angle is greater than the rotation threshold calculated based on the number of feature points in the current frame; S313: Set the current frame as a key frame when the maximum eigenvalue of the pose covariance matrix is greater than a preset uncertainty threshold.

9. The method according to claim 4, wherein, S32 step includes: S321: Solve the global coordinates of the tag based on the ranging signals of at least three UWB base stations, and solve the pose of the target relative to the vehicle based on the feature point positions in the target image and the known geometric dimensions of the target; S322: Calculate the standard deviation of multiple UWB positioning results, and confirm the positioning result valid when the standard deviation is less than a preset consistency threshold; S323: Establish a kinematic model based on the wheelbase and maximum steering angle of the vehicle, and filter out UWB positioning data that exceeds the model prediction range; S324: Calculate the reliability score based on the UWB signal reception strength and historical positioning accuracy, and set the weight of the position constraint according to the score.

10. The method according to claim 4, wherein, S34 steps include: S341: Extract the ORB feature points of the key frame and construct the bag-of-words vector, calculate the cosine similarity between the current frame and the historical frame bag-of-words vector; S342: Select frame pairs with similarity greater than a preset similarity threshold, perform point cloud registration and calculate point cloud registration residual; S343: Convert the UWB-solved label global coordinates to the laser radar coordinate system, and calculate the spatial distance between the coordinates and the point cloud registration result; S344: When the point cloud registration residual is less than a preset residual threshold and the spatial distance is less than a preset distance threshold, add a loop constraint edge based on the relative pose transformation between the two frames.

Images