A precise inspection method and inspection robot for underground caverns

Abstract

The present invention relates to the field of robot navigation technology, and in particular to a precise inspection method and inspection robot for underground caverns. The method comprises the following steps: acquiring spatial point cloud data, robot posture data, and scene magnetic field data, and constructing a magnetic field model based on the scene magnetic field data to obtain a scene magnetic field model; obtaining interference feature data based on the scene magnetic field model and the spatial point cloud data; obtaining magnetic disturbance weight data based on the interference feature data and the robot posture data; performing fusion filtering on the spatial point cloud data and the robot posture data based on the magnetic disturbance weight data to obtain navigation optimization data; performing residual analysis on the spatial point cloud data and the robot posture data based on the navigation optimization data to obtain residual trend data; and performing dynamic compensation based on the residual trend data to obtain spatial optimized point cloud data and robot optimized posture data. The present invention improves the anti-interference capability and environmental adaptability of the inspection robot.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a precise inspection method and an inspection robot for underground caverns. Background Art

[0002] With the development of intelligent robotics technology, the demand for automated inspections in complex environments such as underground caverns, mines, tunnels, and underground pipeline corridors continues to grow. Existing underground inspection robot systems mostly rely on laser radar (LiDAR) and inertial navigation units (IMU) for positioning and navigation, completing environmental mapping and path planning by constructing three-dimensional point cloud maps. However, in practical applications, strong magnetic interference sources (such as metal components, power distribution equipment, and ventilation systems) are prevalent in underground spaces. This causes inertial navigation data to drift and navigation errors to accumulate rapidly, thereby affecting the accuracy of point cloud mapping and the anti-interference ability of inspection paths. To improve the robustness of navigation systems in interference environments, some studies have attempted to introduce magnetic field sensors to correct IMU outputs. However, these methods often use static or mean filtering, which is difficult to effectively address problems such as complex spatial magnetic disturbance distribution and significant nonlinear distortion. Summary of the Invention

[0003] In order to solve the above technical problems, the present invention proposes a precise inspection method and an inspection robot for underground caverns to solve at least one of the above technical problems.

[0004] This application provides a precise inspection method for underground caverns, the method comprising:

[0005] S1. Acquire spatial point cloud data, robot posture data, and scene magnetic field data, and construct a magnetic field model based on the scene magnetic field data to obtain a scene magnetic field model;

[0006] S2. Extract interference features based on the scene magnetic field model and spatial point cloud data to obtain interference feature data;

[0007] S3. Calculate magnetic disturbance weights based on the interference feature data and the robot posture data to obtain magnetic disturbance weight data; and fuse and filter the spatial point cloud data and the robot posture data based on the magnetic disturbance weight data to obtain navigation optimization data.

[0008] S4. Perform residual analysis on the spatial point cloud data and the robot posture data based on the navigation optimization data to obtain residual trend data; perform dynamic compensation based on the residual trend data to obtain spatial optimized point cloud data and robot optimized posture data.

[0009] The present invention achieves accurate identification and quantitative modeling of navigation system errors in magnetic field disturbance environments by introducing multi-source perception fusion of spatial point cloud data, robot posture data, and scene magnetic field data, combined with the construction of a magnetic field model and the extraction of interference features. The use of magnetic disturbance weight-driven fusion filtering significantly improves the collaborative robustness of the lidar and inertial navigation systems in complex magnetic field environments, effectively avoiding the impact of inertial navigation drift on positioning accuracy. Through residual trend analysis and dynamic compensation mechanisms, adaptive optimization and closed-loop correction of navigation paths and point cloud reconstruction results are achieved, improving the path reproduction accuracy and spatial modeling accuracy in inspection tasks.

[0010] Optionally, S1 includes:

[0011] Acquire spatial point cloud data, robot posture data, and scene magnetic field data;

[0012] Perform spatial mapping of magnetic field sampling points based on scene magnetic field data to obtain magnetic induction sampling spatial map data;

[0013] Performing magnetic potential function interpolation on the magnetic induction sampling space map data to obtain magnetic potential space data;

[0014] The magnetic potential space data is distorted and modeled to obtain the scene magnetic field model.

[0015] In the present invention, by introducing the spatial mapping of magnetic field sampling points and the interpolation calculation of magnetic potential functions, a high-fidelity conversion channel from raw magnetic field data to continuous magnetic potential distribution is constructed, which significantly improves the analytical accuracy of magnetic field environment information. By generating magnetic induction sampling spatial map data and adopting the magnetic potential function interpolation method based on it, continuous reconstruction can be achieved in areas where there is magnetic field shielding or noise disturbance, avoiding the problems of sparse point data and local abnormal amplification in traditional magnetic field modeling. The distortion modeling process based on magnetic potential spatial data can effectively characterize the nonlinear magnetic field distortion characteristics caused by metal structures and complex spatial layouts in underground caverns, providing accurate physical model support for magnetic disturbance compensation and fusion positioning.

[0016] Optionally, the distortion modeling includes:

[0017] Calculating the magnetic potential gradient on the magnetic potential space data to obtain magnetic potential gradient data;

[0018] Performing high-order perturbation metric calculation on magnetic potential gradient data to obtain high-order perturbation metric data;

[0019] Perform local detection on high-order disturbance measurement data to obtain local abnormal data;

[0020] Perform distortion cluster analysis based on local abnormal data to obtain distorted fast body data;

[0021] The magnetic field model is encapsulated according to the distorted block data to obtain the scene magnetic field model.

[0022] The distortion modeling step described in the present invention constructs a multi-scale detection mechanism from continuous magnetic potential field to local magnetic disturbance feature identification by performing gradient analysis and high-order disturbance measurement on magnetic potential space data. Through the calculation of magnetic potential gradient, the changing trend of the magnetic field in space can be accurately revealed, providing a basis for judging the intensity and direction of disturbance; the introduction of high-order disturbance measurement methods can identify magnetic disturbance structures such as nonlinear changes and local vortices, which is significantly better than the adaptability of the traditional first-order gradient method in high-interference areas. Combining local detection with distortion agglomeration analysis, discrete magnetic disturbance anomaly points can be aggregated into distorted blocks with spatial continuity and structural boundaries, realizing the expression upgrade from point anomaly to regional anomaly. By encapsulating and modeling the distorted blocks, a high-resolution, structured magnetic field environment model is formed, which provides interference shielding boundaries and dynamic correction basis for path planning and navigation filtering.

[0023] Optionally, S2 includes:

[0024] Extract candidate magnetic disturbance areas based on the scene magnetic field model and spatial point cloud data to obtain candidate magnetic disturbance area data;

[0025] Performing point cloud local structure analysis on candidate magnetic disturbance area data to obtain point cloud local structure data;

[0026] Perform interference layer division on the local structure data of the point cloud to obtain interference layer data;

[0027] An interference feature tensor is generated according to the interference layer data to obtain interference feature data.

[0028] In the present invention, by fusing the scene magnetic field model and spatial point cloud data, a hierarchical identification and feature extraction mechanism for magnetic disturbance structures is constructed, which significantly improves the accuracy of magnetic disturbance source positioning and impact range assessment in underground environments. Through the spatial extraction of candidate areas for magnetic disturbance zones, the analysis range can be accurately limited to areas where magnetic field anomalies are concentrated, reducing the burden of redundant calculations; then combined with the local structure analysis of the point cloud, not only spatial geometric features can be identified, but also structural features such as complex components or structural gaps that may cause magnetic disturbance aggregation can be mined. Through the hierarchical division of interference, a multi-level interference mapping relationship can be constructed based on factors such as point cloud density changes and edge gradients, effectively separating surface interference and deep interference effects. The generation of interference feature tensors can provide a tensorized input data structure for subsequent weight modeling and filter design while retaining the spatial distribution, intensity level and disturbance morphology.

[0029] Optionally, the extracting of magnetic disturbance area candidate regions includes:

[0030] Interference confidence voxel screening is performed based on the scene magnetic field model and spatial point cloud data to obtain interference confidence voxel data;

[0031] Clustering candidate regions are generated on the interference confidence voxel data to obtain magnetic disturbance candidate region data;

[0032] Morphological analysis is performed on the magnetic disturbance candidate area data to obtain the magnetic disturbance candidate area data.

[0033] The magnetic disturbance candidate region extraction described in the present invention introduces a screening mechanism for interference confidence voxels, constructing a three-dimensional magnetic disturbance detection path based on the collaborative identification of magnetic field models and spatial point clouds. By using a joint analysis of magnetic field models and point cloud data to perform interference confidence voxel screening, regions with high magnetic field disturbance intensity and abnormal spatial structural characteristics can be accurately identified at the voxel level, significantly improving the positioning accuracy of candidate regions. Cluster analysis is performed based on voxel data to generate magnetic disturbance candidate regions with strong aggregation and continuous boundaries, which not only preserves the geometric shape of the disturbance source but also enhances the distinguishability and spatial closure of the region. Combined with morphological analysis, the clustering results can be subjected to scale filtering, morphological fitting, and boundary clipping to eliminate error interference areas and extract magnetic disturbance candidate regions that meet the actual impact range of navigation interference.

