Precise inspection method and inspection robot for underground cavern

Abstract

The invention relates to the technical field of robot navigation, in particular to a precise inspection method for an underground cavity and an inspection robot. The method comprises the following steps: acquiring spatial point cloud data, robot pose data and scene magnetic field data, and constructing a magnetic field model according to the scene magnetic field data to obtain a scene magnetic field model; obtaining interference characteristic data according to the scene magnetic field model and the spatial point cloud data; obtaining magnetic disturbance weight data according to the disturbance characteristic data and the robot pose data; performing fusion filtering on the spatial point cloud data and the robot pose data according to the magnetic disturbance weight data to obtain navigation optimization data; performing residual analysis on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; and performing dynamic compensation according to the residual trend data to obtain space optimization point cloud data and robot optimization pose data. The anti-interference capability and the environmental adaptability of the inspection robot are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot navigation, and particularly to a precise inspection method for underground chambers and an inspection robot. Background Art

[0002] With the development of intelligent robot technology, the demand for automated inspection in complex environments such as underground chambers, mines, tunnels, and underground pipe galleries is increasing continuously. Existing underground inspection robot systems mostly rely on lidar (LiDAR) and inertial navigation units (IMU) to achieve positioning and navigation, and complete environmental mapping and path planning by constructing a three-dimensional point cloud map. However, in practical applications, strong magnetic interference sources (such as metal components, power distribution equipment, ventilation systems, etc.) are widespread in underground spaces, resulting in easy drift of inertial navigation data, rapid accumulation of navigation errors, and thus affecting the accuracy of point cloud mapping and the anti-interference ability of inspection paths. In order to improve the robustness of the navigation system in an interference environment, some studies have tried to introduce magnetic field sensors to correct the output of the IMU, but mostly use static or mean filtering methods, which are difficult to effectively cope with problems such as complex spatial magnetic disturbance distribution and significant non-linear distortion. Summary of the Invention

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

[0004] The present application provides a precise inspection method for underground chambers, and the method includes: S1. Obtain spatial point cloud data, robot pose data, and scene magnetic field data, and construct a magnetic field model according to the scene magnetic field data to obtain a scene magnetic field model; S2. Extract interference features according to the scene magnetic field model and the spatial point cloud data to obtain interference feature data; S3. Calculate magnetic interference weights according to the interference feature data and the robot pose data to obtain magnetic interference weight data; perform fusion filtering on the spatial point cloud data and the robot pose data according to the magnetic interference weight data to obtain navigation optimization data; S4. Perform residual analysis on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; perform dynamic compensation according to the residual trend data to obtain spatially optimized point cloud data and robot optimized pose data.

[0005] In the present invention, through the introduction of multi-source perception fusion of spatial point cloud data, robot pose data, and scene magnetic field data, combined with the construction of a magnetic field model and the extraction of interference characteristics, the accurate identification and quantitative modeling of the navigation system error in a magnetic field perturbation environment are achieved. By using a fusion filter driven by magnetic disturbance weights, the collaborative robustness of lidar and inertial navigation systems in a complex magnetic field environment is significantly improved, effectively avoiding the influence of inertial navigation drift on positioning accuracy. Through residual trend analysis and a dynamic compensation mechanism, the adaptive optimization and closed-loop correction of the navigation path and point cloud reconstruction results are realized, improving the path reproduction accuracy and spatial modeling accuracy in inspection tasks.

[0006] Optionally, S1 includes: Obtain spatial point cloud data, robot pose data, and scene magnetic field data; Perform spatial mapping of magnetic field sampling points according to the scene magnetic field data to obtain magnetic induction sampling space map data; Perform magnetic potential function interpolation on the magnetic induction sampling space map data to obtain magnetic potential space data; Perform distortion modeling on the magnetic potential space data to obtain a scene magnetic field model.

[0007] In the present invention, by introducing spatial mapping of magnetic field sampling points and magnetic potential function interpolation calculation, a high-fidelity conversion channel from original magnetic field data to continuous magnetic potential distribution is constructed, significantly improving the analysis accuracy of magnetic field environment information. By generating magnetic induction sampling space map data and using magnetic potential function interpolation method on this basis, continuous reconstruction can be achieved in areas with magnetic field occlusion or noise perturbation, avoiding the problems of sparse punctual data and amplified local anomalies in traditional magnetic field modeling. The distortion modeling process based on magnetic potential space data can effectively characterize the non-linear magnetic field distortion characteristics caused by metal structures and complex spatial layouts in underground caverns, providing an accurate physical model support for magnetic disturbance compensation and fusion positioning.

[0008] Optionally, the distortion modeling includes: Perform magnetic potential gradient calculation on the magnetic potential space data to obtain magnetic potential gradient data; Perform high-order perturbation metric calculation on the magnetic potential gradient data to obtain high-order perturbation metric data; Perform local detection on the high-order perturbation metric data to obtain local anomaly data; Perform distortion agglomeration analysis according to the local anomaly data to obtain distortion block data; Perform magnetic field model encapsulation according to the distortion block data to obtain a scene magnetic field model.

[0009] In the present invention, the distortion modeling step constructs a multi-scale detection mechanism from a continuous magnetic potential field to local magnetic disturbance feature recognition by performing gradient analysis and high-order perturbation measurement on the magnetic potential space data. Through magnetic potential gradient calculation, the variation trend of the magnetic field in space can be accurately revealed, providing a basis for judging the disturbance intensity and direction; by introducing a high-order perturbation measurement method, magnetic disturbance structures such as non-linear variations and local vortices can be identified, which is significantly superior to the adaptability of the traditional first-order gradient method in high-interference regions. Combining local detection with distortion agglomeration analysis, discrete magnetic disturbance anomaly points can be aggregated into distortion blocks with spatial continuity and structural boundaries, realizing the upgrade of expression from point anomaly to regional anomaly. Through the encapsulation modeling of the distortion blocks, a high-resolution and structured magnetic field environment model is formed, providing an interference shielding boundary and dynamic correction basis for path planning and navigation filtering.

[0010] Optionally, S2 includes: Extract candidate regions of the magnetic disturbance area according to the scene magnetic field model and the spatial point cloud data to obtain candidate region data of the magnetic disturbance area; Perform local structure analysis of the point cloud on the candidate region data of the magnetic disturbance area to obtain local structure data of the point cloud; Perform interference layer division on the local structure data of the point cloud to obtain interference layer data; Generate interference feature tensors according to the interference layer data to obtain interference feature data.

[0011] In the present invention, by fusing the scene magnetic field model and the spatial point cloud data, a hierarchical recognition and feature extraction mechanism for magnetic disturbance structures is constructed, significantly improving the accuracy of magnetic disturbance source positioning and influence range assessment in the underground environment. Through the spatial extraction of candidate regions of the magnetic disturbance area, the analysis range can be accurately limited to the area where the magnetic field anomalies are concentrated, reducing the redundant calculation burden; then, combined with the local structure analysis of the point cloud, not only the spatial geometric features can be identified, but also the structural features that may cause magnetic disturbance aggregation, such as complex components or structural gaps, can be mined. Through interference layer division, a multi-level interference mapping relationship can be constructed according to factors such as point cloud density change and edge gradient, effectively separating the surface interference and deep interference effects. Generating 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.

[0012] Optionally, the extraction of the candidate regions of the magnetic disturbance area includes: Perform screening of interference confidence voxels according to the scene magnetic field model and the spatial point cloud data to obtain interference confidence voxel data; Generate clustering candidate regions for the interference confidence voxel data to obtain magnetic disturbance candidate region data; Perform morphological analysis on the magnetic disturbance candidate region data to obtain candidate region data of the magnetic disturbance area.