[0034] Optionally, the interference hierarchical division includes:

[0035] Calculate the local point density change rate and coordinate disturbance rate of the point cloud local structure data to obtain the local point density change rate data and coordinate disturbance rate data;

[0036] Clustering is performed based on the local point density change rate data and the coordinate disturbance rate data to obtain the point cloud local clustering data;

[0037] Performing interference local perception processing on the point cloud local structure data according to the point cloud local clustering data to obtain interference local perception data;

[0038] Calculate the interference degree according to the local interference perception data to obtain interference degree data;

[0039] According to the interference degree data, the local structure data of the point cloud is divided into interference intensity levels to obtain interference layered data.

[0040] The calculation of the local point density change rate in the present invention can reveal the trend of changes such as tightening and loosening of spatial structures under the influence of magnetic disturbances, and the coordinate disturbance rate is used to quantify the degree of spatial drift of point cloud nodes under continuous frames or multi-source observations, thereby capturing the geometric distortion characteristics caused by magnetic disturbances. Clustering processing based on the two not only enhances the perception of abnormal structural patterns, but also improves the stability of noise filtering. The introduction of local interference perception processing realizes the reconstruction of the local response area affected by magnetic disturbances at the structural semantic level, providing accurate data support for calculations. Through the calculation and hierarchical division of the degree of interference, interference layered data with hierarchical expression capabilities are formed, providing a quantifiable basis for magnetic disturbance compensation and adaptive filter design. Compared with the coarse-grained evaluation method that only relies on point cloud intensity or mean displacement in traditional methods, this method significantly improves the expressiveness and engineering applicability of magnetic disturbance spatial response modeling by combining local dynamic behavior with hierarchical representation.

[0041] Optionally, S3 includes:

[0042] The disturbance space confidence map is constructed based on the interference feature data and the robot posture data to obtain the magnetic disturbance weight data;

[0043] Perform posture disturbance matching on the spatial point cloud data and the robot posture data according to the magnetic disturbance weight data to obtain posture disturbance matching data;

[0044] The pose disturbance matching data is subjected to high and low disturbance fusion filtering to obtain navigation optimization data.

[0045] The present invention constructs a disturbance space confidence map through joint analysis of interference feature data and robot posture data, which can achieve spatial encoding of magnetic disturbance intensity and navigation credibility in different regions, thereby giving each posture point a differentiated magnetic disturbance weight expression, significantly improving the local adaptability of interference modeling and the ability to control navigation weights. Through posture disturbance matching processing guided by magnetic disturbance weights, the geometric calibration of point clouds and posture sequences can be enhanced in high-interference areas, and the stability of the navigation system can be maintained in low-interference areas, forming a differentiated matching scheme for dynamic response to interference. The fusion matching results are subjected to high and low disturbance filtering, combined with extended Kalman filtering or unscented Kalman filtering strategies to achieve refined denoising and error compensation of the navigation trajectory.

[0046] Optionally, the high-interference and low-interference fusion filtering includes:

[0047] Perform perturbation layered matching annotation on the pose perturbation matching data to obtain pose perturbation layered data;

[0048] Perform filter response configuration mapping according to the pose disturbance layered data to obtain filter response configuration data;

[0049] Generate kernel weight control factors according to filter response configuration data to obtain perturbation filter kernel data;

[0050] The perturbation filter kernel data is used to perform fusion filtering on the pose perturbation matching data to obtain navigation optimization data.

[0051] The present invention performs disturbance hierarchical matching and annotation based on the posture disturbance matching data, which can divide the key frames in the navigation path into levels according to the strength of the disturbance, and accurately characterize the response state of the navigation node under different interference levels. Through the configuration mapping between the disturbance level and the filtering strategy, the response-differentiated filtering control parameters are generated to ensure that the system enhances the suppression response in the high-disturbance area and maintains the trajectory smoothness in the low-disturbance area. The kernel weight control factor is generated through the mapping result, and the disturbance filter kernel is constructed to realize the dynamic adjustment of the filter window shape, weight distribution and kernel function type. During the filtering execution process, the disturbance filter kernel is introduced to perform fusion filtering on the posture disturbance matching data, which not only ensures the rapid response of the filter to high-frequency disturbances, but also avoids excessive smoothing in the low-interference area, and retains the fine structure of the navigation path to the greatest extent.

[0052] Optionally, S4 includes:

[0053] Based on the navigation optimization data, the spatial point cloud data and the robot posture data are processed with the residual field across time and space scales to obtain the residual trend data;

[0054] Perform residual classification on the residual trend data to obtain residual classification data;

[0055] Generating residual control parameters according to the residual classification data and a preset compensation parameter library to obtain residual control parameter data;

[0056] The spatial point cloud data and robot posture data are optimized according to the residual control parameter data to obtain the spatial optimized point cloud data and the robot optimized posture data.

[0057] The cross-spatiotemporal scale residual field processing performed based on navigation optimization data in the present invention can not only identify short-term drift errors, but also capture long-term cumulative error trends, forming a complete error evolution trajectory, laying the foundation for refined compensation. By performing residual classification on residual trend data, systematic errors (such as magnetic disturbance drift) and sporadic errors (such as environmental occlusion) can be effectively distinguished, and a structured expression of error types can be achieved. Combined with a preset compensation parameter library, the system automatically calls the most matching control strategy according to different residual types, generates residual control parameters, and thus realizes diversified compensation mapping. Driven by this control parameter, the spatial point cloud data and posture data are optimized and adjusted, which not only improves the coupling accuracy between the point cloud and the navigation trajectory, but also significantly enhances the path coherence and environmental model fitting capabilities.

[0058] Optionally, the present application further provides a precision inspection robot for an underground cavern, for performing the precision inspection method for an underground cavern as described above, wherein the precision inspection robot for an underground cavern comprises:

[0059] The multi-source sensing acquisition and magnetic field modeling module is used to obtain spatial point cloud data, robot posture data, and scene magnetic field data, and construct a magnetic field model based on the scene magnetic field data to obtain a scene magnetic field model;

[0060] The magnetic disturbance feature extraction module is used to extract disturbance features based on the scene magnetic field model and spatial point cloud data to obtain disturbance feature data;

[0061] The magnetic disturbance weight driven fusion filter module is used to calculate the magnetic disturbance weight based on the interference feature data and the robot posture data to obtain the magnetic disturbance weight data; the spatial point cloud data and the robot posture data are fused and filtered based on the magnetic disturbance weight data to obtain the navigation optimization data;

[0062] The residual trend analysis and compensation module is used to perform residual analysis on the spatial point cloud data and the robot posture data according to the navigation optimization data to obtain the residual trend data; and perform dynamic compensation according to the residual trend data to obtain the spatial optimized point cloud data and the robot optimized posture data.

[0063] The purpose of the present invention is to construct a three-dimensional magnetic field model based on magnetic potential gradient and distorted blocks, which can effectively characterize the local nonlinear magnetic disturbance structure in the underground space and provide a physical basis for subsequent interference identification; the interference feature extraction and hierarchical processing mechanism realizes the structured expression of local disturbance behavior and enhances the system's ability to identify multi-scale interference. The magnetic disturbance weight control factor is introduced in the fusion filtering process to enable the filtering strategy to have the ability to respond to regional differences and effectively balance the robustness of high-interference areas and the fidelity of low-interference areas. The residual trend analysis is combined with the control parameter library to form a closed-loop dynamic compensation mechanism to achieve continuous correction of navigation and mapping errors across the time domain. On the whole, the present invention breaks through the technical bottleneck of unstable positioning and distorted mapping of traditional inspection methods in electromagnetic interference environments, and has the advantages of high robustness, high precision and adaptability. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Other features, objects and advantages of the present application will become more apparent upon reading the detailed description of non-limiting embodiments made with reference to the following drawings:

[0065] Figure 1 A flowchart showing the steps of a precise inspection method for an underground cavern according to an embodiment is shown;

[0066] Figure 2 A flowchart showing the steps of a multi-source sensing acquisition and magnetic field modeling method according to an embodiment is shown;

[0067] Figure 3 A flowchart showing the steps of a method for extracting magnetic disturbance features according to an embodiment is shown;

[0068] Figure 4 A flowchart of a magnetic disturbance weight driven fusion filtering method according to an embodiment is shown;

[0069] Figure 5 A flowchart showing the steps of a residual trend analysis and compensation method according to an embodiment is shown;

[0070] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0071] The following is a clear and complete description of the technical method of the present invention in conjunction with the accompanying drawings. It is obvious that the embodiments described are part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making any creative efforts are within the scope of protection of the present invention.

[0072] Furthermore, the accompanying drawings are merely schematic illustrations of the present invention and are not necessarily drawn to scale. Functional entities may be implemented in software, in one or more hardware modules or integrated circuits, or in different network and / or processor and / or microcontroller approaches.

[0073] It should be understood that although the terms "first," "second," and the like may be used herein to describe various elements, these elements should not be limited by these terms. These terms are used solely to distinguish one element from another. For example, a first element may be referred to as a second element, and similarly, a second element may be referred to as a first element, without departing from the scope of the exemplary embodiments. The term "and / or" as used herein includes any and all combinations of one or more of the listed associated items.

[0074] See also Figures 1 to 5 , the present application provides a precise inspection method for underground caverns, the method comprising:

[0075] S1. Acquire spatial point cloud data, robot posture data, and scene magnetic field data, and construct a magnetic field model based on the scene magnetic field data to obtain a scene magnetic field model;

[0076] In one embodiment, the system acquires spatial point cloud data, robot posture data, and scene magnetic field data. Specifically, the spatial point cloud data is collected by a 360-degree rotating laser radar deployed on the inspection robot with a sampling frequency of 5Hz, which can generate a three-dimensional point cloud frame sequence containing spatial structure information; the robot posture data is output by a fiber optic inertial navigation unit (FOG), providing inertial navigation parameters including three-axis attitude angle and acceleration; the scene magnetic field data is measured by a three-axis fluxgate magnetometer in nanotesla (nT) with a sampling frequency of 10Hz. The system uses a point cloud coordinate system to perform three-dimensional space registration on each magnetic field sampling point, that is, to pair and bind the magnetic field measurement value with its position in the point cloud space. Divide the entire space into a three-dimensional voxel grid with equal spacing (each voxel size is 0.5 meters) 0.5 meters 0.5 meters), and weighted averaging of magnetic field measurements within each voxel generates magnetic induction sampling spatial map data. Based on this data, the system further constructs a continuous magnetic potential field model. Specifically, a radial basis function (RBF) interpolation model is constructed using a spherical Gaussian kernel function to interpolate and reconstruct discrete magnetic flux values. The interpolation function takes spatial points as its independent variable, and the interpolated value is the magnetic potential function value. Its form is: the magnetic potential value is equal to the weighted sum of several weighted kernel functions. Each kernel function's center corresponds to a magnetic field sampling point, and the weight is related to the magnetic induction intensity. The kernel function width is controlled by the interpolation scale parameter, which adjusts the spatial smoothness and local responsiveness of the interpolation. The system performs distortion modeling on the magnetic potential spatial data. The first-order gradient of the magnetic potential field in each direction is calculated to characterize the direction and rate of magnetic field change. A perturbation tensor related to the third-order derivative is calculated to measure the nonlinearity or spatial complexity of the local magnetic field change. If the perturbation measure value in a region exceeds a set threshold (for example, the rate of change of the magnetic potential exceeds 10%), the region is determined to have abnormal magnetic field distortion. The system spatially clusters these anomalous voxel regions, aggregating adjacent high-disturbance areas into several distorted blocks. These blocks, along with the continuous magnetic potential field, form a complete scene magnetic field model.

[0077] S2. Extract interference features based on the scene magnetic field model and spatial point cloud data to obtain interference feature data;

[0078] In one embodiment, the system targets voxels in the magnetic field model that overlap with the point cloud spatial region and calculates the magnetic disturbance intensity index within each voxel. If the magnetic disturbance intensity of a voxel exceeds a set threshold (e.g., 50 nanotesla), the voxel is identified as a candidate region with interference confidence and denoted as the interference confidence voxel. A density-based clustering algorithm (such as DBSCAN) is used to spatially cluster the interference confidence voxels, with a cluster radius of 1.0 meter and a minimum cluster size of 10 points. The clustering results form multiple preliminary magnetic disturbance candidate regions. For each candidate region, the system uses principal component analysis to extract its geometric principal axis direction and, based on this, calculates morphological metrics such as length, width, and flatness. If the candidate region exhibits blocky structural features—i.e., the principal axis length does not exceed a certain threshold and the structural compactness meets a set range—then the region is retained as a magnetic disturbance candidate. Within the candidate region, the point cloud local structure analysis is performed. The system extracts a point cloud subset from each candidate region and calculates the local point density for each point. The density value is defined as the number of points within a fixed neighborhood radius (such as 0.2 meters) divided by the neighborhood volume. ,point The local density It can be expressed as the number of neighboring points contained in the specified space sphere neighborhood of the point The volume of the sphere The system calculates the spatial disturbance rate of the point at consecutive moments, that is, the difference in spatial coordinates of the same point in the two frames before and after divided by the time interval between frames. After obtaining the local density and coordinate disturbance rate, the system uses an unsupervised clustering method (such as K-means) to jointly cluster the two. The number of cluster categories is set to 3, corresponding to the three levels of "strong disturbance", "medium disturbance" and "weak disturbance". After the clustering is completed, the corresponding interference feature tensor is generated for each point. The tensor includes the density value, disturbance rate value and clustering result label of the point. The tensor is in the form of a triple, which represents the point structure density, the degree of dynamic disturbance and the type of disturbance layer to which it belongs, forming an interference feature data set.

[0079] S3. Calculate magnetic disturbance weights based on the interference feature data and the robot posture data to obtain magnetic disturbance weight data; and fuse and filter the spatial point cloud data and the robot posture data based on the magnetic disturbance weight data to obtain navigation optimization data.

[0080] In one embodiment, the system constructs a disturbance space confidence map based on interference signature data, using each navigation keyframe or robot pose node as a graph node and calculating its corresponding disturbance confidence value. For each pose frame, the disturbance rate of all points within its neighborhood is counted and averaged, recorded as the average disturbance rate for that frame. The system uses a sigmoid function to nonlinearly map the average disturbance rate, generating a disturbance weight between 0 and 1 that reflects the uncertainty of the current pose frame in the global magnetic disturbance space. The scaling factor of the sigmoid function can be set based on empirical experience (e.g., the α value is used to adjust the disturbance sensitivity) to achieve a gradient response to varying degrees of interference. Pose perturbation matching is then performed. The system obtains the pose estimate output by the lidar SLAM module and the inertial navigation pose provided by the fiber-optic inertial navigation (FOG) module, respectively, and calculates the Euclidean distance residual between the two as a measure of pose inconsistency for the current frame. If this residual exceeds the product of the perturbation weight for that frame and a set error threshold (i.e., the residual exceeds the upper limit after confidence threshold modulation), the system determines that the frame has a potential alignment error. At this point, the registration realignment operation is triggered, using an iterative closest point algorithm to fine-tune the alignment of the current frame's point cloud with the map, thereby updating the pose estimate for the current frame. After the alignment correction is complete, the system assigns perturbation labels to each frame based on perturbation weights, classifying all pose frames into three levels: high, medium, and low. For each level, the system uses a table lookup to configure differentiated filtering strategies: For high-perturbation areas, an unscented Kalman filter (UKF) is used to cope with strong interference environments under nonlinear state models; for medium or low-perturbation areas, an extended Kalman filter (EKF) is used for state updates, balancing computational efficiency and stability. The system configures an adaptive filter kernel for each perturbation level. The kernel takes the form of a Gaussian function, and a perturbation control factor is introduced as a dynamic adjustment parameter for the kernel width. This ensures faster kernel response and more robust weight changes in high-perturbation environments, while maintaining smoothness and stability in low-perturbation areas. Based on the constructed disturbance level mapping relationship and control kernel, the system performs time-series weighted fusion filtering on the entire pose sequence to generate navigation optimization data with accuracy, robustness and trajectory continuity.

[0081] S4. Perform residual analysis on the spatial point cloud data and the robot posture data based on the navigation optimization data to obtain residual trend data; perform dynamic compensation based on the residual trend data to obtain spatial optimized point cloud data and robot optimized posture data.

[0082] In one embodiment, the system constructs a residual field model across time scales. For each moment and spatial position, the navigation residual is defined as the spatial deviation between the current navigation estimate and a reference true point. This reference point can be derived from a high-precision trajectory estimate from a visual SLAM system or a quasi-reference value provided by manual annotation. The system performs sliding statistical analysis on the navigation residuals within a three-dimensional voxel space based on a fixed time window, calculating the residual mean, residual growth rate, and degree of fluctuation for each voxel region over consecutive moments. Residual classification is then performed. The system aggregates all residual points into a residual point set and clusters them in three-dimensional space using a density clustering algorithm (such as DBSCAN). For each cluster, the system calculates the average residual amplitude and classifies it into three categories based on preset rules: If the average residual is greater than 20 cm, it is classified as a systematic error, caused by magnetic disturbances or sensor offset; if the residual is between 5 and 20 cm, it is classified as noise, caused by environmental occlusion, short-term drift, or sensor ambiguity; and if the residual is less than 5 cm, it is classified as a normal error. Each category is assigned a label for compensation strategy selection. The system generates corresponding control parameters based on the residual classification label. A pre-established control parameter library is used to map different error categories to different parameter configurations. For example, system errors utilize the Kalman filter's gain adjustment strategy to improve update response speed, while noise errors utilize the filter weight compression strategy to reduce outlier interference on state estimation. Each residual region is mapped to a set of control parameter vectors, including filter gain factors, residual suppression weight factors, and state correction step size factors. During the compensation execution phase, the system adjusts the corresponding point cloud segments and pose frames based on these parameter vectors. Point cloud compensation achieves structural alignment through coordinate transformation, while pose correction achieves state translation and orientation fine-tuning through filter update formulas. The compensation process takes into account residual trend direction, spatial aggregation structure, and pose continuity. It outputs spatially optimized point cloud data and optimized robot pose data, achieving closed-loop error control and enhancing stability for the navigation chain.