[0013] In the present invention, the extraction of candidate regions of the magnetic disturbance area constructs a three-dimensional magnetic disturbance detection path based on the collaborative recognition of a magnetic field model and spatial point cloud by introducing a screening mechanism for interfering confidence voxels. Through the joint analysis of the magnetic field model and point cloud data, the screening of interfering confidence voxels is performed, and regions with a relatively high magnetic disturbance intensity and abnormal spatial structure features can be accurately identified at the voxel level, significantly improving the positioning accuracy of candidate regions; based on voxel data, clustering analysis is performed to generate magnetic disturbance candidate regions with strong aggregation and continuous boundaries, which not only retains the geometric shape of the disturbance source but also enhances the distinguishability and spatial closure of the regions; combined with morphological analysis, scale filtering, morphological fitting, and boundary clipping can be performed on the clustering results to exclude error interference regions and refine candidate regions of the magnetic disturbance area that conform to the actual influence range of navigation interference.

[0014] Optionally, the interference hierarchical division includes: Calculate the local point density change rate and coordinate perturbation rate for the local structure data of the point cloud to obtain local point density change rate data and coordinate perturbation rate data; Perform clustering processing based on the local point density change rate data and coordinate perturbation rate data to obtain local clustering data of the point cloud; Perform interference local perception processing on the local structure data of the point cloud according to the local clustering data of the point cloud to obtain interference local perception data; Calculate the interference degree according to the interference local perception data to obtain interference degree data; Perform interference intensity level division on the local structure data of the point cloud according to the interference degree data to obtain interference hierarchical data.

[0015] In the present invention, the calculation of the local point density change rate can reveal the changing trends such as contraction and loosening of the spatial structure under the influence of magnetic disturbance, and the coordinate perturbation rate is used to quantify the spatial drift degree of point cloud nodes under continuous frames or multi-source observations, so as to capture the geometric distortion characteristics caused by magnetic disturbance. Based on the two for clustering processing not only enhances the perception ability of abnormal structure patterns but also improves the stability of noise filtering. Introducing interference local perception processing realizes the reconstruction of the local response area affected by magnetic disturbance at the structural semantic level, providing accurate data support for calculation. Through interference degree calculation and hierarchical division, interference hierarchical data with hierarchical expression ability is 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 shift in traditional methods, this method combines local dynamic behavior and hierarchical representation, significantly improving the expressiveness and engineering applicability of magnetic disturbance spatial response modeling.

[0016] Optionally, S3 includes: Construct a perturbation space confidence map according to the interference feature data and robot pose data to obtain magnetic disturbance weight data; Perform pose perturbation matching on the spatial point cloud data and the robot pose data according to the magnetic disturbance weight data to obtain pose perturbation matching data; Perform high-low disturbance fusion filtering on the pose perturbation matching data to obtain navigation optimization data.

[0017] In the present invention, a perturbation space confidence map is constructed through the joint analysis of interference feature data and robot pose data, which can realize the spatial encoding of the magnetic disturbance intensity and navigation credibility in different regions, thereby endowing each pose point with a differential magnetic disturbance weight expression, significantly improving the local adaptability of interference modeling and the navigation weight regulation ability. Through the pose perturbation matching process guided by the magnetic disturbance weight, the geometric calibration of the point cloud and the pose sequence can be enhanced in high-interference regions, and the stability of the navigation system can be maintained in low-interference regions, forming a differential matching scheme for dynamic response to interference. The fusion matching results are filtered by high and low disturbances, combined with the extended Kalman filter or unscented Kalman filter strategy, to achieve refined denoising and error compensation of the navigation trajectory.

[0018] Optionally, the high-low disturbance fusion filtering includes: Perform perturbation hierarchical matching annotation on the pose perturbation matching data to obtain pose perturbation hierarchical data; Perform filtering response configuration mapping according to the pose perturbation hierarchical data to obtain filtering response configuration data; Generate a kernel weight regulation factor according to the filtering response configuration data to obtain perturbation filtering kernel data; Perform fusion filtering on the pose perturbation matching data by using the perturbation filtering kernel data to obtain navigation optimization data.

[0019] In the present invention, based on the pose perturbation matching data, perturbation hierarchical matching annotation is performed, and the key frames in the navigation path can be hierarchically divided according to the strength of the perturbation, accurately depicting the response states of navigation nodes under different interference levels. Through the configuration mapping between the perturbation level and the filtering strategy, filtering control parameters with different responses are generated to ensure that the system enhances the suppression response in high-perturbation regions and maintains the smoothness of the trajectory in low-perturbation regions. A kernel weight regulation factor is generated through the mapping result to construct a perturbation filtering kernel, realizing the dynamic adjustment of the filtering window shape, weight distribution, and kernel function type. During the filtering execution process, the perturbation filtering kernel is introduced to perform fusion filtering on the pose perturbation matching data, which not only ensures the fast response of the filter to high-frequency perturbations but also avoids over-smoothing in low-interference regions, retaining the fine structure of the navigation path to the greatest extent.

[0020] Optionally, S4 includes: Perform cross-temporal and spatial scale residual field processing on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; Perform residual classification on the residual trend data to obtain residual classification data; Generate residual regulation parameters based on the residual classification data and a preset compensation parameter library to obtain residual regulation parameter data; Optimize the spatial point cloud data and the robot pose data according to the residual regulation parameter data to obtain optimized spatial point cloud data and optimized robot pose data.

[0021] In the present invention, the cross - temporal - scale residual field processing performed based on the navigation optimization data can not only identify short - term drift errors, but also capture the trend of long - term cumulative errors, forming a complete error evolution trajectory, laying a foundation for refined compensation. By classifying the residual trend data, systematic errors (such as magnetic disturbance drift) and accidental errors (such as environmental occlusion) can be effectively distinguished, realizing the structured expression of error types. Combining with the preset compensation parameter library, the system automatically calls the most matching regulation strategy according to different residual types to generate residual regulation parameters, thus realizing diversified compensation mapping. Driven by the regulation parameters, the optimization adjustment of the spatial point cloud data and the pose data not only improves the coupling accuracy between the point cloud and the navigation trajectory, but also significantly enhances the path coherence and the environmental model fitting ability.

[0022] Optionally, the present application also provides a precise inspection robot for underground caverns, which is used to execute the precise inspection method for underground caverns as described above. The precise inspection robot for underground caverns includes: A multi - source perception acquisition and magnetic field modeling module, which is used to obtain spatial point cloud data, robot pose data, and scene magnetic field data, and construct a magnetic field model according to the scene magnetic field data to obtain a scene magnetic field model; A magnetic disturbance feature extraction module, which is used to extract disturbance features according to the scene magnetic field model and the spatial point cloud data to obtain disturbance feature data; A magnetic disturbance weight - driven fusion filtering module, which is used to calculate magnetic disturbance weights according to the disturbance feature data and the robot pose data to obtain magnetic disturbance weight data; perform fusion filtering on the spatial point cloud data and the robot pose data according to the magnetic disturbance weight data to obtain navigation optimization data; A residual trend analysis and compensation module, which is used to perform residual analysis on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; perform dynamic compensation according to the residual trend data to obtain optimized spatial point cloud data and optimized robot pose data.

[0023] The object of the present invention is to effectively characterize the local non-linear magnetic disturbance structure in the underground space by constructing a three-dimensional magnetic field model based on the magnetic potential gradient and the distorted block, providing 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 identification ability for multi-scale interference. A magnetic disturbance weight regulation factor is introduced in the fusion filtering process, enabling the filtering strategy to have the ability to respond to regional differences and effectively balancing the robustness in high-interference areas and the fidelity in low-interference areas. The residual trend analysis combined with the regulation parameter library forms a closed-loop dynamic compensation mechanism to achieve continuous cross-time domain correction of navigation and mapping errors. Overall, the present invention breaks through the technical bottlenecks of unstable positioning and distorted mapping in the traditional inspection method in an electromagnetic interference environment, and has the advantages of high robustness, high precision, and adaptability. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Other features, objects, and advantages of the present application will become more apparent by reading the following detailed description of non-limiting embodiments with reference to the accompanying drawings: Figure 1 The flowchart showing the steps of a precise inspection method for an underground chamber in one embodiment; Figure 2 The flowchart showing the steps of a multi-source perception acquisition and magnetic field modeling method in one embodiment; Figure 3 The flowchart showing the steps of a magnetic disturbance feature extraction method in one embodiment; Figure 4 The flowchart showing the steps of a magnetic disturbance weight-driven fusion filtering method in one embodiment; Figure 5 The flowchart showing the steps of a residual trend analysis and compensation method in one embodiment; The implementation, functional characteristics, and advantages of the object of the present invention will be further described with reference to the accompanying drawings in combination with the embodiments. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0025] The technical method of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some of the embodiments of the present invention, rather than all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative work belong to the scope of protection of the present invention.