[0083] Optionally, S1 includes:

[0084] S11, obtaining spatial point cloud data, robot posture data and scene magnetic field data;

[0085] In one embodiment, the system uses a 360-degree omnidirectional mechanical rotating lidar (such as the Velodyne HDL-32E) to scan the target space, acquiring 3D point cloud data at a frequency of 5 Hz. Each scan generates a 3D point cloud frame. Each frame is timestamped and contains a number of 3D spatial coordinates in the (x, y, z) format, describing the positional distribution of each measurement point in the target space. The system uses a fiber-optic inertial navigation unit (FOG IMU) combined with wheel encoders to acquire the robot's six-degree-of-freedom (6DOF) pose information during operation. These six degrees of freedom include 3D position coordinates (x, y, z) and 3D pose information (which can be represented using Euler angles, including roll angle φ, pitch angle θ, and yaw angle ψ, or using quaternions to represent rotational pose). The pose data is sampled at a frequency of no less than 100 Hz and aligned with the point cloud data using timestamps. The system uses a three-axis fluxgate magnetometer to sense the magnetic field strength of the robot's environment. The magnetometer has a measurement range of ±1000 microtesla (μT) and collects data on the robot's current ambient magnetic field strength at a frequency of 10 Hz. The magnetic field strength recorded at each moment is represented as a vector, consisting of three components: the magnetic induction along the X, Y, and Z axes (i.e., Bx, By, and Bz). This magnetic field data is synchronized with the robot's timestamp and position information to form a magnetic field strength data sequence with spatial reference information.

[0086] S12, performing spatial mapping of magnetic field sampling points according to the scene magnetic field data to obtain magnetic induction sampling spatial map data;

[0087] In one embodiment, the system performs coordinate transformation on the magnetic induction intensity vector of each magnetic field sampling point, mapping it from the sensor local coordinate system to the global space coordinate system. Specifically, at each moment when the robot performs a sampling task, the system obtains its corresponding rotation posture information and calculates the rotation matrix at that moment. The magnetic induction intensity vector is multiplied by the rotation matrix to complete the transformation from the sensor coordinate system to the world coordinate system, and the magnetic field intensity vector data under a unified spatial reference is obtained. After completing the spatial coordinate transformation, the system divides the entire target inspection area into three dimensions and constructs an equally spaced spatial voxel grid structure. Each voxel unit is a cubic unit, and the size is set to, for example, 0.5 meters. 0.5 meters 0.5 meters. The system assigns each magnetic field sampling point to the corresponding voxel unit according to its position in the global coordinate system, thereby realizing the spatial classification of magnetic induction data. For multiple magnetic field sampling points belonging to the same voxel, the system summarizes their magnetic induction intensities. Specifically, all magnetic induction intensity vectors falling into the same voxel are vector-summed and divided by the total number of sampling points in the voxel to obtain the average magnetic field intensity value of the voxel unit. This value can be used as the representative magnetic induction vector of the voxel in the spatial magnetic induction map.

[0088] S13, performing magnetic potential function interpolation on the magnetic induction sampling space map data to obtain magnetic potential space data;

[0089] In one embodiment, to achieve continuous modeling of magnetic field distribution, the system introduces the mathematical definition of the magnetic potential scalar field. According to physical laws, the magnetic field intensity vector can be given by the gradient of the magnetic potential function. That is, the direction and magnitude of the magnetic field at any point in space are determined by the negative gradient of the magnetic potential field at that point. Mathematically speaking, the magnetic field intensity vector is equal to the negative gradient of the magnetic potential function with respect to the spatial location. The system uses multi-kernel radial basis functions as an interpolation method. Each sampling point is considered the center of a radial basis function. The system performs a weighted superposition operation on the coordinates of each voxel center to estimate its corresponding magnetic potential value. The general form of the interpolation function is as follows: the magnetic potential value at each spatial location is obtained by superimposing weighted Gaussian functions of all sampling points. Each Gaussian function is centered at the sampling point, and its amplitude is determined by the magnetic induction intensity at that sampling point. The specific weighting parameters are determined through fitting. The scale parameter (kernel width) of the Gaussian function is set to 0.75 meters to control the spatial smoothness of the interpolation function. During the modeling process, the system uses the center of each voxel as the interpolation target point and calculates the magnetic potential function value for each voxel. By sequentially inputting all voxel grid points into the interpolation model, the system generates continuous magnetic potential scalar field data in three-dimensional space. This magnetic potential field data uses voxels as the basic unit and has complete spatial distribution characteristics.

[0090] S14. Perform distortion modeling on the magnetic potential space data to obtain a scene magnetic field model.

[0091] In one embodiment, the system first performs a numerical gradient calculation on the magnetic potential scalar field data generated in the previous stage, thereby obtaining the gradient vector field of the magnetic potential field in three-dimensional space. The calculation adopts the three-dimensional central difference method, that is, at each voxel center point, the difference quotient of the magnetic potential values ​​of its adjacent points is calculated along the X-axis, Y-axis and Z-axis directions respectively, and the gradient value is approximately estimated. The resulting gradient vector represents the direction and rate of change of the magnetic potential field at that point. The system derives a high-order perturbation index based on the gradient field data. It mainly includes two contents: one is to calculate the second-order gradient of the magnetic potential field (that is, the Laplacian approximation); the other is to count the variance changes of the magnetic induction intensity at each spatial position. Combining the above two indicators, the system defines the interference energy function, which is expressed as the perturbation energy level intensity at a certain spatial point, that is , is the interference energy value, is the second-order gradient of the magnetic potential, is the magnetic induction variance weighting factor, is the variance of magnetic induction intensity, is the magnetic induction intensity vector. The energy function is a weighted combination of the squared modulus of the second-order derivative of the magnetic potential field and the variance of the magnetic field intensity, where the weighting factor can be set as a hyperparameter. The system sets a predefined threshold (for example, the corresponding value of the 95% quantile) based on the defined interference energy index to divide the abnormal high disturbance area. All spatial points exceeding the threshold are judged as magnetic field distortion areas. The system uses a spatial clustering method (such as the density-based DBSCAN algorithm) to aggregate spatially adjacent or structurally connected high disturbance points into multiple closed three-dimensional block structures. Each block represents an independent magnetic distortion area, and the boundary can be encapsulated and managed using an octree structure or a three-dimensional bounding box structure. The system integrates the above processing results to construct a complete scene magnetic field model. The model contains the following three types of core data: (1) magnetic potential continuous function; (2) magnetic potential gradient vector field; (3) multiple extracted magnetic field distortion block data structures.

[0092] Optionally, the distortion modeling includes:

[0093] Calculating the magnetic potential gradient on the magnetic potential space data to obtain magnetic potential gradient data;

[0094] In one embodiment, the magnetic potential spatial data Perform a 3D central difference method to calculate the gradient vector of the magnetic potential at the center of each voxel , which is defined as ,in is the magnetic potential scalar field, For space Axis position, For space Axis position, For space Axis position, is the forward magnetic potential value in the x direction, is the backward magnetic potential value in the x direction, The voxel spacing / coordinate difference is stored as the magnetic potential gradient tensor field at each voxel. .

[0095] Performing high-order perturbation metric calculation on magnetic potential gradient data to obtain high-order perturbation metric data;

[0096] In one embodiment, The gradient rate of change is further calculated to obtain the gradient change intensity at each point, that is, the second-order derivative or a Laplace-like quantity: , combined with the local magnetic induction fluctuation variance , and obtain the high-order disturbance metric: ,in is the experience weight (e.g. ), forming a disturbed energy field .

[0097] Perform local detection on high-order disturbance measurement data to obtain local abnormal data;

[0098] In one embodiment, Set the local abnormality judgment threshold , which can be determined adaptively through empirical settings or statistical distribution (such as 95% quantile); all the values ​​that meet the following conditions will be set: , the voxel points are marked as abnormal disturbance points; thus the output is the abnormal point set , as the basic data for distortion agglomeration analysis.

[0099] Perform distortion cluster analysis based on local abnormal data to obtain distorted fast body data;

[0100] In one embodiment, based on the obtained local abnormal point set, in order to identify the structural distribution characteristics of the magnetic field abnormal area, the system uses a spatial clustering algorithm to perform cluster analysis on the abnormal point set. Preferably, a density-based spatial clustering method is used. The parameters of the clustering process include the spatial distance threshold. is 1.0 meter, and the minimum number of cluster points minPts is 20 points, that is, if there are no less than 20 neighboring abnormal points within 1 meter around any point, it can constitute an initial cluster core point. After the cluster analysis is completed, each cluster represents a potential distortion area. For each cluster, the system performs a morphological description analysis, which specifically includes the following contents: calculating the minimum three-dimensional bounding box of the abnormal points contained in the cluster to describe its spatial boundary range; calculating the total volume of the voxel points it contains; extracting the coordinates of the spatial geometric center point of the area. The system sets the block judgment rule: when the number of voxel points in the cluster exceeds 50, and the calculated volume exceeds 0.25 cubic meters, it is determined to have significant spatial block characteristics. For each cluster that meets the above conditions, the system marks it as a distorted block unit, denoted as Block_i, where i represents the i-th valid distortion area. All distorted block units that meet the conditions together constitute a distorted block data set, denoted as , represented as an ordered structure set consisting of Block_1, Block_2 to Block_n.

[0101] The magnetic field model is encapsulated according to the distorted block data to obtain the scene magnetic field model.

[0102] In one embodiment, the structured magnetic field model is denoted as , contains the following core components: continuous magnetic potential scalar field, with spatial position coordinates As input, the discrete magnetic potential data φ is processed continuously through radial basis function interpolation to generate a full-space continuous magnetic potential scalar field The first-order magnetic potential gradient tensor field, based on the continuous magnetic potential field , the gradient distribution in space is calculated by the three-dimensional central difference method to form the first-order magnetic potential gradient tensor field The high-order perturbation measurement field combines the second-order derivative information of the magnetic potential with the variance of the local magnetic induction fluctuation to construct a high-order perturbation measurement index. , which constitutes the disturbance energy tensor field The distorted block list is used to organize the distorted area data obtained by clustering to form a distorted block set. The collection contains all the distorted regions that meet the spatial distribution and volume characteristics and can be stored using a spatial index structure (such as an octree or voxel grid). Magnetic field model object The above four types of data are organized into a unified structure as a multi-level expression of the scene magnetic field.

[0103] Optionally, S2 includes:

[0104] S21, extracting candidate magnetic disturbance areas according to the scene magnetic field model and the spatial point cloud data to obtain candidate magnetic disturbance area data;

[0105] In one embodiment, before extracting the candidate area, the system voxelizes the entire three-dimensional space. For each voxel grid point, the system obtains the disturbance energy value of the corresponding position from the magnetic field model. If the disturbance energy intensity at the position is higher than the set confidence threshold (for example, the 95th percentile of the high-order disturbance energy distribution is taken as the threshold), the voxel is marked as an interference confidence voxel. The system performs spatial clustering based on the voxel adjacency relationship, and merges the interconnected interference confidence voxel sets into several preliminary disturbance candidate areas. The clustering method adopts a three-dimensional 8-neighborhood connectivity strategy, that is, any two adjacent voxels that share at least one face, edge or vertex are considered to be connected. For each candidate area, the system further extracts its morphological features, including but not limited to the total number of voxels, the size range of the minimum circumscribed cube, the ratio of the length to flatness in the main axis direction, and the compactness of the overall spatial distribution. The system sets the following filtering conditions: the candidate region must contain at least 30 voxels, the ratio of its major axis length to its minor axis width must not exceed 3 times, and the candidate region's minimum bounding diameter in space must be at least 1.5 meters. Only regions that meet these constraints are retained as magnetic interference zone candidates. The output magnetic interference zone candidate regions are a set of spatial bounding volumes, each of which represents a spatial region that may interfere with navigation, recorded as a candidate region set consisting of several regions.

[0106] S22, performing point cloud local structure analysis on the candidate magnetic disturbance area data to obtain point cloud local structure data;

[0107] In one embodiment, for each candidate magnetic disturbance zone, the system extracts point cloud segments within its corresponding range based on the boundaries of its spatial bounding volume, forming a local point cloud subset for that candidate zone. Each subset represents a sample of spatial structure within the significant magnetic disturbance area and serves as the basis for subsequent feature calculations. The system extracts structural features from the local point cloud. For each point, a local density index is calculated. Specifically, the system constructs a spherical neighborhood with a radius of 0.2 meters centered on the point and counts the number of points within this neighborhood. The number of points in the neighborhood is divided by the volume of the sphere to obtain the local point density value for that point. The system further extracts a normal vector volatility index to reflect the perturbation behavior of the local surface structure across consecutive frames. Specifically, the system fits the principal normal vector of the local neighborhood to each point and records the principal normal vector direction in the current and next frames. The system then calculates the cosine of the angle between the normal vectors in the two frames and takes the difference in their absolute values ​​as the normal vector volatility for that point. A value closer to 1 indicates a stable structure, while a value closer to 0 indicates a dramatic change in direction, typically caused by magnetic disturbance or motion error. The system combines the local density value of each point with the normal vector volatility value to form a structural feature vector, which is used to characterize the response characteristics of the point in the magnetic disturbance area.

[0108] S23, performing interference layer division on the point cloud local structure data to obtain interference layer data;

[0109] In one embodiment, the system performs a joint clustering analysis based on the local structural feature vectors corresponding to each point, namely, the local point density and normal vector volatility values. The clustering algorithm can employ a K-means method or a Gaussian mixture model (GMM) to cluster the feature vectors of all points in an unsupervised manner. The number of cluster categories is set to three, corresponding to high, medium, and low interference levels. The system also sets rules for determining disturbance levels. For example, the following threshold conditions can be set to assist in label interpretation: if a point's local point density is greater than 50 and its normal vector volatility is greater than 0.3, the point is labeled "high disturbance"; if the density is less than 20 and the volatility is less than 0.1, the point is labeled "low disturbance"; all other areas are classified as "medium disturbance." After clustering, the system assigns a disturbance level label to each point, encoding the level as an integer. High, medium, and low disturbance correspond to numerical labels of 3, 2, and 1, respectively. The system pairs the spatial coordinates of all points with their corresponding disturbance level labels to form a point-level disturbance stratified dataset.

[0110] S24. Generate an interference feature tensor according to the interference layer data to obtain interference feature data.

[0111] In one embodiment, for each point that has completed the disturbance level classification, that is, each spatial point in the interference layered data, the system constructs a corresponding disturbance feature vector. This feature vector is composed of the following five elements: the local point density value, which indicates the degree of point cloud clustering at the point in space; the normal vector perturbation rate, which indicates the amplitude of the change in the normal direction of the point in consecutive frames; the disturbance level label, which takes a value of 1 (low disturbance), 2 (medium disturbance), or 3 (high disturbance), determined by the hierarchical clustering results of the previous stage; the disturbance energy value, extracted from the disturbance intensity map in the scene magnetic field model, which is used to reflect the absolute energy of the local magnetic field disturbance; and the magnetic potential gradient modulus, which indicates the magnitude of the magnetic potential gradient at the location of the point. The system combines these five feature fields into a local tensor unit, each of which is recorded using the corresponding point as the index.

[0112] Optionally, the extracting of magnetic disturbance area candidate regions includes:

[0113] Interference confidence voxel screening is performed based on the scene magnetic field model and spatial point cloud data to obtain interference confidence voxel data;

[0114] In one embodiment, the input magnetic potential field disturbance energy tensor (from S1 magnetic potential modeling output); spatial point cloud data , which has been voxelized into a voxel grid For each voxel , calculate the corresponding magnetic potential disturbance energy value ; Set the disturbance confidence threshold ,use ,in and is the mean and standard deviation of the global magnetic potential perturbation; select voxels that meet the following conditions: , the number of point clouds in the voxel ≥ , for example, set it to 10 points. The filtered voxel set is marked as the interference confidence voxel set.

[0115] Clustering candidate regions are generated on the interference confidence voxel data to obtain magnetic disturbance candidate region data;

[0116] In one embodiment, Use spatial clustering method to generate connected areas, use DBSCAN clustering, set parameters including distance threshold ; The minimum number of voxels minPts = 15. The center coordinate of each interference confidence voxel is used as the clustering input point; for each cluster in the clustering result , generating the set of voxels it contains Clusters must meet the following geometric constraints to be retained, such as the number of voxels ≥ 30; the angle between the main axis and the spatial XYZ axis is less than 60° (to avoid slender noise clusters); the overall bounding box volume of the voxel ≥ The qualified cluster set is recorded as the magnetic disturbance candidate area dataset: .