[0026] In addition, the accompanying drawings are only schematic diagrams of the present invention and are not necessarily drawn to scale. The functional entities can be implemented in software form, or in one or more hardware modules or integrated circuits, or in different networks and / or processor methods and / or microcontroller methods.

[0027] It should be understood that although terms such as "first", "second", etc. may be used herein to describe various units, these units should not be limited by these terms. These terms are only used to distinguish one unit from another. For example, without departing from the scope of the exemplary embodiments, the first unit may be referred to as the second unit, and similarly, the second unit may be referred to as the first unit. The term "and / or" used herein includes any and all combinations of one or more of the listed related items.

[0028] Please refer to Figures 1 to 5 , this application provides a precise inspection method for underground chambers, and the method includes: S1. Obtain spatial point cloud data, robot pose 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; In one embodiment, the system obtains spatial point cloud data, robot pose data, and scene magnetic field data. Specifically, the spatial point cloud data is collected by a 360-degree rotating lidar deployed on the inspection robot, with a sampling frequency of 5 Hz, and a three-dimensional point cloud frame sequence containing spatial structure information can be generated; the robot pose data is output by a fiber optic inertial navigation unit (FOG), providing inertial navigation parameters including three-axis attitude angles and accelerations; the scene magnetic field data is measured by a three-axis fluxgate magnetometer, with the unit of nanotesla (nT) and a sampling frequency of 10 Hz. The system registers each magnetic field sampling point in a three-dimensional space using the point cloud coordinate system, that is, pairs and binds the magnetic field measurement value with its position in the point cloud space. The entire space is divided into equidistant three-dimensional voxel grids (each voxel size is 0.5 meters 0.5 meters 0.5 m), and perform weighted averaging on the measured magnetic field values within each voxel to generate magnetic induction sampling space map data. Based on the magnetic induction sampling space map data, the system further constructs a continuous magnetic potential field model. The specific method is to use a spherical Gaussian kernel function to construct a radial basis function (RBF) interpolation model to interpolate and reconstruct the discrete magnetic flux values. The interpolation function takes the spatial point position as the independent variable and the interpolation value as the magnetic potential function value, and its form is: the magnetic potential value is equal to the weighted sum of several weighted kernel functions; the center of each kernel function corresponds to a magnetic field sampling point, the weight is related to the magnetic induction intensity, and the width of the kernel function is controlled by the interpolation scale parameter, which is used to adjust the spatial smoothness and local response ability of the interpolation. The system performs distortion modeling on the magnetic potential space data. Calculate the first-order gradient values of the magnetic potential field in each direction to characterize the direction and rate of magnetic field change; calculate the perturbation tensor related to the third derivative to measure the nonlinear degree or spatial complexity of the local magnetic field change. If the perturbation metric value of a certain region exceeds the set threshold (for example, the magnetic potential change rate exceeds 10%), then this region is determined to have abnormal magnetic field distortion. The system performs spatial clustering on these abnormal voxel regions, aggregates adjacent high-perturbation regions into several distortion blocks, and uses the Octree structure to encapsulate their spatial boundaries and shapes. The distortion blocks and the continuous magnetic potential field together constitute a complete scene magnetic field model.

[0029] S2. Extract interference feature data according to the scene magnetic field model and the spatial point cloud data; In one embodiment, the system uses the voxels in the magnetic field model that overlap with the point cloud spatial region as the detection objects, and calculates the magnetic disturbance intensity index within each voxel. If the magnetic disturbance intensity of a certain voxel is greater than the set threshold (for example, 50 nT), then this voxel is determined to be a candidate region with interference confidence, denoted as an interference confidence voxel. Use a density-based clustering algorithm (such as DBSCAN) to perform spatial clustering on the interference confidence voxels, set the clustering radius to 1.0 m, and the minimum number of clustering points to 10. The clustering results form multiple preliminary magnetic disturbance candidate regions. For each candidate region, the system uses the principal component analysis method to extract its geometric principal axis direction, and calculates its morphological indexes such as length, width, and flatness accordingly. If the candidate region shows a blocky feature in structure, that is, the principal axis length does not exceed a certain threshold and the structural compactness meets the set range, then this region is retained as a magnetic disturbance region candidate. Within the candidate region range, perform local point cloud structure analysis. 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 m) divided by the neighborhood volume. The formula , point 's local density can be expressed as the number of neighboring points contained in the specified spatial spherical neighborhood of this point and the volume of this sphere The ratio. The system calculates the spatial perturbation rate of points at consecutive moments, that is, the difference in spatial coordinates of the same point in the previous and next frames divided by the time interval between frames. After obtaining the local density and the coordinate perturbation rate, the system uses an unsupervised clustering method (such as K-means) to perform joint clustering on the two, and the number of clustering categories is set to 3, corresponding to three levels of "strong perturbation", "medium perturbation" and "weak perturbation" respectively. After clustering, an interference feature tensor is generated for each point, and the tensor includes the density value, perturbation rate value and clustering result label of the point. The form of this tensor is a triple, representing the point structure density, dynamic perturbation degree and the type of perturbation stratification to which it belongs, constituting an interference feature data set.

[0030] S3. Calculate the magnetic interference weight according to the interference feature data and the robot pose data to obtain the magnetic interference weight data; perform fusion filtering on the spatial point cloud data and the robot pose data according to the magnetic interference weight data to obtain the navigation optimization data; In one embodiment, the system constructs a perturbation space confidence map based on interference feature data, uses each navigation key frame or robot pose node as a map node, and calculates its corresponding perturbation confidence value. For each pose frame, the perturbation rates of all points within its neighborhood range are statistically analyzed, and their average value is calculated, denoted as the average perturbation rate of this frame. The system uses the Sigmoid function to perform a non-linear mapping on the average perturbation rate to generate a perturbation weight between 0 and 1, which is used to reflect the uncertainty degree of the current pose frame in the global magnetic perturbation space. The scaling coefficient of the Sigmoid function can be set according to experimental experience (such as the α value is used to adjust the perturbation sensitivity) to achieve a gradient response to different interference levels. Subsequently, pose perturbation matching is performed. The system separately obtains the pose estimation output by the lidar SLAM module and the inertial navigation pose provided by the fiber optic inertial navigation FOG module, and calculates the Euclidean distance residual between the two as the pose inconsistency metric for the current frame. If this residual value exceeds the product of the perturbation weight of this frame and the set error threshold (i.e., the residual is higher than the upper limit modulated by the confidence threshold), the system determines that there is a potential alignment error in this frame. At this time, a registration re-alignment operation is triggered, and the iterative closest point algorithm is used to perform fine registration and correction on the current frame point cloud and the map, thereby updating the pose estimation of the current frame. After completing the matching correction, the system performs perturbation hierarchical annotation on each frame according to the perturbation weight, and divides all pose frames into three categories: high perturbation, medium perturbation, and low perturbation. For frames of different levels, the system looks up the table to configure different filtering strategies: for high-perturbation regions, the unscented Kalman filter (UKF) is used to cope with the strong interference environment under the non-linear state model; for medium-perturbation or low-perturbation regions, the extended Kalman filter (EKF) is selected for state update, taking into account both computational efficiency and stability. The system configures an adaptive filtering kernel for each perturbation level. The filtering kernel adopts the form of a Gaussian function, and a perturbation regulation factor is introduced as a dynamic adjustment parameter for the kernel width, so that the filtering kernel responds faster and the weight changes stronger in a high-perturbation environment, while remaining smooth and stable in a low-perturbation region. Based on the constructed perturbation level mapping relationship and regulation kernel, the system performs temporal weighted fusion filtering on the entire pose sequence, thereby generating navigation optimization data with both accuracy, robustness, and trajectory continuity.

[0031] S4. Perform residual analysis on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; perform dynamic compensation according to the residual trend data to obtain spatially optimized point cloud data and robot optimized pose data.

[0032] 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 estimated point and the reference true point. The reference point can be derived from the high-precision trajectory estimation of the visual SLAM system or the quasi-reference value provided by manual annotation. In the three-dimensional voxel space, the system performs sliding statistical analysis on the navigation residual based on a fixed time window, calculates the residual mean, residual growth rate, and fluctuation degree of each voxel region within consecutive moments. Subsequently, residual classification processing is performed. The system aggregates all residual points into a residual point set and uses a density clustering algorithm (such as DBSCAN) to cluster it in three-dimensional space. For each clustering cluster, the system calculates its average residual amplitude and classifies it into three categories according to preset rules: if the average residual is greater than 20 cm, it is determined as the systematic error category, caused by magnetic interference or sensor offset; if the residual is between 5 and 20 cm, it is classified as the noise category, caused by environmental occlusion, short-term drift, or perceptual ambiguity; if the residual is less than 5 cm, it is identified as the normal error category. Each category will be assigned a label for compensation strategy selection. The system generates corresponding regulation parameters according to the residual classification label. The system pre-establishes a regulation parameter library, and different types of error categories will be mapped to different parameter configurations. For example: the systematic error category will adopt the gain adjustment strategy in the Kalman filter to improve the update response speed; the noise category error will adopt the filter weight compression strategy to weaken the interference of outliers on state estimation. Each residual region will be mapped to a set of regulation parameter vectors, and the parameter content includes filter gain factors, residual suppression weight factors, and state correction step factors, etc. In the compensation execution stage, the system performs adjustment operations on the corresponding point cloud segment and pose frame according to the above parameter vector. Point cloud compensation realizes structural alignment through coordinate transformation, and pose correction realizes state translation and direction fine-tuning through the filter update formula. The compensation process takes into account the residual trend direction, spatial aggregation structure, and pose continuity, and outputs the spatially optimized point cloud data and robot optimized pose data, realizing closed-loop error control and stability enhancement of the navigation chain.

[0033] Optionally, S1 includes: S11. Obtain spatial point cloud data, robot pose data, and scene magnetic field data; In one embodiment, the system uses a 360-degree omnidirectional mechanical rotating lidar (such as Velodyne HDL-32E) to scan the target space and obtains three-dimensional point cloud data at a frequency of 5 Hz. Each scan generates a set of three-dimensional point cloud frames. Each frame of data is attached with a timestamp and contains a number of three-dimensional spatial coordinate points in the coordinate format of (x, y, z), which are used to describe the position distribution of each measurement point in the target space. The system uses a combination of a fiber optic inertial navigation system (FOG IMU) and a wheel encoder to obtain the six-degree-of-freedom pose information of the robot during operation. The six degrees of freedom include three-dimensional position coordinates (x, y, z) and three-dimensional attitude information (which can be represented by Euler angles, including roll angle φ, pitch angle θ, and yaw angle ψ, or the rotation attitude can be represented in the form of quaternions). The pose data sampling frequency is not less than 100 Hz and is aligned with the point cloud data through timestamps. The system senses the magnetic field strength of the environment where the robot is located by carrying a three-axis fluxgate magnetometer. The measurement range of this magnetometer is ±1000 microtesla (μT), and it collects the environmental magnetic field strength data of the current robot at a frequency of 10 Hz. The magnetic field strength recorded at each moment is represented in vector form and contains three components, namely the magnetic induction intensities along the X-axis, Y-axis, and Z-axis directions (i.e., Bx, By, and Bz). The magnetic field data is synchronously bound with the timestamp and pose information of the robot to form a magnetic field strength data sequence containing spatial reference information.

[0034] S12. Perform spatial mapping of the magnetic field sampling points according to the scene magnetic field data to obtain the magnetic induction sampling space map data; In one embodiment, the system performs coordinate transformation on the magnetic induction intensity vector of each magnetic field sampling point, mapping it from the local coordinate system of the sensor to the global space coordinate system. Specifically, at each moment when the robot performs the sampling task, the system obtains its corresponding rotation attitude information and calculates the rotation matrix at that moment. Multiply the magnetic induction intensity vector by the rotation matrix to complete the transformation from the sensor coordinate system to the world coordinate system, and obtain the magnetic field intensity vector data under the unified space reference. After completing the spatial coordinate transformation, the system divides the entire target inspection area into three dimensions and constructs an equidistant 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 classifies each magnetic field sampling point into the corresponding voxel unit according to its position in the global coordinate system to realize the spatial classification of the magnetic induction data. For multiple magnetic field sampling points belonging to the same voxel, the system performs a summary process on their magnetic induction intensities. Specifically, vectorially sum all the magnetic induction intensity vectors falling within the same voxel and divide by the total number of sampling points within 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.

[0035] S13. Interpolate the magnetic potential function for the magnetic induction sampling space map data to obtain magnetic potential space data; In one embodiment, to achieve continuous modeling of the 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 spatial point are determined by the negative gradient of the magnetic potential field at that point. Expressed in mathematical terms, the magnetic field intensity vector is equal to the result of the negative gradient operation of the magnetic potential function with respect to the spatial position. The system uses a multi-core radial basis function as the interpolation method. Each sampling point is regarded as the center of a radial basis function, and the system performs a weighted superposition operation on the center coordinate points of each voxel to estimate its corresponding magnetic potential value. The general form of the interpolation function is as follows: the magnetic potential value at each spatial position is obtained by the superposition of weighted Gaussian functions from all sampling points. Each Gaussian function is centered at a sampling point, and its amplitude is determined by the magnetic induction intensity at that sampling point. The specific weight parameters are determined by 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 takes the center position of each voxel as the interpolation target point and calculates its magnetic potential function value one by one. 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 takes voxels as the basic unit and has a complete spatial distribution characteristic.

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

[0037] In one embodiment, the system first performs a numerical gradient calculation on the magnetic potential scalar field data generated in the previous stage to obtain the gradient vector field of the magnetic potential field in three-dimensional space. This calculation uses a three-dimensional central difference method, that is, at the center point of each voxel, 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 to approximately estimate the gradient value. The obtained gradient vector represents the change direction and change rate 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 parts: one is to calculate the second-order gradient of the magnetic potential field (i.e., the Laplacian approximation); the other is to statistically analyze the variance change of the magnetic induction intensity at each spatial position. Combining the above two indexes, the system defines an interference energy function, which represents 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 magnetic induction intensity variance, is the magnetic induction intensity vector. The energy function is composed of the weighted combination of the square of the modulus of the second derivative of the magnetic potential field and the variance term of the magnetic field strength, where the weighting factor can be set as a hyperparameter. The system sets a predefined threshold (for example, set as the value corresponding to the 95th percentile) according to the defined interference energy index to divide the abnormally high perturbation region. All spatial points exceeding this threshold are determined as the magnetic field distortion region. The system uses a spatial clustering method (such as the density-based DBSCAN algorithm) to aggregate the spatially adjacent or structurally connected high-perturbation point sets into multiple closed three-dimensional block structures. Each block represents an independent magnetic distortion region, and an octree structure or a three-dimensional bounding box structure can be used for boundary encapsulation and management. The system integrates the foregoing processing results to construct a complete scene magnetic field model. This model contains the following three types of core data: (1) the continuous magnetic potential function; (2) the magnetic potential gradient vector field; (3) the extracted data structures of multiple magnetic field distortion blocks.