[0117] Morphological analysis is performed on the magnetic disturbance candidate area data to obtain the magnetic disturbance candidate area data.

[0118] In one embodiment, for each candidate region , calculate its three-dimensional minimum external bounding box and extract the following morphological features, such as length : The longest side of the bounding box; width : Second longest side; thickness : shortest side; calculate compactness index ; Discreteness rate ,in is the number of voxel points, is the voxel point index, For the The three-dimensional coordinate vector of the individual pixel point, is the geometric center coordinate vector (center of gravity). The candidate regions that meet the following conditions are retained: ; ; The candidate areas with qualified morphology are marked as magnetic disturbance area candidate area data: .

[0119] Optionally, the interference hierarchical division includes:

[0120] Calculate the local point density change rate and coordinate disturbance rate of the point cloud local structure data to obtain the local point density change rate data and coordinate disturbance rate data;

[0121] In one embodiment, for each point cloud frame and its subsequent frames , select a fixed spatial region to be analyzed in space (such as cube); calculate the rate of change of the number of points in the same spatial area : ,in Indicates time The point cloud density of the area at the moment, is the time interval (inter-frame difference), To prevent the small constant (such as 0.001) from dividing by 0. Coordinate perturbation rate calculation, for the same point Position in two consecutive frames and , define the perturbation rate as ,in for point At the moment The coordinate position of for point At the moment The coordinate position of is the time interval (time difference between frames), and two types of local change data are obtained: local point density change rate data , coordinate perturbation rate data .

[0122] Clustering is performed based on the local point density change rate data and the coordinate disturbance rate data to obtain the point cloud local clustering data;

[0123] In one embodiment, the system provides a Construct a two-dimensional feature vector , including two local dynamic indicators: one is the local point density change rate , which represents the point density increment of the spatial region where the point is located in the time series; the second is the coordinate disturbance rate of the point , which represents the rate of change of the position of the point in consecutive frames. Therefore, the feature vector of each point can be expressed as In the clustering stage, the system uses the K-means algorithm or Gaussian mixture model as the clustering method for unsupervised classification. The number of clusters K is set to 3, which means that all points are divided into three types of disturbance groups, as follows: Cluster 0: low disturbance group, including points with small local density changes and coordinate disturbances; Cluster 1: medium disturbance group, representing medium-level disturbance behavior; Cluster 2: high disturbance group, mainly including points with significant density or position changes. After clustering is completed, the system will calculate the spatial position coordinates of each point. , the corresponding perturbation eigenvector and its cluster labels Output together to form a point cloud local clustering dataset. The dataset is organized in the form of triples, that is, each element contains a spatial point , its dynamic characteristics and cluster labels .

[0124] Performing interference local perception processing on the point cloud local structure data according to the point cloud local clustering data to obtain interference local perception data;

[0125] In one embodiment, each point p i As the center, a three-dimensional convolution window is constructed in its space, and the window size is set to 3×3×3 voxel units (i.e., cubic neighborhood). The frequency of occurrence of various cluster labels in the neighborhood is counted, and the current point p is judged. i The label of the current point is consistent with the labels of the majority of points in the neighborhood. If the current label is inconsistent with the majority label, it is replaced with the main label value in the neighborhood. The following perceptual features are extracted for each point, such as cluster label, recording the spatial disturbance category of the current point (such as low disturbance, medium disturbance, high disturbance); cluster centroid distance, calculate the current point The Euclidean distance to the center of the cluster to which it belongs is expressed as ; Cluster compactness, statistics of the standard deviation of the distance from all points to the centroid in the current cluster, recorded as , indicating the spatial distribution discreteness of the cluster. The system organizes the above three perception attributes into the interference perception feature vector of each point in the form of , and the perception vectors of all points are uniformly summarized to form the interference local perception data set, recorded as .

[0126] Calculate the interference degree according to the local interference perception data to obtain interference degree data;

[0127] In one embodiment, a linear weighted model is used to calculate the interference level value. , is the local disturbance intensity weight factor, , is the local disturbance intensity, is the disturbance change weight factor, , is the spatial disturbance variation, is the intra-cluster volatility reverse factor, , is the interference variance of the cluster to which it belongs. The system calculates the spatial position of each point The corresponding interference level value It is organized into a binary form, and the output is a set of interference degree data, recorded as .

[0128] According to the interference degree data, the local structure data of the point cloud is divided into interference intensity levels to obtain interference layered data.

[0129] In one embodiment, according to each point Corresponding interference level value , the system uses a fixed threshold method to divide it into three disturbance levels. The specific division rules are as follows:

[0130] Table 1: Level classification rules

[0131]

[0132] For each point , the system outputs its spatial coordinates together with the corresponding disturbance level code to form a set of interference layered data. This set is recorded as , the structure is defined as a set of two-tuples, each element contains a point and its disturbance level ,Right now .in, It is an integer identifier, corresponding to the above level code 1, 2 or 3.

[0133] Optionally, S3 includes:

[0134] S31, constructing a disturbance space confidence map based on the interference feature data and the robot posture data to obtain magnetic disturbance weight data;

[0135] In one embodiment, for each robot pose frame, the system extracts all valid points in the current interference feature tensor set within a spherical neighborhood with a radius of 1.0 meter, centered at the robot's spatial location, to construct a set of locally relevant feature points. This set of points represents the interference situation near the robot's current position. Based on this local feature point set, the system calculates the average disturbance energy within the neighborhood and, based on the variance of this mean and the structural discreteness, generates a disturbance confidence weight corresponding to the pose point. This weight calculation utilizes a normalized Sigmoid mapping function, which takes the linear combination of the disturbance mean and the discreteness variance as inputs to a Sigmoid function, outputting a continuous weight value between 0 and 1. In this function, the adjustment coefficient can be set as a hyperparameter, such as a disturbance intensity coefficient of 5.0 and a structural discreteness adjustment coefficient of 2.0. The system encapsulates all pose frames and their corresponding disturbance weight values ​​into a disturbance space confidence map structure. This structure uses pose frames as nodes, with disturbance confidence weights as node attributes, forming a set of mapping data in the form of (pose point, disturbance weight value).

[0136] S32, performing posture disturbance matching on the spatial point cloud data and the robot posture data according to the magnetic disturbance weight data to obtain posture disturbance matching data;

[0137] In one embodiment, for each frame, the system calculates the Euclidean displacement error between the laser pose and the IMU pose. This error value represents the spatial difference between the two pose estimates. The error value is calculated using the Euclidean distance between two points in three-dimensional space. The system introduces a dynamic matching threshold related to the perturbation weight. Specifically, a unique threshold is calculated for each frame. This threshold is calculated by multiplying the maximum allowable matching error by the perturbation weight factor. This tightens the tolerance range in high-confidence interference areas and relaxes the limit in low-confidence interference areas. If the pose error of the current frame exceeds this dynamic threshold, the system determines that there is potential matching distortion in that frame and requires pose correction. The correction strategy uses the iterative closest point (ICP) algorithm to align the IMU pose with the current frame's point cloud data, thereby obtaining a corrected fused pose estimate. The system encapsulates the fused matching data for all time intervals into a matching correction set. Each data item contains a triple structure, including the fused corrected pose, the original error value, and the corresponding perturbation weight value.

[0138] S33. Perform high- and low-disturbance fusion filtering on the pose disturbance matching data to obtain navigation optimization data.

[0139] In one embodiment, for each frame of pose matching results, the system classifies the disturbance level based on its disturbance weight value. The specific rules are as follows: if the weight value is greater than or equal to 0.7, the frame is determined to be in the high-disturbance region; if the weight value is between 0.3 and 0.7 (inclusive of the lower limit but excluding the upper limit), it is determined to be in the medium-disturbance region; if the weight value is less than 0.3, it is determined to be in the low-disturbance region. The system is configured with three types of filtering algorithms for different disturbance levels: in the high-disturbance region, the unscented Kalman filter is used; in the medium-disturbance region, the extended Kalman filter is selected; in the low-disturbance region, the weighted moving average filter method is used. The system sets different filter kernel weight coefficients for each disturbance level to control the influence strength of each frame within the filter window: the filter kernel weight coefficient for the high-disturbance region is set to 1.5; the weight coefficient for the medium-disturbance region is set to 1.0; and the weight coefficient for the low-disturbance region is set to 0.5. When performing filtering calculations, the system selects the corresponding filter and kernel function based on the disturbance level of each frame and performs a time-weighted update on that frame and several frames before and after it. The system integrates all pose frames after fusion filtering into a continuous navigation trajectory, which is recorded as the optimized navigation trajectory dataset.

[0140] Optionally, the high-interference and low-interference fusion filtering includes:

[0141] Perform perturbation layered matching annotation on the pose perturbation matching data to obtain pose perturbation layered data;