[0038] Optionally, the distortion modeling includes: Calculating the magnetic potential gradient of the magnetic potential spatial data to obtain magnetic potential gradient data; In one embodiment, for the magnetic potential spatial data Performing a three-dimensional central difference method to calculate the gradient vector of the magnetic potential at the center of each voxel , which is defined as , where is the magnetic potential scalar field, is the spatial axis position, is the spatial axis position, is the spatial axis position, is the forward magnetic potential value in the x direction, is the backward magnetic potential value in the x direction, is the voxel spacing / coordinate difference, and the gradient vector at each voxel is stored as a magnetic potential gradient tensor field .

[0039] Calculating the high-order perturbation metric of the magnetic potential gradient data to obtain high-order perturbation metric data; In one embodiment, further performing a gradient rate calculation on to obtain the gradient change intensity at each point, that is, the second derivative or a Laplacian-like quantity: , combined with the local magnetic induction value fluctuation variance , to obtain the high-order perturbation metric index: , where is the empirical weight (such as ), forming a perturbation energy field .

[0040] Perform local detection on high-order perturbation metric data to obtain local abnormal data; In one embodiment, on set a local anomaly determination threshold , which can be set empirically or adaptively determined by statistical distribution (such as the 95th percentile); mark all voxels that satisfy: , as abnormal perturbation points; and thus output as an abnormal point set , which serves as the basic data for distortion agglomeration analysis.

[0041] Perform distortion agglomeration analysis based on the local abnormal data to obtain distorted bulk data; In one embodiment, based on the obtained local abnormal point set, to identify the structural distribution characteristics of the magnetic field abnormal region, the system uses a spatial clustering algorithm to perform clustering analysis on the abnormal point set. Preferably, a density-based spatial clustering method is used. The parameters set for the clustering process include the spatial distance threshold is 1.0 meter, and the minimum number of clustering points minPts is 20 points, that is, if there are no less than 20 neighboring abnormal points within 1 meter around any point, an initial clustering core point can be formed. After the clustering analysis is completed, each clustering cluster represents a potential distortion region. For each clustering cluster, the system performs morphological description analysis, specifically including the following content: Calculate the minimum three-dimensional bounding box of the abnormal points included in the clustering cluster to describe its spatial boundary range; calculate the total volume of the voxels it contains; extract the spatial geometric center point coordinates of this region. The system sets a block determination rule: When the number of voxels within the clustering cluster exceeds 50 and the calculated volume exceeds 0.25 cubic meters, it is considered to have significant spatial block characteristics. For each clustering 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 effective distortion region. All distorted block units that meet the conditions together constitute a distorted block data set, denoted as , which is represented as an ordered structure set composed of Block_1, Block_2 to Block_n.

[0042] Perform magnetic field model encapsulation based on the distorted block data to obtain a scene magnetic field model.

[0043] In one embodiment, this structured magnetic field model is denoted as , and it contains the following core components: a continuous magnetic potential scalar field, taking the spatial position coordinates as input, performing continuous processing on the discrete magnetic potential data ϕ through radial basis function interpolation to generate a continuous magnetic potential scalar field throughout the space . The first-order magnetic potential gradient tensor field, based on the continuous magnetic potential field , calculates its gradient distribution in space through a three-dimensional central difference method to form a first-order magnetic potential gradient tensor field The high-order perturbation metric field combines the second derivative information of the magnetic potential and the local magnetic induction fluctuation variance to construct a high-order perturbation metric index. This forms the perturbation energy tensor field. The distorted block list structurally organizes the data of the clustered distorted regions to form a set of distorted blocks. This set contains all distorted regions that meet the spatial distribution and volume characteristics and can be stored using a spatial indexing structure (such as an octree or voxel grid). The magnetic field model object organizes the above four types of data into a unified structure as a multi-level expression form of the scene magnetic field.

[0044] Optionally, S2 includes: S21. Extract candidate regions of the magnetic disturbance area based on the scene magnetic field model and the spatial point cloud data to obtain candidate region data of the magnetic disturbance area. In one embodiment, before extracting the candidate regions, the system performs voxelization on the entire three-dimensional space. For each voxel grid point, the system obtains the perturbation energy value at the corresponding position from the magnetic field model. If the perturbation energy intensity at this position is higher than the set confidence threshold (for example, taking the 95th percentile of the high-order perturbation energy distribution as the threshold), then this voxel is marked as an interference confidence voxel. The system performs spatial clustering based on the voxel adjacency relationship and merges the connected sets of interference confidence voxels into several preliminary perturbation candidate regions. The clustering method uses a three-dimensional 8-neighborhood connectivity strategy, that is, any two adjacent voxels sharing at least one face, edge, or vertex are considered connected. For each candidate region, 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 the flatness in the main axis direction, and the compactness of the overall spatial distribution. The system sets the following filtering conditions, such as the number of voxels contained in the candidate region should be no less than 30, the ratio of the length in the main axis direction to the width of the secondary axis should not exceed 3 times, and the minimum enclosing diameter of the candidate region in space should not be less than 1.5 meters. Only the regions that meet the above constraints are retained as candidate regions of the magnetic disturbance area. The output candidate regions of the magnetic disturbance area are a set of spatial bounding volumes, and each bounding volume represents a spatial region that may interfere with navigation, denoted as a candidate region set composed of several regions.

[0045] S22. Perform point cloud local structure analysis on the candidate region data of the magnetic disturbance area to obtain point cloud local structure data. 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.

[0046] S23, performing interference layer division on the point cloud local structure data to obtain interference layer data; 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.

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

[0048] In one embodiment, for each point for which the disturbance level division has been completed, that is, each spatial point in the interference stratified data, the system constructs its corresponding interference feature vector. This feature vector consists of the following five elements, including the local point density value, which represents the degree of point cloud aggregation of the point in space; the normal vector perturbation rate, which represents the change amplitude of the normal direction of the point in consecutive frames; the interference level label, with values of 1 (low interference), 2 (medium interference), or 3 (high interference), determined by the results of the previous-stage hierarchical clustering; the perturbation energy value, extracted from the perturbation intensity map in the scene magnetic field model, used to reflect the absolute energy of the local magnetic field perturbation; and the magnitude of the magnetic potential gradient, which represents the magnitude of the magnetic potential gradient at the position of the point. The system combines the above five feature fields into a local tensor unit, and each tensor is recorded with the corresponding point as the index.

[0049] Optionally, the extraction of the candidate region of the magnetic disturbance area includes: Performing interference confidence voxel screening based on the scene magnetic field model and the spatial point cloud data to obtain interference confidence voxel data; In one embodiment, the input magnetic potential field perturbation energy tensor (from the output of S1 magnetic potential modeling); the spatial point cloud data , which has been voxelized into a voxel grid . For each voxel , calculate its corresponding magnetic potential perturbation energy value ; set the perturbation confidence threshold , and adopt , where and are the mean and standard deviation of the global magnetic potential perturbation; screen the voxels that meet the following conditions: , the number of point clouds in the voxel ≥ , such as set to 10 points. Mark the set of screened voxels as the interference confidence voxel set.

[0050] Generate a clustering candidate area for the interference confidence voxel data to obtain magnetic disturbance candidate area data; In one embodiment, for use a spatial clustering method to generate a connected region, use DBSCAN clustering, and set parameters including the distance threshold ; the minimum number of voxels minPts = 15. Use the center coordinates of each interference confidence voxel as the clustering input points; for each cluster in the clustering result, generate the set of voxels it contains; the clustering cluster needs to meet the following geometric constraint conditions to be retained, such as the number of voxels ≥ 30; the included angle between the main axis direction and the spatial XYZ axes is less than 60° (to avoid long and thin noise clusters); the volume of the overall bounding box of the voxels ≥ . The set of qualified clustering clusters is recorded as the magnetic disturbance candidate area data set: .