[0142] In one embodiment, the input data is a pose perturbation matching dataset containing information from multiple navigation keyframes. Each frame of data consists of three components: a matched and corrected pose, representing the pose estimation result obtained by fusing the lidar point cloud with inertial navigation data; a matching residual between the inertial navigation estimate and the lidar estimate for that frame, which measures the magnitude of the pose correction; and a perturbation weight, which quantifies the intensity of the magnetic perturbation in the current frame's environment. This weight is a continuous value between 0 and 1, with higher values ​​indicating stronger perturbations. The system classifies each frame into a perturbation level based on the perturbation weight. The specific classification rules are as follows: when the perturbation weight is greater than or equal to 0.7, the frame is labeled "high perturbation level"; when the perturbation weight is between 0.3 and 0.7, it is labeled "medium perturbation level"; and when the perturbation weight is less than 0.3, it is labeled "low perturbation level." After stratification, the system associates each frame's matched pose with its corresponding perturbation level label to form structured output data. The resulting annotation is a set of disturbance level identification data in navigation frames. Each element includes a corrected pose record and its corresponding disturbance level label. The disturbance level can be "high disturbance", "medium disturbance" or "low disturbance".

[0143] Perform filter response configuration mapping according to the pose disturbance layered data to obtain filter response configuration data;

[0144] In one embodiment, the system first establishes a mapping strategy table between disturbance levels and filter parameters. This strategy table maps the three levels of "high disturbance," "medium disturbance," and "low disturbance" to different filtering configurations. For navigation frames with a high disturbance level, the system uses an unscented Kalman filter (UKF) with a filter sampling window of 5 frames. For frames with a medium disturbance level, the system uses an extended Kalman filter (EKF) with a sampling window of 3 frames. For frames with a low disturbance level, the system uses a sliding average filter (SMA), also with a sampling window of 3 frames. During actual execution, the system traverses the pose disturbance layer data one by one. For each data item containing a matching pose and disturbance level label, the system queries the preset strategy table and, based on the corresponding disturbance level label, obtains the corresponding filter type, filter sampling window length, and state modeling method. The system combines each frame number, its corresponding filter type, sampling window size, and state model description into a structured configuration output, forming a filter response configuration dataset.

[0145] Generate kernel weight control factors according to filter response configuration data to obtain perturbation filter kernel data;

[0146] In one embodiment, the system presets a Gaussian kernel function as the basic weighting model of the filter. This kernel function is used to describe the weighted relationship between the current frame and its preceding and following time series frames. Its value is determined by the time index interval. The longer the frame interval, the lower the weight. Specifically, the weight of the kernel function decays exponentially with the time difference. Its function form is that the current weight is equal to the negative value of the exponential function. The exponential term is determined by the square of the time interval divided by twice the square of the kernel function scale. , is the weight value of the perturbation filter kernel function, is an exponential function, is the time index interval (inter-frame distance), is the filter kernel scale parameter (time series standard deviation). The kernel function scale parameter, or kernel width, is adjusted based on the perturbation level. The system sets different kernel scale values ​​for different perturbation levels: 1.0 for high perturbation levels, 0.6 for medium perturbation levels, and 0.3 for low perturbation levels. The system sequentially reads the perturbation level, filter window width, and type configuration for each frame. Based on the perturbation level, it matches the corresponding kernel scale value and constructs a symmetrical time window centered on the current frame within this scale. Within this time window, the kernel function values ​​are calculated for the current frame and the preceding and following frames, forming a complete sequence of filter kernel weights for that frame. For example, if the window width is 5 frames, the system takes five time positions, two frames forward and two frames backward, centered on the current frame. For each position, the system calculates the time interval between the position and the central frame and generates a corresponding weight using a preset kernel function. The system combines the time index corresponding to each frame with the generated filter kernel weight sequence to form the perturbation filter kernel data structure.

[0147] The perturbation filter kernel data is used to perform fusion filtering on the pose perturbation matching data to obtain navigation optimization data.

[0148] In one embodiment, the system first classifies all navigation frames according to the disturbance level marked in the configuration data. Specifically, for navigation frames in high-disturbance areas, the system constructs a nonlinear state update equation based on the previous state estimate. , is the current state vector, is the state transfer function (such as linear matrix multiplication or nonlinear mapping), is the previous state vector, is the process noise vector, is the current observation vector, is the observation function (for the lidar system, it represents the point cloud measurement model determined by the robot state; for the IMU sensor, it represents the expected value of angular velocity and acceleration; for the vision system, it represents the perspective projection function from world coordinates to image coordinates), To observe the noise vector, a set of representative Sigma points are selected in the state space, and the state prediction is realized by propagating these Sigma points in the state transfer function; then the prediction result is nonlinearly fused with the actual observation value, and weighted reconstruction is performed to obtain the optimal state estimate of the current frame. For the navigation frame in the medium interference area, the system performs Jacobian linearization on the nonlinear state model to obtain a linear approximation model; then the state estimation is performed according to the standard prediction-update filtering framework, that is, the state propagation is performed through the prediction step, and the observation information is fused through the update step to complete the state correction. For the navigation frame in the low interference area, the system takes the current frame as the center, selects multiple matching and corrected pose frames in the front and back time windows, and uses the aforementioned Gaussian kernel function weights to perform weighted averaging on the pose vectors of these frames to generate the optimized pose of the frame, that is , is the current optimized pose vector, is the relative time offset, is the half-width of the sliding window, which indicates the width of the range of consideration of the previous and next frames in the sliding weighted average, for example =3 means selecting the current frame and the 3 frames before and after it. is the Gaussian kernel weight factor, is the fused pose of adjacent frames within the sliding window. For the boundary frames at both ends, due to the insufficient number of adjacent frames, the system employs an edge processing mechanism. One approach is to use a mirror filling strategy, mirroring the missing frame data within the window; another approach is to retain the previous optimization result. The system outputs the fused filtering results of all navigation frames in chronological order, forming a complete navigation optimization trajectory sequence.

[0149] Optionally, S4 includes:

[0150] S41, performing cross-temporal and spatial scale residual field processing on the spatial point cloud data and the robot posture data according to the navigation optimization data to obtain residual trend data;

[0151] In one embodiment, for each frame moment, the system extracts the optimized posture and the reference posture respectively, and calculates the difference between the two. The residual vector is defined as the optimized posture minus the reference posture, which contains two parts, including the position residual, which measures the position difference with Euclidean distance and is expressed as a three-dimensional coordinate offset; the direction residual, which can be used to calculate the attitude angle change by quaternion difference or Euler angle difference to characterize the degree of orientation offset. The system uses a sliding time window method (for example, every 5 frames) to aggregate the residuals in the local time period to form a time trend curve. In the spatial dimension, the system maps the residual value of each frame to its corresponding voxel position in the spatial point cloud to achieve point-frame joint modeling. Specifically, for a certain position coordinate in space and a time index The system constructs a residual function to describe the strength of the pose difference between the optimized trajectory and the reference trajectory at that point in space and time. The output of this residual function is the residual modulus. The system output includes two result structures: one is a multidimensional residual tensor, represented as a set of residual values ​​indexed by spatial position and time; the other is a residual trend graph constructed based on the time series residuals. The time domain residual curve is smoothed using the exponentially weighted moving average method to highlight the dynamic trend of the system error change.

[0152] S42, performing residual classification on the residual trend data to obtain residual classification data;

[0153] In one embodiment, for each spatiotemporal position point in the residual trend data, the system extracts the following statistical characteristic indicators, including the average residual value, which calculates the average residual amplitude of the position in the current time window; the residual standard deviation, which is used to measure the fluctuation amplitude of the residual at the position in the time window; and the residual growth slope, which estimates the trend rate of residual change over time by taking the difference between the current average residual value and the average residual value of the previous time window and dividing it by the time interval.

[0154] Table 2: Residual type determination rules, as follows

[0155]

[0156] A residual growth slope of approximately zero can be defined as a trendless change within a certain tolerance (for example, an absolute value less than 0.005 meters per second). The system outputs the residual type tag for each spatiotemporal location, generating a residual classification data structure. This structure uses the time series index as the primary key and records the residual type at the corresponding moment.

[0157] S43, generating residual control parameters according to the residual classification data and a preset compensation parameter library to obtain residual control parameter data;

[0158] In one embodiment, a compensation parameter library is preset, which establishes a mapping relationship based on the residual type and contains the corresponding filtering strategy and its control parameters. Each residual type is associated with the following three parameters, including the filter type, which is used for the control method selected in the subsequent compensation process; the gain factor, which is used to adjust the residual feedback response strength; the calibration step size, which controls the iterative amplitude of the posture correction or compensation; and the threshold expansion parameter, which is used for adaptive adjustment of the tolerance range in the compensation strategy. The structure of the preset parameter library includes the following mapping relationships, as shown in Table 3,

[0159] Table 3: Preset parameter library mapping table

[0160]

[0161] For each residual classification result, the system searches the parameter library for a matching entry based on its corresponding type and extracts its associated compensation parameters. The frame index is then bound to the control parameters to form a control parameter vector, which includes the filter type, gain factor, calibration step, and threshold expansion factor. The system encapsulates the set of control parameters generated by the mapping into a residual control parameter dataset. Each record consists of a time frame index and its corresponding parameter vector, representing the residual control strategy for that frame.

[0162] S44. Optimize the spatial point cloud data and the robot pose data according to the residual control parameter data to obtain the spatial optimized point cloud data and the robot optimized pose data.

[0163] In one embodiment, the system selects a matching pose optimization method based on the residual classification results and their corresponding control parameters, and adjusts the optimized pose at each moment. The specific strategy is as follows: for steady-state residual frames, a sliding window averaging method is used to calculate the pose average within a certain range before and after the current position; for cumulative residual frames, a frame-by-frame fine-tuning strategy is implemented. The system linearly corrects the current residual vector using a calibration step factor, subtracting a weighted portion of the residual from the original optimized pose; for burst residual frames, a Kalman filter method is used to fuse state prediction and measurement updates for the current pose; and for offset residual frames, a keyframe-level registration correction strategy is introduced. The system selects historical high-confidence keyframes as relocalization benchmarks and reconstructs the current frame pose through image reprojection or geometric feature matching. This process generates a new optimized pose for each pose frame requiring residual control, forming the robot's optimized pose dataset. If the system detects that a frame pose has been updated and requires geometric verification, pose transformation processing is performed synchronously on the point cloud data corresponding to that frame. Specifically, the system calculates the global coordinate reprojection of the point cloud data based on the transformation matrix between the original and updated poses of the frame, mapping the original point cloud data to the updated spatial position, forming an optimized point cloud frame after spatial alignment. The system outputs two types of optimization results: a spatially optimized point cloud dataset, which contains all point cloud frames that have undergone pose transformation correction; and an optimized robot pose dataset, which contains the updated pose results at each moment.

[0164] Optionally, the present application further provides a precision inspection robot for an underground cavern, for performing the precision inspection method for an underground cavern as described above, wherein the precision inspection robot for an underground cavern comprises:

[0165] The multi-source sensing acquisition and magnetic field modeling module is used to obtain spatial point cloud data, robot posture data, and scene magnetic field data, and construct a magnetic field model based on the scene magnetic field data to obtain a scene magnetic field model;

[0166] The magnetic disturbance feature extraction module is used to extract disturbance features based on the scene magnetic field model and spatial point cloud data to obtain disturbance feature data;

[0167] The magnetic disturbance weight driven fusion filter module is used to calculate the magnetic disturbance weight based on the interference feature data and the robot posture data to obtain the magnetic disturbance weight data; the spatial point cloud data and the robot posture data are fused and filtered based on the magnetic disturbance weight data to obtain the navigation optimization data;

[0168] The residual trend analysis and compensation module is used to perform residual analysis on the spatial point cloud data and the robot posture data according to the navigation optimization data to obtain the residual trend data; and perform dynamic compensation according to the residual trend data to obtain the spatial optimized point cloud data and the robot optimized posture data.

[0169] Therefore, no matter from which point of view, the embodiments should be regarded as illustrative and non-restrictive, the scope of the present invention is limited by the attached application documents rather than the above description, and it is intended that all changes that fall within the meaning and scope of equivalent elements of the application documents are included in the present invention.

[0170] The foregoing description is intended only to provide specific embodiments of the present invention, which will enable those skilled in the art to understand and implement the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not intended to be limited to the embodiments shown herein, but is to be construed in the widest possible manner consistent with the principles and novel features disclosed herein.

Claims

1. A precise inspection method for underground caverns, characterized in that: The method comprises: S1. Acquire spatial point cloud data, robot posture data, and scene magnetic field data, and construct a magnetic field model based on the scene magnetic field data to obtain a scene magnetic field model; S2. Extract interference features based on the scene magnetic field model and spatial point cloud data to obtain interference feature data; S3. Construct a disturbance space confidence map based on the interference feature data and the robot posture data to obtain magnetic disturbance weight data; perform posture disturbance matching on the spatial point cloud data and the robot posture data based on the magnetic disturbance weight data to obtain posture disturbance matching data; perform high and low disturbance fusion filtering on the posture disturbance matching data to obtain navigation optimization data; S4. Perform residual analysis on the spatial point cloud data and the robot pose data based on the navigation optimization data to obtain residual trend data; perform dynamic compensation based on the residual trend data to obtain spatial optimized point cloud data and robot optimized pose data; The high and low interference fusion filtering includes: Perform perturbation layered matching annotation on the pose perturbation matching data to obtain pose perturbation layered data; Perform filter response configuration mapping according to the pose disturbance layered data to obtain filter response configuration data; Generate kernel weight control factors according to filter response configuration data to obtain perturbation filter kernel data; The perturbation filter kernel data is used to perform fusion filtering on the pose perturbation matching data to obtain navigation optimization data.

2. The method according to claim 1, characterized in that S1 includes: Acquire spatial point cloud data, robot posture data, and scene magnetic field data; Perform spatial mapping of magnetic field sampling points based on scene magnetic field data to obtain magnetic induction sampling spatial map data; Perform magnetic potential function interpolation on the magnetic induction sampling space map data to obtain magnetic potential space data; The magnetic potential space data is distorted and modeled to obtain the scene magnetic field model.

3. The method according to claim 2, characterized in that The distortion modeling includes: Calculating the magnetic potential gradient on the magnetic potential space data to obtain magnetic potential gradient data; Performing high-order perturbation metric calculation on magnetic potential gradient data to obtain high-order perturbation metric data; Perform local detection on high-order disturbance measurement data to obtain local abnormal data; Perform distortion cluster analysis based on local abnormal data to obtain distorted fast body data; The magnetic field model is encapsulated according to the distorted block data to obtain the scene magnetic field model.

4. The method according to claim 1, wherein S2 include: Extract candidate magnetic disturbance areas based on the scene magnetic field model and spatial point cloud data to obtain candidate magnetic disturbance area data; Performing point cloud local structure analysis on candidate magnetic disturbance area data to obtain point cloud local structure data; Perform interference layer division on the local structure data of the point cloud to obtain interference layer data; An interference feature tensor is generated according to the interference layer data to obtain interference feature data.

5. The method according to claim 4, characterized in that The magnetic disturbance area candidate extraction comprises: Interference confidence voxel screening is performed based on the scene magnetic field model and spatial point cloud data to obtain interference confidence voxel data; Clustering candidate regions are generated on the interference confidence voxel data to obtain magnetic disturbance candidate region data; Morphological analysis is performed on the magnetic disturbance candidate area data to obtain magnetic disturbance candidate area data.

6. The method according to claim 4, characterized in that The interference hierarchical division includes: Calculate the local point density change rate and coordinate disturbance rate of the point cloud local structure data to obtain the local point density change rate data and coordinate disturbance rate data; Clustering is performed based on the local point density change rate data and the coordinate disturbance rate data to obtain point cloud local clustering data; Performing interference local perception processing on the point cloud local structure data according to the point cloud local clustering data to obtain interference local perception data; Calculate the interference degree according to the local interference perception data to obtain interference degree data; According to the interference degree data, the local structure data of the point cloud is divided into interference intensity levels to obtain interference layered data.

7. The method according to claim 1, characterized in that S4 includes: Based on the navigation optimization data, the spatial point cloud data and the robot posture data are processed with residual fields across time and space scales to obtain residual trend data; Perform residual classification on the residual trend data to obtain residual classification data; Generating residual control parameters according to the residual classification data and a preset compensation parameter library to obtain residual control parameter data; The spatial point cloud data and robot posture data are optimized according to the residual control parameter data to obtain the spatial optimized point cloud data and the robot optimized posture data.

8. A precision inspection robot for underground caverns, characterized in that: For executing the precise inspection method of an underground cavern according to claim 1, the precise inspection robot for the underground cavern comprises: The multi-source sensing acquisition and magnetic field modeling module is used to obtain spatial point cloud data, robot posture data, and scene magnetic field data, and construct a magnetic field model based on the scene magnetic field data to obtain a scene magnetic field model; The magnetic disturbance feature extraction module is used to extract disturbance features based on the scene magnetic field model and spatial point cloud data to obtain disturbance feature data; The magnetic disturbance weight driven fusion filter module is used to calculate the magnetic disturbance weight based on the interference feature data and the robot posture data to obtain the magnetic disturbance weight data; the spatial point cloud data and the robot posture data are fused and filtered based on the magnetic disturbance weight data to obtain the navigation optimization data; The residual trend analysis and compensation module is used to perform residual analysis on the spatial point cloud data and the robot posture data according to the navigation optimization data to obtain the residual trend data; and perform dynamic compensation according to the residual trend data to obtain the spatial optimized point cloud data and the robot optimized posture data.

Images