[0051] Perform morphological analysis on the data of the magnetic disturbance candidate area to obtain the data of the magnetic disturbance area candidate region.

[0052] In one embodiment, for each candidate area , calculate its three-dimensional minimum bounding box and extract the following morphological features, such as length : the longest side of the bounding box; width : the second longest side; thickness : the shortest side; calculate the compactness index ; the discrete rate , where is the number of voxel points, is the voxel point index, is the three-dimensional coordinate vector of the th voxel point, ; ; . Retain the candidate areas that meet the following conditions: .

[0053] Optionally, the interference hierarchical division includes: Calculate the local point density change rate and the coordinate perturbation rate for the point cloud local structure data to obtain the local point density change rate data and the coordinate perturbation rate data; In one embodiment, for each point cloud frame and its subsequent frame , select a fixed spatial area to be analyzed in space (such as a cube with a side length of ); calculate the change rate of the number of points in the same spatial area : , where represents the point cloud density of this area at time , is the time interval (frame difference), is a small constant to prevent division by zero (such as 0.001). For the coordinate perturbation rate calculation, for the same point at positions and in two consecutive frames, define the perturbation rate as , where is the coordinate position of point at time , is the coordinate position of point at time , 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 .

[0054] 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; 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 .

[0055] 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; 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 ; Clustering compactness, which is the standard deviation of the distances from all points in the current cluster to the centroid, denoted as , representing 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 unifies and aggregates the perception vectors of all points to form the interference local perception data set, denoted as .

[0056] Calculate the interference degree based on the interference local perception data to obtain the interference degree data; In one embodiment, a linear weighted model is used to calculate the interference degree value , is the local perturbation intensity weight factor, , is the local perturbation intensity, is the perturbation change weight factor, , is the spatial perturbation change amount, is the within-cluster fluctuation reverse factor, , is the interference variance of the cluster to which it belongs. The system combines the spatial position of each point with its corresponding interference degree value into a binary tuple form and outputs it as the interference degree data set, denoted as .

[0057] Perform interference intensity level division on the point cloud local structure data according to the interference degree data to obtain the interference hierarchical data.

[0058] In one embodiment, according to each point corresponding interference degree value , the system uses the fixed threshold method to divide it into three perturbation levels. The specific division rules are as follows: Table 1: Level division rule table For each point , the system outputs its spatial coordinates together with the corresponding perturbation level code to form the interference hierarchical data set. This set is denoted as , and the structure is defined as consisting of a group of binary tuples, and each element contains the point and its perturbation level , that is . Among them, is an integer identifier, corresponding to the above hierarchical codes 1, 2, or 3.

[0059] Optionally, S3 includes: S31. Construct a perturbation space confidence map based on the interference feature data and the robot pose data to obtain the magnetic interference weight data; In one embodiment, for each frame of the robot pose, the system extracts all valid points in the interference feature tensor set at the current moment within a spherical neighborhood with a radius of 1.0 meter centered at its spatial position to construct a local relevant feature point set. This point set is used to represent the interference situation near the current position of the robot. Based on the local feature point set, the system calculates the average perturbation energy within this neighborhood and generates the perturbation confidence weight corresponding to this pose point based on the variance of this mean value and the structural dispersion. The weight calculation uses a normalized Sigmoid mapping function, which forms by inputting the linearly combined interference mean value and the dispersion variance into the Sigmoid function to output a continuous weight value between 0 and 1. In this function, the adjustment coefficient can be set as a hyperparameter. For example, the interference intensity coefficient is taken as 5.0, and the structural dispersion adjustment coefficient is taken as 2.0. The system encapsulates all pose frames and their corresponding perturbation weight values into a perturbation space confidence map structure. This structure uses the pose frame as a node, and the node attribute is the perturbation confidence weight, forming a set of mapping data collections in the form of (pose point, perturbation weight value).

[0060] S32. Perform pose perturbation matching on the spatial point cloud data and the robot pose data according to the magnetic interference weight data to obtain the pose perturbation matching data; In one embodiment, for each frame moment, the system calculates the Euclidean displacement error between the laser pose and the IMU pose. This error value represents the difference between the two types of pose estimations in space. The calculation of the error value uses the Euclidean distance between two points in three-dimensional space. The system introduces a dynamic matching threshold related to the perturbation weight. Specifically, a dedicated threshold value is calculated for each frame, and this threshold value is obtained by multiplying the maximum allowable matching error by the perturbation weight factor, so as to tighten the tolerance range in the high-confidence interference area and relax the limit in the low-perturbation area. If the pose error of the current frame exceeds this dynamic threshold, the system determines that there is a potential matching distortion in this frame and needs to perform pose correction. The correction strategy is to use the current frame point cloud data as the target and adopt the Iterative Closest Point (ICP) algorithm to perform point cloud alignment registration on the IMU pose, so as to obtain the corrected fused pose estimation. The system encapsulates all the fused matching data at all moments into a matching correction set. Each piece of data contains a triple structure, including the corrected fused pose, the original error value, and the corresponding perturbation weight value.

[0061] S33. Perform high-low perturbation fusion filtering on the pose perturbation matching data to obtain the navigation optimization data.

[0062] In one embodiment, for each frame of pose matching result, the system divides the disturbance level according to its disturbance weight value. The specific rules are as follows: If the weight value is greater than or equal to 0.7, this frame is determined to be a high-disturbance area; if the weight value is between 0.3 and 0.7 (including the lower limit, excluding the upper limit), it is determined to be a medium-disturbance area; if the weight value is less than 0.3, it is determined to be a low-disturbance area. For different disturbance levels, the system configures three types of filtering algorithms: In the high-disturbance area, unscented Kalman filtering is used; in the medium-disturbance area, extended Kalman filtering is selected; in the low-disturbance area, a weighted moving average filtering method is used. The system sets different filtering kernel weight coefficients for each disturbance level to control the influence intensity of each frame within the filtering window: the filtering kernel weight coefficient in the high-disturbance area is set to 1.5; the weight coefficient in the medium-disturbance area is set to 1.0; the weight coefficient in the low-disturbance area is set to 0.5. When performing filtering calculations, the system selects the corresponding filter and kernel function according to the disturbance level of each frame, and performs time-series weighted updates on this frame and several frames before and after it. The system integrates all the pose frames after fusion filtering processing into a continuous navigation trajectory, denoted as the optimized navigation trajectory dataset.

[0063] Optionally, the high-low disturbance fusion filtering includes: Performing disturbance hierarchical matching annotation on the pose disturbance matching data to obtain pose disturbance hierarchical data; In one embodiment, the input data is a pose disturbance matching dataset, which contains information of multiple navigation key frames. Each frame of data includes the following three components: One is the pose after matching and correction, which represents the pose estimation result obtained by fusing lidar point cloud and inertial navigation data; the second is the matching residual between this frame in the inertial navigation estimation and the lidar estimation, which is used to measure the pose correction amplitude; the third is the disturbance weight, which is used to quantify the magnetic disturbance intensity of the current frame environment. This weight is a continuous value between 0 and 1, and the higher the value, the stronger the disturbance. The system divides the disturbance level of each frame according to the disturbance weight value. The specific division rules are as follows: When the disturbance weight is greater than or equal to 0.7, mark this frame as "high disturbance level"; when the disturbance weight is between 0.3 and 0.7, mark it as "medium disturbance level"; when the disturbance weight is less than 0.3, mark it as "low disturbance level". After completion of the layering, the system binds the matching pose of each frame with its corresponding disturbance level label to form structured output data. The obtained annotation result is a set of disturbance level identification data in units of navigation frames, and each element includes a corrected pose record and its corresponding disturbance level label. The disturbance level can take values of "high disturbance", "medium disturbance" or "low disturbance".

[0064] Performing filtering response configuration mapping according to the pose disturbance hierarchical data to obtain filtering response configuration data; In one embodiment, the system first establishes a mapping strategy table between the disturbance levels and the filter parameters. This strategy table maps the three levels of "high disturbance", "medium disturbance" and "low disturbance" to different filter configuration schemes respectively. For the navigation frames at the high disturbance level, the system configures the Unscented Kalman Filter (UKF) with a filter sampling window of 5 frames; for the frames at the medium disturbance level, the system selects the Extended Kalman Filter (EKF) with a sampling window size of 3 frames; for the frames at the low disturbance level, the system configures the Moving Average Filter (SMA) with a sampling window also set to 3 frames. During the actual execution process, the system traverses the pose disturbance hierarchical data item by item. For each data item containing the matching pose and disturbance level label, the system queries the above-mentioned preset strategy table, and according to its corresponding disturbance level label, obtains the corresponding filter type, filter sampling window length and state modeling method. The system combines the frame number, its corresponding filter type, sampling window size and state model description of each frame into a structured configuration output to form a filter response configuration data set.

[0065] Generate the kernel weight adjustment factor according to the filter response configuration data to obtain the disturbance filter kernel data; In one embodiment, the system preset the Gaussian kernel function as the basic weighting model of the filter. This kernel function is used to describe the weighting relationship between the current frame and its previous and subsequent time-series frames, and its value is determined by the time index interval. The farther the frame interval is, the lower the weight. Specifically, the weight of the kernel function decays exponentially with the time difference, and its functional form is that the current weight is equal to the negative value of the exponential function, and the exponential term is determined by the square of the time interval divided by twice the square of the kernel scale of the filter. , is the weight value of the disturbance filter kernel function, is the exponential function, is the time index interval (distance between frames), is the filter kernel scale parameter (temporal standard deviation). The scale parameter of the kernel function, that is, the kernel width, is adjusted according to the disturbance level. The system sets different kernel scale values for different disturbance levels: at the high disturbance level, the kernel scale is set to 1.0; at the medium disturbance level, the kernel scale is set to 0.6; at the low disturbance level, the kernel scale is set to 0.3. The system reads the disturbance level, filter window width and type configuration of each frame in turn. According to its disturbance level, it matches the corresponding kernel scale value, and constructs a symmetric time window centered on the current frame at this scale. Within this time window, the system calculates the kernel function values of the current frame and the frames before and after it to form the complete filter kernel weight sequence of this frame. For example, if the window width is 5 frames, then taking two frames forward and backward with the current frame as the center, a total of five time positions; for each position, the system calculates its time interval from the center frame and generates the corresponding weight through the preset kernel function. The system combines the time index corresponding to each frame with its generated filter kernel weight sequence to form a disturbance filter kernel data structure.

[0066] The pose perturbation matching data is fused and filtered using the perturbed filter kernel data to obtain the navigation optimization data.

[0067] In one embodiment, the system first classifies all navigation frames according to the perturbation levels marked in the configuration data. Specifically, for the navigation frames in the high-perturbation area, the system constructs a nonlinear state update equation based on the previous state estimate. , is the current state vector, is the state transition function (such as a linear matrix multiplication or a nonlinear mapping), is the previous state vector, is the process noise vector, is the current observation vector, is the observation function (for a lidar system, it represents the point cloud measurement model determined by the robot state; for an IMU sensor, it represents the expected values of angular velocity and acceleration; for a vision system, it represents the perspective projection function from world coordinates to image coordinates), is the observation noise vector, and a set of representative Sigma points are selected in the state space. State prediction is achieved through the propagation of these Sigma points in the state transition function; subsequently, 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 frames in the medium-perturbation area, the system performs Jacobian linearization on the nonlinear state model to obtain a linear approximation model; subsequently, state estimation is performed according to the standard prediction-update filter framework, that is, state propagation is carried out through the prediction step, and observation information is fused through the update step to complete state correction. For the navigation frames in the low-perturbation area, the system takes the current frame as the center, selects multiple pose frames after matching and correction within the front and rear time windows, and weights and averages the pose vectors of these frames using the aforementioned Gaussian kernel function weights, thereby generating the optimized pose of this frame, that is , is the current optimized pose vector, is the relative time offset, is the half-width of the sliding window, indicating the width of the consideration range of the front and rear frames in the sliding weighted average. For example = 3 means selecting the current frame and the three 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, since the number of adjacent frames is insufficient, the system adopts an edge processing mechanism. One way is to use the mirror filling strategy, that is, mirror-completing the missing frame data within the window; another way is to keep the previous optimization result unchanged. The system outputs the fused filter results of all navigation frames in chronological order to form a complete navigation optimization trajectory sequence.

[0068] Optionally, S4 includes: S41. Perform cross - space - time scale residual field processing on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; In one embodiment, for each frame moment, the system extracts the optimized pose and the reference pose respectively, and calculates the difference between the two. The residual vector is defined as the optimized pose minus the reference pose, which includes two parts: the position residual, which measures the position difference by Euclidean distance and is expressed as a three - dimensional coordinate offset; and the direction residual, which can calculate the attitude angle change through quaternion difference or Euler angle difference and is used to describe the degree of orientation offset. The system aggregates the residuals within a local time period in a sliding time window manner (for example, every 5 frames) 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 certain time index , the system constructs a residual function to describe the pose difference intensity between the optimized trajectory and the reference trajectory at this spatial point and this time point. The output of this residual function is the residual norm. The system output contains two result structures: one is a multi - dimensional residual tensor, which is expressed 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, which uses the exponentially weighted moving average method to smooth the time - domain residual curve to highlight the dynamic trend of the system error change.

[0069] S42. Classify the residual trend data to obtain residual classification data; In one embodiment, for each spatio - temporal position point in the residual trend data, the system extracts the following statistical feature indicators, including the average residual value, which calculates the average residual amplitude at this position within the current time window; the residual standard deviation, which is used to measure the fluctuation amplitude of the residuals at this position within the time window; and the residual growth slope, which estimates the trend rate of the residuals changing with time by taking the difference between the current average residual value and the average residual value of the previous time window and dividing by the time interval.

[0070] Table 2: Residual type determination rules are as follows Among them, the residual growth slope being approximately zero can be defined as having no trend change within a certain tolerance range (for example, the absolute value is less than 0.005 meters per second). The system marks and outputs the residual type of each spatio - temporal position point to generate a residual classification data structure. This structure uses the time - series index as the primary key to record the residual type at the corresponding moment.

[0071] S43. Generate residual regulation parameters according to the residual classification data and the preset compensation parameter library to obtain residual regulation parameter data; In one embodiment, a preset compensation parameter library is provided. The parameter library establishes a mapping relationship according to the residual type and includes corresponding filtering strategies and their control parameters. Each residual type is associated with the following three parameters, including the filter type, which is used for the selected control method in the subsequent compensation process; the gain factor, which is used to adjust the residual feedback response intensity; the calibration step size, which controls the iterative amplitude of the pose correction or compensation; and the threshold extension parameter, which is used for the adaptive adjustment of the tolerance range in the compensation strategy. The structure of the preset parameter library includes the following mapping relationship, as shown in Table 3, Table 3: Preset Parameter Library Mapping Table For each residual classification result, the system searches for a matching item in the parameter library according to its corresponding type and extracts the associated compensation parameters. The index of this frame is bound to the regulation parameters to form a regulation parameter vector, and the vector content includes the filter type, the gain factor, the calibration step size, and the threshold extension factor. The system encapsulates the set of all mapped regulation parameters as a residual regulation parameter data set. Each record is composed of a time frame index and its corresponding parameter vector, indicating the adjustment strategy of this frame in the residual control.

[0072] S44. Optimize the spatial point cloud data and the robot pose data according to the residual regulation parameter data to obtain the spatially optimized point cloud data and the robot optimized pose data.

[0073] In one embodiment, the system selects a matching pose optimization method according to the residual classification result and its corresponding regulation parameter, and adjusts the optimized pose at each moment. The specific strategy is as follows. For the steady-state residual frame, the sliding window average method is used to calculate the average pose of the current position within a certain range before and after it. For the cumulative residual frame, the frame-by-frame fine-tuning strategy is executed. The system linearly corrects the current residual vector by using the calibration step factor, and subtracts the weighted share of the residual amount from the original optimized pose. For the surge residual frame, the Kalman filtering method is used to fuse the state prediction and measurement update of the current pose. For the offset residual frame, a registration correction strategy at the key frame level is introduced. The system selects the historical key frames with high confidence as the relocalization reference, and globally reconstructs the current frame pose through image reprojection or geometric feature matching. The above process will generate a new optimized pose for each pose frame with residual regulation requirements, constituting the robot optimized pose dataset. If the system detects that a certain frame pose has been updated and geometric detection is required, the pose transformation process is synchronously performed on the point cloud data corresponding to that frame. Specifically, the system calculates the global coordinate reprojection of the point cloud data according to the transformation matrix between the original pose and the updated pose of this frame, maps the original point cloud data to the updated spatial position, and forms an optimized point cloud frame after spatial alignment. The system outputs two types of optimization results: one is the spatially optimized point cloud dataset, which contains all the point cloud frames corrected by pose transformation; the other is the optimized robot pose dataset, which contains the updated pose results at each moment.

[0074] Optionally, the present application also provides a precise inspection robot for underground caverns, which is used to execute the precise inspection method for underground caverns as described above. The precise inspection robot for underground caverns includes: A multi-source perception acquisition and magnetic field modeling module, which is used to acquire spatial point cloud data, robot pose data, and scene magnetic field data, and construct a magnetic field model according to the scene magnetic field data to obtain a scene magnetic field model; A magnetic disturbance feature extraction module, which is used to extract interference features according to the scene magnetic field model and spatial point cloud data to obtain interference feature data; A magnetic disturbance weight-driven fusion filtering module, which is used to calculate magnetic disturbance weights according to the interference feature data and robot pose data to obtain magnetic disturbance weight data; and perform fusion filtering on the spatial point cloud data and robot pose data according to the magnetic disturbance weight data to obtain navigation optimization data; A residual trend analysis and compensation module, which is used to perform residual analysis on the spatial point cloud data and robot pose data according to the navigation optimization data to obtain residual trend data; and perform dynamic compensation according to the residual trend data to obtain spatially optimized point cloud data and robot optimized pose data.

[0075] Therefore, from any perspective, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended application documents rather than the above description. Therefore, all changes falling within the meaning and scope of the equivalent elements of the application documents are intended to be encompassed within the present invention.

[0076] The above are only specific embodiments of the present invention, enabling those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be obvious to those skilled in the art. The general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown herein, but rather to the broadest scope consistent with the principles and novel features invented herein.

Claims

1. A precise inspection method for underground chambers, characterized in that, The method includes: S1. Obtain spatial point cloud data, robot pose 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 feature data according to the scene magnetic field model and the spatial point cloud data; S3. Calculate the magnetic interference weight according to the interference feature data and the robot pose data to obtain magnetic interference weight data; perform fusion filtering on the spatial point cloud data and the robot pose data according to the magnetic interference weight data to obtain navigation optimization data; S4. Perform residual analysis on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; perform dynamic compensation according to the residual trend data to obtain spatially optimized point cloud data and robot optimized pose data.

2. The method according to claim 1, characterized in that, S1 includes: Obtain spatial point cloud data, robot pose data, and scene magnetic field data; Perform spatial mapping of magnetic field sampling points according to the scene magnetic field data to obtain magnetic induction sampling space map data; Perform magnetic potential function interpolation on the magnetic induction sampling space map data to obtain magnetic potential space data; Perform distortion modeling on the magnetic potential space data to obtain a scene magnetic field model.

3. The method according to claim 2, characterized in that, The distortion modeling includes: Calculate the magnetic potential gradient of the magnetic potential space data to obtain magnetic potential gradient data; Calculate the high-order perturbation metric of the magnetic potential gradient data to obtain high-order perturbation metric data; Perform local detection on the high-order perturbation metric data to obtain local anomaly data; Perform distortion aggregation analysis according to the local anomaly data to obtain distortion bulk data; Perform magnetic field model encapsulation according to the distortion block data to obtain a scene magnetic field model.

4. The method according to claim 1, wherein S2 It includes: Extract candidate regions of magnetic interference areas according to the scene magnetic field model and the spatial point cloud data to obtain candidate region data of magnetic interference areas; Perform local point cloud structure analysis on the candidate region data of magnetic interference areas to obtain local point cloud structure data; Perform interference layer division on the local point cloud structure data to obtain interference layer data; Generate interference feature tensors according to the interference layer data to obtain interference feature data.

5. The method according to claim 4, wherein The extraction of candidate regions of magnetic interference areas includes: Screen interference confidence voxels according to the scene magnetic field model and the spatial point cloud data to obtain interference confidence voxel data; Generate candidate regions for clustering from the interference confidence voxel data to obtain candidate data of magnetic interference areas; Perform morphological analysis on the candidate data of magnetic interference areas to obtain candidate region data of magnetic interference areas.

6. The method according to claim 4, characterized in that The interference layer division includes: Calculate the local point density change rate and the coordinate perturbation rate of the local point cloud structure data to obtain local point density change rate data and coordinate perturbation rate data; Perform clustering processing according to the local point density change rate data and the coordinate perturbation rate data to obtain local point cloud clustering data; Perform local interference perception processing on the local point cloud structure data according to the local point cloud clustering data to obtain local interference perception data; Calculate the interference degree according to the local interference perception data to obtain interference degree data; Perform interference intensity level division on the local point cloud structure data according to the interference degree data to obtain interference layer data.

7. The method according to claim 1, wherein S3 It includes: Construct a perturbation space confidence map according to the interference feature data and the robot pose data to obtain magnetic interference weight data; Perform pose perturbation matching on the spatial point cloud data and the robot pose data according to the magnetic disturbance weight data to obtain pose perturbation matching data; Perform high-low disturbance fusion filtering on the pose perturbation matching data to obtain navigation optimization data.

8. The method according to claim 7, wherein The high-low disturbance fusion filtering includes: Perform perturbation hierarchical matching annotation on the pose perturbation matching data to obtain pose perturbation hierarchical data; Perform filtering response configuration mapping according to the pose perturbation hierarchical data to obtain filtering response configuration data; Generate a kernel weight regulation factor according to the filtering response configuration data to obtain disturbance filtering kernel data; Perform fusion filtering on the pose perturbation matching data by using the disturbance filtering kernel data to obtain navigation optimization data.

9. The method according to claim 1, characterized in that S4 includes: Perform cross-space-time scale residual field processing on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; Classify the residual trend data to obtain residual classification data; Generate residual regulation parameter data according to the residual classification data and a preset compensation parameter library; Optimize the spatial point cloud data and the robot pose data according to the residual regulation parameter data to obtain spatially optimized point cloud data and robot optimized pose data.

10. A precise inspection robot for underground chambers, characterized in that, For implementing the precise inspection method of the underground cavern as described in claim 1, the precise inspection robot for the underground cavern includes: A multi-source perception acquisition and magnetic field modeling module, configured to acquire spatial point cloud data, robot pose data, and scene magnetic field data, and construct a magnetic field model according to the scene magnetic field data to obtain a scene magnetic field model; A magnetic disturbance feature extraction module, configured to extract interference feature data according to the scene magnetic field model and the spatial point cloud data; A magnetic disturbance weight-driven fusion filtering module, configured to calculate magnetic disturbance weight data according to the interference feature data and the robot pose data; perform fusion filtering on the spatial point cloud data and the robot pose data according to the magnetic disturbance weight data to obtain navigation optimization data; A residual trend analysis and compensation module, configured to perform residual analysis on the spatial point cloud data and the robot pose data according to the navigation optimization data to obtain residual trend data; perform dynamic compensation according to the residual trend data to obtain spatially optimized point cloud data and robot optimized pose data.

Images