Mapping and inspection positioning method and system based on laser radar inertial odometer

Abstract

The application discloses a kind of based on laser radar inertial odometry's mapping and inspection positioning method and system, wherein the method comprises: based on the current point cloud data is carried out distortion correction to inertial data, obtain the point set after distortion and predicted global point set;Adaptive selection target voxel level and target neighborhood scale of predicted global point, obtain the neighborhood point set of each predicted global point, and construct point-to-plane residual vector based on neighborhood point set;Based on predicted pose, covariance and residual vector, error state iteration update is carried out by iterative extended Kalman filter and obtains posterior state;Local map update is carried out based on posterior state;Based on key frame pose, obtain modified transformation matrix, and obtain modified release pose based on modified transformation matrix.The application can realize the adaptive adjustment of voxel and neighborhood, effectively prevent data pollution, realize the smoothing and global consistency of closed loop trajectory under the constraint of not re-optimization state and not re-mapping map.

Description

Technical Field

[0001] This invention relates to the field of simultaneous positioning and mapping technology, specifically to a mapping and inspection positioning method and system based on lidar inertial odometry. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM), a key component of technologies such as autonomous driving, augmented reality (AR), and virtual reality (VR), fundamentally relies on onboard sensors to continuously estimate the six degrees of freedom of the vehicle and construct an environmental representation that can be directly consumed for planning and obstacle avoidance. A typical SLAM architecture consists of a real-time front-end and a back-end optimization: the front-end handles data spatiotemporal alignment, constraint construction, and incremental map maintenance, while the back-end performs consistency constraints and drift suppression over a longer temporal domain. Among various branches, compared to lighting solutions that are susceptible to changes in illumination and severe motion blur, LiDAR provides more robust, dense, and accurate depth measurement information. Laser Inertial Odometry (LIO) tightly couples the high-frequency motion priors of the Inertial Measurement Unit (IMU) with the geometric constraints of the point cloud, balancing trajectory continuity and measurement alignment accuracy during dynamic maneuvers while consuming limited computational costs, making it a primary solution.

[0003] Within this framework, recent research has focused on the coordinated optimization of front-end registration strategies, state estimator morphology, and map organization and retrieval structures. On the one hand, the reliance on manual feature extraction is gradually weakened, with more direct or semi-dense geometric registration being adopted to reduce external hyperparameters and rule design, thereby improving adaptability to scanning modes and multiple scenarios. On the other hand, low-latency estimators such as Iterated Extended Kalman Filter (IEKF) are replacing heavy-duty batch map optimization, shortening the closed-loop cycle and improving online response. Furthermore, to support high-frequency registration and online mapping, data structures that are easier to incrementally maintain, such as voxel grids and hash voxels, are used to replace traditional tree indexes, significantly improving the throughput of nearest neighbor queries through constant-level bucket operations and good cache locality. For mixed scenarios with alternating near and far scales, recent work explores hierarchical and multi-scale organization, combining parallel K-Nearest Neighbor (KNN) and neighborhood pruning to maintain average query overhead within an approximately constant range while ensuring retrieval accuracy.

[0004] As a representative work in this field, the Fast-LIO series, while Faster-LIO, based on FAST-LIO2, introduces sparse incremental voxels and adopts parallel nearest neighbor retrieval and matching to further improve efficiency and achieve controllable costs without relying on hardware acceleration, still has key shortcomings in the practical deployment of lidar odometry using the IEKF framework: First, moving objects are prone to entering the corresponding construction and mapping under short-term repeated observations, contaminating the plane fitting and causing drift. Existing methods mostly rely on image domain semantic detection or multi-sensor priors to filter out dynamic components, which not only introduces additional costs and cross-modal coupling, but also affects the stability under pure laser configuration; voxel-level temporal consistency statistics are often not directly included in neighborhood selection and gating, resulting in a lack of pre-suppression of dynamic contamination before residual construction. Second, the single voxel resolution superimposed with a fixed neighborhood size is difficult to simultaneously take into account the mixed conditions of long-distance sparseness and short-distance density, resulting in jitter in normal estimation and decreased recognition of long-distance constraints. Third, long-distance predictions have large drift due to accumulated errors. Summary of the Invention

[0005] This invention addresses the problems in the prior art by disclosing a mapping and inspection positioning method and system based on lidar inertial odometry, thereby solving at least one of the technical problems in the related technologies.

[0006] This invention is achieved through the following technical solution:

[0007] In a first aspect, the present invention provides a mapping and inspection positioning method based on lidar inertial odometry, comprising the following steps:

[0008] Acquire the current frame point cloud data of the LiDAR and the IMU data, perform state propagation processing on the IMU data to obtain the predicted pose and covariance, and perform distortion correction processing on the current frame point cloud data of the LiDAR based on the predicted pose to obtain the distortion-corrected point set and obtain the predicted global point set based on the distortion-corrected point set.

[0009] The target voxel level and target neighborhood scale of each predicted global point in the predicted global point set are adaptively selected by the pyramid-type adaptive voxel and neighborhood strategy PAVEN. Based on the target voxel level and target neighborhood scale, the neighborhood point set of each predicted global point in the predicted global point set is obtained. Based on the neighborhood point set of the predicted global point set, a residual vector from point to plane is constructed. The neighborhood point set is obtained by combining the time voxel stability gated TVSG.

[0010] Based on the predicted pose, the covariance, and the residual vector, the error state is iteratively updated using the Iterative Extended Kalman Filter (IEKF) algorithm to obtain the updated state. The residual vector is then linearized based on the updated state to obtain the posterior state.

[0011] Based on the posterior state and the distortion-free point set, a local map update is performed to obtain the updated local map.

[0012] In the release branch of the lidar inertial odometry, a corrected transformation matrix is ​​obtained based on the keyframe pose, and a corrected release pose is obtained based on the corrected transformation matrix and the odometry pose.

[0013] Secondly, the present invention also provides a mapping and inspection positioning system based on lidar inertial odometry, comprising:

[0014] The data processing module is used to acquire the current frame point cloud data of the LiDAR and the IMU data, perform state propagation processing on the IMU data to obtain the predicted pose and covariance, and perform distortion correction processing on the current frame point cloud data of the LiDAR based on the predicted pose to obtain the distortion-corrected point set and obtain the predicted global point set based on the distortion-corrected point set.

[0015] The residual construction module is used to adaptively select the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set through the pyramid-type adaptive voxel and neighborhood strategy PAVEN, and obtain the neighborhood point set of each predicted global point in the predicted global point set based on the target voxel level and target neighborhood scale, and construct the point-to-plane residual vector based on the neighborhood point set of the predicted global point set, wherein the neighborhood point set is obtained by combining the time voxel stability gated TVSG;

[0016] The error update module is used to iteratively update the error state based on the predicted pose, the covariance, and the residual vector using the iterative extended Kalman filter (IEKF) algorithm to obtain the updated state, and to linearize the residual vector based on the updated state to obtain the posterior state.

[0017] The map update module is used to perform local map updates based on the posterior state and the distortion-free point set to obtain an updated local map.

[0018] The pose publishing module is used to obtain a corrected transformation matrix based on the keyframe pose in the publishing branch of the lidar inertial odometry, and to obtain a corrected publishing pose based on the corrected transformation matrix and the odometry pose.

[0019] The features and beneficial effects of this invention are as follows:

[0020] This scheme adaptively selects the target voxel level and target neighborhood scale for each predicted global point in the predicted global point set, simultaneously considering normal stability and constraint discernibility. Based on the corrected transformation matrix obtained from the keyframe pose, this scheme corrects the odometry pose to obtain the corrected published pose, reducing drift accumulation and improving long-term trajectory consistency. Furthermore, this scheme uses temporal voxel stability gating to eliminate dynamic points, avoiding interference and solidifying the map structure at low cost. Attached Figure Description

[0021] To more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0022] Figure 1 This is a flowchart illustrating the mapping and inspection positioning method based on lidar inertial odometry as described in an embodiment of the present invention.

[0023] Figure 2 This is a structural block diagram of the mapping and inspection positioning system based on lidar inertial odometry according to an embodiment of the present invention;

[0024] Figure 3 This is a visualization of an embodiment of the present invention;

[0025] Figure 4 This is a visualization of the comparative example 1 of the present invention;

[0026] Figure 5 This is a comparison chart showing the single-frame processing time of this scheme and several other schemes.

[0027] Figure 6 This is a schematic diagram illustrating how the number of maintenance map points changes over time, including this scheme and several other schemes. Detailed Implementation

[0028] To facilitate understanding of the present invention, a more comprehensive description of the present invention will be given below, and embodiments of the present invention will be provided, but this does not limit the scope of the present invention.

[0029] Glossary:

[0030] LiDAR: LiDAR (Light Detection and Ranging);

[0031] LIO: LiDAR inertial odometry;

[0032] IEKF: Iterative Extended Kalman Filter;

[0033] IMU: Inertial Measurement Unit;

[0034] DAC-LIO: This is the perceptual voxel-based laser odometry proposed in this application, a dynamic perceptual voxel-based LIO technology with stable counting guidance and lightweight loop smoothing. It explicitly maintains multi-scale voxel layers through a pyramidal adaptive voxel and neighborhood strategy (PAVeN), introducing a hierarchical perception strategy in residual construction to achieve adaptive perceptual adjustment of voxels and their neighborhoods. PAVEN uses hierarchical selection, cross-layer backoff, and hierarchical perception weighting to achieve adaptive adjustment of voxel and neighborhood sizes, balancing long-range normal stability and near-field computational efficiency. Temporal voxel stability gate (TVSG) maintains temporal statistics for each voxel at the map level and applies them simultaneously to neighbor retrieval and map writing. The core idea of ​​TVSG is to maintain a stability counter for each voxel in the map to encode the voxel's occupancy persistence in recent frames, recording the frequency and persistence of each voxel observed in the time series, thereby reflecting the stability of the environmental structure.

[0035] LLS: The lightweight loop smoothing module proposed in this application does not backfeed the internal state of the iteratively extended Kalman filter. It runs on the release branch and only refines the output trajectory without modifying the internal state or map of the IEKF. It performs intra-segment pose interpolation on the key frame sequence and applies an external global correction matrix to the release pose to obtain a smoother trajectory performance at the closed loop.

[0036] LRU stands for Least Recently Used, and it is a commonly used page replacement algorithm.

[0037] LiDAR inertial odometry (LIO) is a core technology for mobile robot localization and mapping, aiming to obtain stable, low-drift odometry estimates. However, most current work is rapidly evolving along three main lines: from sparse features to direct / semi-dense constraints; from batch graph optimization to tightly coupled filtering; and from single-scale to multi-scale organization. In real-world scenarios, a single voxel and a fixed neighborhood cannot simultaneously ensure normal stability and constraint discriminability, while interference from moving objects can contaminate plane fitting, leading to increased drift. To address this, we propose a mapping and inspection-based localization method based on LiDAR inertial odometry. We use temporal voxel stability-gated TVSG to maintain temporal statistics for each voxel at the map level, simultaneously applying this to nearest neighbor retrieval and map writing. This achieves pre-suppression of moving voxels before residual construction at a controllable cost, directly improving geometric constraint discriminability and estimation consistency. Temporal voxel stability-gated TVSG provides stable counting and observation statistics for voxel maintenance time, used bidirectionally to avoid interference being fixed into the map structure at low cost. Unlike conventional iVox and iVox-PHC's hierarchical storage within a single global voxel resolution, this application proposes a pyramid-style adaptive voxel and neighborhood strategy, PAVEN. PAVEN explicitly maintains multi-scale voxel layers and introduces hierarchical-aware weights and robust thresholds in the observation modeling stage. It adaptively selects the retrieval level and neighborhood size based on measurement distance and local point density, achieving adaptive sensing adjustment of voxels and neighborhoods. This effectively alleviates the condition number degradation in long-range normal estimation and the noise bias caused by excessive near-field density, stabilizing the average query complexity within an approximately constant range. Simultaneously, this application also designs a lightweight loop closure smoothing (LLS) to perform intra-segment pose interpolation on keyframe sequences and applies an external global correction matrix to the published pose, obtaining smoother trajectory performance at loop closures.

[0038] Figure 1 This is a flowchart illustrating a mapping and inspection positioning method based on a lidar inertial odometry, provided as an embodiment of this application. Figure 1 As shown, the mapping and inspection positioning method based on lidar inertial odometry includes the following steps:

[0039] Step 101: Acquire the current frame point cloud data of the LiDAR and the IMU data, perform state propagation processing on the IMU data to obtain the predicted pose and covariance, and perform distortion correction processing on the current frame point cloud data of the LiDAR based on the predicted pose to obtain the distortion-corrected point set and obtain the predicted global point set based on the distortion-corrected point set.

[0040] In some embodiments, the above-described state propagation processing of IMU data to obtain the predicted pose and covariance, and the distortion correction processing of the LiDAR current frame point cloud data based on the predicted pose to obtain the distortion-corrected point set, and the prediction global point set based on the distortion-corrected point set, includes: performing state propagation processing on IMU data after pre-integration processing to obtain the predicted pose. With covariance The current frame point cloud data of LiDAR is registered with the map without feature extraction to obtain the registered current frame point cloud data. Based on the predicted pose, the registered current frame point cloud data is subjected to distortion correction to obtain the distortion correction point set, and the predicted global point set is obtained based on the distortion correction point set.

[0041] As an example, for each frame of incoming LiDAR point cloud, high-frequency IMU data The propagation is performed forward to obtain the predicted pose and covariance. Based on this, distortion correction is performed on the LiDAR point cloud using the formula. Compensate for motion distortion during the scanning process to obtain the predicted global points. (i.e., the point set after distortion removal); This process can be represented as follows:

[0042] (1)

[0043] (2)

[0044] in, This represents the predicted global point set of the point cloud in the global coordinate system G for the k-th frame. This indicates state propagation based on IMU data. This indicates the LiDAR distortion correction operation. This means mapping the state to the corresponding global rigid body transformation. This represents the IMU data sequence collected within the time interval [k-1, k]. This represents the coordinates of the j-th original point within the k-th frame LiDAR point cloud in the LiDAR coordinate system L. This represents the coordinates of the j-th point in the k-th frame of the LiDAR point cloud after distortion correction in the LiDAR coordinate system L. This represents the predicted pose (also known as the predicted state, or simply the predicted pose) at time k. This represents the posterior state estimate at time k-1. The covariance matrix (or simply covariance) represents the prediction error at time k.

[0045] The IMU data is pre-integrated before state propagation; the raw LiDAR point cloud is directly registered with the map without feature extraction; for each incoming LiDAR point cloud scan, motion distortion correction is performed using the propagation results from the IMU; the state propagation of the IMU in this application is essentially based on the known state of the device at the current moment, combined with sensor data to infer the state at the next moment. Although the IMU has a high sampling frequency and can output acceleration and angular velocity in real time, when directly integrating the raw data frame by frame to infer the state, if the historical state is adjusted in subsequent optimizations, all relevant raw data need to be re-integrated, resulting in a huge computational load. Pre-integration first integrates a large number of IMU measurements between two keyframes into three composite quantities: rotation increment, velocity increment, and position increment, and then uses these composite quantities to complete the state propagation. This avoids repeated integration and can adapt to the rhythm of low-frequency sensors such as vision, significantly improving system efficiency.

[0046] Step 102: The target voxel level and target neighborhood scale of each predicted global point in the predicted global point set are adaptively selected by the pyramid-type adaptive voxel and neighborhood strategy PAVEN. Based on the target voxel level and target neighborhood scale, the neighborhood point set of each predicted global point in the predicted global point set is obtained. Based on the neighborhood point set of the predicted global point set, the residual vector from point to plane is constructed. The neighborhood point set is obtained by combining the time voxel stability gated TVSG.

[0047] In some embodiments, the above-mentioned adaptive selection of the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set through the pyramid-type adaptive voxel and neighborhood strategy PAVEN, and the obtaining of the neighborhood point set of each predicted global point in the predicted global point set based on the target voxel level and target neighborhood scale, includes: adaptively selecting the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set through the pyramid-type adaptive voxel and neighborhood strategy PAVEN; obtaining the neighborhood point set of each predicted global point in the predicted global point set based on the target voxel level and target neighborhood scale; wherein, PAVEN is used to explicitly maintain a multi-scale voxel pyramid, simultaneously selecting the voxel level and neighborhood scale for each predicted global point, so as to achieve a balance between the stability of the long-distance normal vector and the computational efficiency of the short-distance calculation.

[0048] In some embodiments, the above-mentioned PAVEN adaptive selection of the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set; and the obtaining of the neighborhood point set of each predicted global point in the predicted global point set based on the target voxel level and target neighborhood scale, including:

[0049] For each predicted global point in the predicted global point set, based on the prior distance, a target voxel level is selected for the predicted global point from the multi-scale voxel pyramid; the number of map points in the neighborhood voxels of different neighborhood scales under the target voxel level is obtained, and it is determined whether there are neighborhood voxels of the target neighborhood scale with the number of map points that meet the first neighborhood point requirement.

[0050] If there is no neighborhood voxel sphere at the target neighborhood scale that meets the first neighborhood point requirement, return to the voxel level selection step, update the next level of the target voxel level to the target voxel level of the predicted global point, and continue execution.

[0051] If there exists a target neighborhood voxel sphere with a map point count that meets the first neighborhood point count requirement, obtain the first neighborhood point set corresponding to the target neighborhood voxel sphere. Obtain the stability count of each point in the first neighborhood point set at the target voxel level through Time Voxel Stability Gating (TVSG). Filter out points in the first neighborhood point set whose stability count does not meet the first stability threshold to obtain the filtered first neighborhood point set. TVSG is used to continuously update two attribute values ​​for each voxel in the map. The two attribute values ​​include the frequency of the voxel being observed in the time series and whether the voxel is observed in the current frame. The stability count of each voxel is obtained based on the two attribute values.

[0052] Determine whether the number of points in the first neighboring point set after filtering meets the first neighboring point count threshold. If not, return to the voxel level selection step, update the next level of the target voxel level to the target voxel level of the predicted global point, and continue execution until the number of points in the first neighboring point set after filtering meets the first neighboring point count threshold, thus obtaining the neighboring point set of the predicted global point, and thus obtaining the neighboring point set of each predicted global point in the predicted global point set.

[0053] This embodiment explicitly maintains a multi-scale voxel pyramid using PAVEN, with its levels... Represented as All levels The corresponding voxel size is represented as , ,in, This indicates the voxel size of the finest layer. This indicates the scale ratio for the corresponding level. This is the multiplier factor. >1; Each level Both are hash-based voxel maps. . This represents the set of predicted global points in the global coordinate system G for the point cloud in frame k; each predicted global point in frame k... The voxel containing the layer is denoted as , This is the upper limit of the level. The default value is 3.

[0054] In some embodiments, TVSG uses a frame-level occupancy indicator. This indicates whether the voxel is observed in the current frame; TVSG is used in two phases: neighborhood selection and map update. In the neighborhood selection phase, it predicts the voxel containing the global point set. The value is set to 1, and the remaining voxels are... The value is set to 0; during the map update phase, the voxel containing the posterior global point set is... The value is set to 1, and the remaining voxels are... The value is set to 0; as an example, during the map update phase, It is expressed as follows:

[0055] (3)

[0056] in, For indicator functions, Represents voxels in a map. , The set of voxels representing map voxels. This represents the coordinates of a 3D point in the global coordinate system G. This represents the posterior global point set of the point cloud in the global coordinate system G for the k-th frame. Point Falling on voxels middle.

[0057] TVSG records the frequency, or number of times, voxels are observed over time using a stability counter. and to Normalization is performed to obtain the stability count; It is expressed as follows:

[0058] (4)

[0059] in, Voxel representation In the The number of times a frame is recorded by a stability counter. Voxel representation In the The number of times a frame is recorded by a stability counter. and These are the control of the count growth and decay rates, respectively. This is the upper limit saturation value;

[0060] To facilitate cross-sequence use, TVSG will Normalization The time stability index (i.e., stability count) of voxels is obtained from the interval. Stability count It is expressed as follows:

[0061] (5)

[0062] To rapidly enhance stability during continuous observations and facilitate rapid forgetting when voxels disappear, TVSG employs the aforementioned mixed saturation counting strategy of increasing and decreasing values. .

[0063] In other words, TVSG is for each map voxel ( The set of voxels representing map voxels adds two attributes: stability counters. ( (representing the set of natural numbers) and frame-level occupancy indicator Stability counter This is used to record the "number of times" a voxel is observed over time, with an upper limit equal to the saturation value. Frame-level occupancy indicator These are binary variables used to indicate whether the voxel is observed in the current frame; both quantities are updated for each frame of input LiDAR data.

[0064] This embodiment applies PAVEN to the distortion-corrected point set, adaptively selecting the target voxel level and target neighborhood scale for each predicted global point; TVSG provides stability gating to filter out map points labeled as dynamic, obtaining the neighborhood point set of each point in the predicted global point set; and plane fitting is performed on each neighborhood point set. ,in, It is a plane normal vector. The data is represented by a point, and all residuals in the entire frame are combined into a residual vector (also called the observation vector).

[0065] As an example, based on distance priors, an initial target voxel level is assigned to each predicted global point in the predicted global point set. The upcoming As the current target voxel level ; It is expressed as follows:

[0066] (6)

[0067] in, Used to represent prior scale information (such as the divergence angle of a laser beam). >0.

[0068] After determining the initial target voxel level, neighborhood selection is performed at that initial target voxel level. In this embodiment, multiple neighborhood voxel spheres of different neighborhood scales are set in the voxel space of the multi-scale voxel pyramid for each voxel level. ( express - Ring voxels (K includes multiple values, defaulting to 3), determine whether there exists a target neighborhood voxel at the current target voxel level among multiple neighborhood voxels of different neighborhood scales with a map point count that meets the first neighborhood point count requirement; if the map point count meets the first neighborhood point count requirement, then the map point count in the neighborhood voxel corresponding to that target neighborhood scale is greater than or equal to... And try to get as close as possible to the target's neighborhood points. The current target voxel level can be determined using the following formula. The target neighborhood scale below :

[0069] (7)

[0070] (8)

[0071] (9)

[0072] in, Indicates the first layer voxels The set of map points stored in the middle, Indicates the number of neighborhood points of the target. This represents the minimum number of valid neighborhood points. Represents each predicted global point In the The voxel containing the layer is in the first The set of neighborhood points within the corresponding neighborhood voxel sphere at each domain scale; express The number of points.

[0073] If there is no neighborhood voxel sphere that meets the first neighborhood point requirement at the target neighborhood scale, then directly backtrack to the target voxel level selection step, update the next level of the current target voxel level to the target voxel level of the predicted global point, and continue execution.

[0074] If there exists a neighborhood voxel sphere at the target neighborhood scale that meets the first neighborhood point requirement, the current target voxel level and its target neighborhood scale are determined. The neighborhood point set of each predicted global point is obtained, and then the Temporal Voxel Stability Gated (TVSG) filter is used to remove neighborhood point sets whose stability counts do not meet the first stability threshold. The points are then used to obtain the filtered neighborhood point set (i.e., the noise-removed neighborhood point set); then, at the current target voxel level, the filtered neighborhood point set obtained after time-voxel stability gated TVSG filtering is determined. Is the number of map points greater than or equal to (Threshold for the number of points in the first neighborhood), if it cannot be satisfied In this case, the process returns to the voxel level selection step, updating the next level of the current target voxel level to the new target voxel level of the predicted global point (i.e., gradually retreating the target voxel level towards a coarser voxel pyramid level) and continues until the condition is met. This process determines the final target voxel level and the target neighborhood scale at each level, thereby determining the final target voxel level, target neighborhood scale, and corresponding neighborhood point set for each predicted global point. As an example, the set of voxel levels ultimately used in the hierarchical backoff process for predicting global points is represented as follows: .

[0075] In this embodiment, during the neighborhood selection stage, TVSG is used to remove map points deemed unstable based on voxel stability, preventing them from participating in feature correspondence. A local plane is then fitted onto the filtered set of surviving points, and the plane normal vector is extracted. With representative point To construct point-to-plane residuals.

[0076] In some embodiments, the above-mentioned construction of a point-to-plane residual vector based on the neighborhood point set of the predicted global point set includes: performing plane fitting on the neighborhood point set of each point in the predicted global point set to obtain the plane normal vector and representative point of each fitted plane; constructing multiple point-to-plane residuals based on the plane normal vector and representative point of each fitted plane, and fusing the multiple residuals to obtain a residual vector.

[0077] In some embodiments, the above-mentioned fusion of multiple residuals to obtain a residual vector includes: obtaining the level weights corresponding to the level where each residual is located, weighting the multiple residuals based on the level weights to obtain aggregated residuals; obtaining the stability weights assigned to each residual by the time voxel stability gate (TVSG); and obtaining the residual vector based on the stability weights and the aggregated residuals.

[0078] As an example, for consistent map points, the observation model can be expressed as the residual from the point to the plane, and the observation model is as follows:

[0079] (10)

[0080] in, This represents the coordinates of the j-th point in the k-th frame of the LiDAR point cloud after distortion correction in the LiDAR coordinate system L. This represents the attitude estimated by the sensor at that moment. Representing the plane normal vector and representing the point, respectively. This represents the rotation matrix of the IMU coordinate system relative to the world coordinate system at that moment. It is the residual at the j-th point, which serves as the state. The function.

[0081] When constructing the residual vector, given an arbitrary neighborhood The centroid of this neighborhood is defined as The covariance matrix is: ,right Perform eigenvalue decomposition to obtain eigenvalues , , At each voxel level Upper plane statistics are defined as follows:

[0082] (11)

[0083] in, , Represents a three-dimensional feature vector space; This represents the coordinate vector of a point within the neighborhood. Represents the eigenvector. , , To The eigenvalues ​​of the eigenvalues ​​decomposed from the eigenvalues. For flatness threshold, ; express The normal vector of the plane, express The centroid of a plane; express Flatness of a plane express Flatness weight of a plane; The stability coefficient is a positive constant much smaller than 1, and can be taken as... This is used to prevent the denominator from being zero or too small, which would lead to unstable values.

[0084] In the Layer, the residual from its point to the plane It can be written as:

[0085] (12)

[0086] in, Let G represent the rotation matrix of the IMU coordinate system relative to the global coordinate system G at time k. This represents the coordinates of the j-th point in the k-th frame of the LiDAR point cloud after distortion correction in the LiDAR coordinate system L. Let G represent the translation vector of the IMU coordinate system relative to the global coordinate system G at time k. express The normal vector of the plane, express The centroid of a plane;

[0087] For all The residuals corresponding to the hierarchy are fused using a normalized weighted scheme that considers the hierarchical structure: aggregated residuals. and the corresponding Jacobi respectively Scaling is applied; this maintains the noise scale while highlighting planar and consistent range constraints, and stabilizes cross-scale contributions even with severely uneven point cloud density; aggregated residuals. It is expressed as follows:

[0088] (13)

[0089] (14)

[0090] in, Represents the j-th point at the th position. The sum weight of the layers (i.e., the layer weight). express Flatness weight of a plane Represents the prior weights of the hierarchy, and , This represents the attenuation coefficient of the hierarchical prior. , used to control the prior strength of the hierarchy; Let be the distance prior weight, and , This represents the Euclidean distance between point j and the origin of the global coordinate system G. Indicates the lower limit of the distance. This is used to avoid over-magnification of extremely close points.

[0091] Stability weights given by TVSG The final observation residual vector (i.e., the residual vector) is defined as Among them, stability weight It is expressed as follows:

[0092] (15)

[0093] in, The first stability threshold, express The centroid of a plane; Indicates the center of mass In the Voxels of the layer.

[0094] This embodiment assigns stability weights to each residual using TVSG. And when used in weighted residual calculation, it can effectively avoid the loss of information or excessive influence of samples near the threshold.

[0095] The observation residuals are stacked together with the corresponding fused Jacobian matrix and fed into the IEKF update; where the fused Jacobian matrix is... for:

[0096] (16)

[0097] in, Is point j at level The Jacobian matrix of the upper residual with respect to the state vector, Represents the j-th point at the th position. The sum weight of the layers (i.e., the layer weight).

[0098] This application explicitly maintains multi-scale voxel layers using PAVEN, introducing hierarchical-aware weights and robust thresholds in the observation modeling stage, and adaptively selecting the retrieval level and neighborhood size based on measurement distance and local point density. This effectively mitigates the condition number degradation in long-range normal estimation and the noise bias caused by excessive near-field density, stabilizing the average query complexity within an approximately constant range. Furthermore, to improve global consistency without increasing backend complexity, it also considers the hierarchical structure when constructing residuals, making IEKF updates more stable even with significant changes in cross-scale geometry and point cloud density. Through TVSG, it achieves pre-suppression of moving voxels before residual construction at a controllable cost, directly improving the discriminability of geometric constraints and estimation consistency.

[0099] This embodiment can be understood as follows: by maintaining a multi-scale voxel pyramid through the PAVEN strategy, the size of the voxel block can be adaptively adjusted according to the distance between the midpoint of the point cloud and the current position (which can be understood as the current viewpoint), and the range of the search radius can be adaptively adjusted according to the density of points in the current region; it can simultaneously take into account normal stability and constraint recognition.

[0100] This embodiment uses TVSG to achieve dynamic point removal. For example, during the collection of point cloud data while the vehicle is in motion, the vehicle itself is also considered part of the environment, and the feature points of the vehicle itself are also collected, forming noise points, which will create obvious noise interference. Therefore, this embodiment uses TVSG to remove these dynamic noise points. During the modeling process, static object features, such as the features of vehicles and pedestrians, are removed to obtain stable points, thereby avoiding interference and solidifying them into the map structure at low cost.

[0101] Step 103: Based on the predicted pose, covariance, and residual vector, the error state is iteratively updated using the extended Kalman filter (IEKF) algorithm to obtain the updated state. The residual vector is then linearized based on the updated state to obtain the posterior state.

[0102] As an example, the residual vector is introduced into the IEKF and iterated to obtain the pose estimate at the end of the scan. The filter performs iterative updates using the residual vector with a maximum of 4 iterations and a convergence threshold of 0.001. The iteration is completed when the absolute value of the increment in each dimension is less than the convergence threshold or the maximum number of iterations is reached. The residual is then relinearized at the updated state. and iterative extended Kalman filter operator Obtain the posterior state :

[0103] (17)

[0104] in, This represents the posterior state estimate at time k. This represents the posterior error covariance matrix at time k. This represents the predicted pose at time k. This represents the prediction error covariance matrix at time k. This represents the residual vector used in this filter update. This represents the iterative extended Kalman filter.

[0105] Step 104: Based on the posterior state and the distortion-free point set, perform local map update to obtain the updated local map.

[0106] In some embodiments, the above-mentioned local map update based on the posterior state and the distortion-free point set to obtain the updated local map includes: obtaining a posterior global point set based on the posterior state and the distortion-free point set, and obtaining the stability count of the voxel corresponding to each point in the posterior global point set; based on the stability count, determining whether the stability count in the posterior global point set is greater than or equal to the insertion threshold. The target point set; based on the target point set, local map updates are performed to obtain the updated local map.

[0107] As an example, the posterior global point set can be obtained based on the posterior state and the distortion-free point set using the following formula. :

[0108] (18)

[0109] in, This represents the posterior global point set of the point cloud in the global coordinate system G for the k-th frame.

[0110] As an example, the following formula is used to insert a posterior global point that meets the requirements.

[0111] (19)

[0112] In some embodiments, the method further includes: obtaining a stability count in the posterior global point set that is less than an insertion threshold. The first point set; temporarily store the first point set in the cache to update local statistics, but do not mark it as a queryable stable neighborhood; the voxels corresponding to the first point set in subsequent... Continuous hits in the point cloud and in subsequent frames The stability count in the frame is greater than or equal to the insertion threshold. In the case of this, the voxel is updated to a searchable voxel; the voxel corresponding to the first point set is subsequently... If a voxel is no longer observed in the frame point cloud and its stability count decays to 0, then the voxel and its corresponding cached content are discarded.

[0113] As an example, during the map writing phase: to mitigate the pollution of the map by dynamic scenes, TVSG employs a candidate caching mechanism. For each posterior global point... Its corresponding voxel is .

[0114] When the stability of the voxel is below the insertion threshold (Right now )hour, This indicates the stability of a voxel. The point is not immediately written to the map, but is only temporarily stored in the cache to update local statistics, but is not marked as a queryable stable neighborhood.

[0115] Only when subsequent frames continuously hit the voxel and satisfy the condition... hour, This indicates that the voxel will be promoted to a searchable voxel only after the voxel's stability has changed in subsequent frames.

[0116] If the voxel is no longer observed and its stability count decays to 0, the voxel and its cached contents will be discarded.

[0117] In this embodiment, DAC-LIO employs a hash-based voxel map structure. Before inserting a new point into the map, a TVSG-based "reverse" gating step is performed to further shield unstable structures; that is, insertion is only performed when the stability counter meets the condition. Simultaneously, voxels no longer within the sensor's neighborhood are removed according to the LRU (Least Recently Used) strategy, thereby maintaining a sliding local map.

[0118] This method prevents new observation points from being permanently added to the map if the voxels they belong to have poor stability (i.e., frequent changes or dynamics), thus avoiding map contamination by dynamic objects. This application achieves a more robust map maintenance process through error-iterative IEKF updates combined with subsequent "reverse TVSG" map update steps.

[0119] Step 105: In the release branch of the lidar inertial odometry, based on the keyframe pose, obtain the corrected transformation matrix, and based on the corrected transformation matrix and the odometry pose, obtain the corrected release pose.

[0120] In some embodiments, the process of obtaining the corrected transformation matrix based on the keyframe pose, and obtaining the corrected release pose based on the corrected transformation matrix and the odometry pose, includes:

[0121] In the case of a loop closure detected between keyframe A and keyframe B, in a standard homogeneous matrix Constructing relative constraints and confidence level ,in, , Among them, the standard homogeneous matrix SE(3) is a 4×4 transformation matrix that describes the pose (position and orientation) of a rigid body in three-dimensional space. It unifies rotation and translation within a matrix framework, which facilitates continuous coordinate transformation calculations.

[0122] For each loop A Lie algebra is defined by the current odometry pose and the observed relative pose. The six-dimensional residual vector is represented as follows:

[0123] (20)

[0124] in, This represents the six-dimensional residual vector between the i-th frame and the j-th frame. This represents the predicted relative pose obtained from the current estimate. Represents the odometry pose of the j-th frame in the global coordinate system G; when Sometimes, ; Indicates that it is defined in The matrix logarithmic mapping on; , This represents the set of currently active loop edges; Represents a six-dimensional eigenvector space;

[0125] Obtain the global correction in the form of left multiplication , The global correction aligns all loop edges in the least squares sense, while keeping the computational complexity within a certain range. Magnitude;

[0126] Based on the six-dimensional residual vector, a single correction torque is obtained between adjacent keyframes A and B. To minimize the objective function, the objective function is expressed as follows:

[0127] (twenty one)

[0128] Among them, the weights of each loop edge It is directly proportional to its confidence level, that is , Let be the confidence level of each loop edge;

[0129] Based on the weights of each loop edge and the six-dimensional residual vector, the weighted average torque is obtained using the following formula. To perform closed-loop updates:

[0130] (twenty two)

[0131] Based on the weighted average torque, the transformation matrix used for publishing will be... The global correction is gradually accumulated by updating the gain to obtain the corrected transformation matrix;

[0132] (twenty three)

[0133] in, , To correct the upper limit of torque, To update the gain, ;

[0134] The odometry pose of the current frame in global coordinate system G is corrected based on the modified transformation matrix to obtain the corrected publication pose. :

[0135] (twenty four)

[0136] in, Indicates the first The odometry pose of frame G in global coordinate system, the first frame The frame represents the current frame.

[0137] In the parallel branch of the odometry front end, this step is implemented by the lightweight loop smoothing module LLS. The lightweight loop smoothing module calculates the alignment transformation based on the keyframe pose and applies the trajectory correction to the published pose through a gradually updated global transformation, thereby obtaining a smoother trajectory at the loop closure.

[0138] As an example, LLS runs on the release branch, refining only the output trajectory without modifying the internal state or map of IEKF. Specifically, it includes the following steps:

[0139] S51, Keyframe Capture: During scene recognition and geometric verification, when capturing keyframes... When a loop closure is detected between standard homogeneous matrices, Constructing relative constraints , And with a confidence level , ;

[0140] S52. Definition of Pose Residual and Loop Closure Detection:

[0141] set up Keyframe Odometry pose in the global coordinate system This represents the set of currently active loop edges.

[0142] For each loop , Define a Lie algebra using the current odometry pose and the observed relative pose. The six-dimensional residual vector is given by equation (20); when Sometimes, ; Indicates that it is defined in The matrix logarithmic mapping on;

[0143] S53. Global Correction Transformation Based on Weighted Average: LLS Finds a Global Correction in Left-Multiplicative Form , Align all loop edges in the least squares sense while keeping the computational complexity within a certain range. Magnitude;

[0144] Using a single "correction torque" The near-complete pose graph optimization problem has an objective as shown in equation (21), where the weight of each edge is... Its confidence level Proportional, that is A robust trimming strategy was adopted to obtain the weighted average torque. ;

[0145] S54. Publish Pose: Transformation matrix used for publishing. With a smaller gain Gradually accumulate global corrections; obtain the corrected transformation matrix, as shown in equation (23); for time... The corrected release pose is obtained as shown in equation (24).

[0146] Therefore, the lightweight loop smoothing module LLS can produce a globally consistent output trajectory that completes loop closure. More importantly, LLS runs on the release branch throughout the process, refining only the output trajectory without modifying the internal state or map of IEKF. Thus, the real-time odometry thread is not interrupted, and drift is smoothly eliminated within a few frames, significantly reducing data processing load and improving data processing efficiency.

[0147] The lightweight loop smoothing module LLS in this application is decoupled from the filtering backbone, performs intra-segment attitude interpolation on the key frame sequence, and applies an external global correction matrix to the published pose, achieving smoothness and global consistency of the closed-loop trajectory without re-optimizing the state or re-mapping the map.

[0148] This can be understood as follows: in long-distance prediction, as errors accumulate, the drift will become larger and larger. This embodiment performs a small-distance correction within a certain period of time, i.e., between two keyframes, to reduce drift accumulation and improve the consistency of long-term trajectories.

[0149] This scheme adaptively selects the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set, which can simultaneously take into account normal stability and constraint recognition. This scheme removes dynamic points through temporal voxel stability gating, avoiding interference and solidifying the map structure at low cost. This scheme corrects the odometry pose based on the corrected transformation matrix obtained from the keyframe pose, and obtains the corrected published pose, reducing drift accumulation and improving the consistency of long-term trajectory.

[0150] Unlike conventional iVox and iVox-PHC, which use hierarchical storage within a single global voxel resolution, this invention explicitly maintains multi-scale voxel layers through PAVEN. It introduces hierarchy awareness and robust thresholding in the residual construction stage, adaptively selecting the retrieval voxel level and neighborhood scale based on measurement distance and local point density. This achieves adaptive perceptual adjustment of the voxel level and neighborhood scale, effectively mitigating the condition number degradation in long-range normal estimation and the noise bias caused by excessive near-field density. It stabilizes the average query complexity within an approximately constant range, making IEKF updates more stable even with significant changes in cross-scale geometry and point cloud density. This improves global consistency without increasing backend complexity.

[0151] This invention utilizes Time Voxel Stability Gated (TVSG) to obtain stability counts for each voxel at the map level through time-series statistics. These counts are then applied simultaneously to the neighborhood selection and map writing stages. This achieves pre-suppression of moving voxels before residual construction at a controllable cost, effectively preventing data pollution and directly improving the identifiability and estimation consistency of geometric constraints. Furthermore, it can be used bidirectionally to avoid interference from being fixed into the map structure at low cost. TVSG, without relying on semantics, pre-suppresses dynamic pollution in both the residual construction and map writing stages, effectively preventing data pollution and further improving data accuracy and processing speed.

[0152] This invention utilizes lightweight loop smoothing (LLS) in the release branch of the lidar inertial odometry. Based on the keyframe pose, a corrected transformation matrix is ​​obtained. Based on the corrected transformation matrix and the odometry pose, a corrected release pose is obtained. This achieves smoothness and global consistency of the closed-loop trajectory without re-optimizing the state or re-mapping the map. Specifically, intra-segment attitude interpolation is performed on the keyframe sequence, and an external global correction matrix is ​​applied to the release pose to obtain a smoother trajectory performance at the closed loop.

[0153] This application presents a mapping and inspection positioning method based on lidar inertial odometry. Building upon the tightly coupled lidar-inertial filtering framework derived from the lightweight lidar-inertial odometry method Faster-LIO, it employs error-state IEKF to fuse IMU and LiDAR point clouds in real time. The proposed method utilizes PAVEN to achieve adaptive adjustment of voxel levels and neighborhood sizes, balancing long-range normal stability with near-field computational efficiency. TVSG, without semantic dependence, suppresses dynamic contamination in the two stages of residual construction and map writing, effectively preventing data pollution and further improving data accuracy and processing speed. LLS, decoupled from the filtering backbone, achieves smooth closed-loop trajectory and global consistency without re-optimizing the state or re-mapping the map.

[0154] Corresponding to the above method embodiments, this application also provides a mapping and inspection positioning system based on lidar inertial odometry, such as... Figure 2 As shown, the mapping and inspection positioning system based on lidar inertial odometry includes:

[0155] The data processing module 201 is used to acquire the current frame point cloud data of the LiDAR and the IMU data, perform state propagation processing on the IMU data to obtain the predicted pose and covariance, and perform distortion correction processing on the current frame point cloud data of the LiDAR based on the predicted pose to obtain the distortion-corrected point set and obtain the predicted global point set based on the distortion-corrected point set.

[0156] The residual construction module 202 is used to adaptively select the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set through the pyramid-type adaptive voxel and neighborhood strategy PAVEN, and based on the target voxel level and target neighborhood scale, obtain the neighborhood point set of each predicted global point in the predicted global point set, and construct the point-to-plane residual vector based on the neighborhood point set of the predicted global point set. The neighborhood point set is obtained by combining the time voxel stability gated TVSG.

[0157] The error update module 203 is used to iteratively update the error state based on the predicted pose, covariance and residual vector through the iterative extended Kalman filter (IEKF) algorithm to obtain the updated state, and to linearize the residual vector based on the updated state to obtain the posterior state.

[0158] The map update module 204 is used to update the local map based on the posterior state and the distortion-free point set to obtain the updated local map.

[0159] The pose publishing module 205 is used in the publishing branch of the lidar inertial odometry to obtain the corrected transformation matrix based on the key frame pose, and to obtain the corrected publishing pose based on the corrected transformation matrix and the odometry pose.

[0160] It should be noted that the foregoing explanation of the mapping and inspection positioning method based on lidar inertial odometer also applies to the mapping and inspection positioning system based on lidar inertial odometer in this embodiment, and will not be repeated here.

[0161] Examples and Comparative Examples

[0162] A device configured with a LiDAR inertial odometry mapping and inspection positioning method based on the above-described embodiments of this application is installed on vehicle number one. A device applying the existing method FAST-LIO2 is installed on vehicle number two as comparative example 1, and a device applying the existing method Faster-LIO is installed on vehicle number three as comparative example 2. Data is collected from the three vehicles respectively, and finally processed... Figure 3 , Figure 4This is illustrated in Table 1, where, Figure 3 This is a rendering of the effect in this embodiment; Figure 4 This is a comparison diagram of the effect.

[0163] Table 1: Data Comparison Table between Examples and Comparative Examples

[0164]

[0165] Wherein, NCLT (North Campus Long-Term) represents the original input data used in the examples and comparisons. This data is a large-scale robotic SLAM dataset collected at the University of Michigan North Campus, emphasizing the challenges posed by environmental changes over time. The names in the table are the names of the data packets in the dataset, sourced from the University of Michigan North Campus Long-Term Vision and LiDAR dataset. The average time (ms) is the average time taken to process each frame of LiDAR data completely. That is, "the average time per frame in milliseconds"; ATE (m) represents the absolute trajectory error, used to measure the global position error of the estimated trajectory relative to the true trajectory; Path Length (m) represents the odometry trajectory length after each method runs on this data packet; GT Path Length (m) represents the true trajectory length of this data packet.

[0166] Data analysis in Table 1:

[0167] 1. The embodiment achieves minimal absolute trajectory error with only a slight increase in computing resources while maintaining high speed. 2. It is evident that the embodiment has the smallest predicted trajectory length, which is also closest to the true length. Combined with the reduction in error, it can be inferred that, thanks to multi-scale voxel space selection and loop closure smoothing, the embodiment can handle the details of each frame better, reducing matching drift caused by insufficient spatial feature accuracy.

[0168] Figure 3 and Figure 4 Data analysis:

[0169] Figure 3 and Figure 4 The visualizations of the embodiment and Comparative Example 1 are shown using the same spatial voxel resolution of 0.5m×0.5m×0.5m. These two images enable more downsampled uniform feature mapping while preserving complex feature environments at long distances. Figure 3 and Figure 4 The green area within the red box represents trees; this is clearly visible when comparing the two images. Figure 3 The display of trees is denser and clearer. Figure 4 The image is rather blurry and unclear. Figure 3 and Figure 4The blue-green color in the image represents the vehicle's trajectory. For clearer comparison, portions of the trajectory are outlined with yellow ellipses. Comparing the two images... Figure 3 The vehicle's trajectory is clearer, and there are fewer impurities besides the vehicle's trajectory. Figure 4 In the image, many blurry yellow dots are caused by unstable structures, thus interfering with the vehicle's actual trajectory. Additionally, the vehicle's starting point is outlined in magenta, and the data collected within these magenta frames appears as yellow circles; this is determined by the inherent characteristics of the radar itself. Figure 3 The yellow circles at the starting point are relatively sparse while preserving the features, while Figure 4 The denser yellow circles indicate that, under the same downsampling parameters, Figure 3 For large-scale scenes, computational resources can be concentrated in areas with dense textures, reducing the consumption in areas with uniform features. In summary, from... Figure 3 and Figure 4 As can be clearly seen, this method can effectively address dynamic noise and equipment issues during the mapping process, thereby significantly eliminating unstable structures and obtaining smoother trajectories.

[0170] To provide a more detailed efficiency evaluation, the single-frame processing time of each method was recorded in the scene corresponding to the long-term planning map and LiDAR dataset of the University of Michigan North Campus. This covered the complete workflow from IMU pre-integration and point cloud deskewing to local map maintenance and incremental mapping, such as... Figure 5 As shown, the vertical axis represents the processing time of a single frame, and the horizontal axis represents the time axis; and the number of map points maintained changes over time, as shown below. Figure 6 As shown, the vertical axis represents the number of map points, and the horizontal axis represents time. The results further demonstrate that, compared to Faster-LIO, the proposed DAC-LIO only increases computational cost by a limited amount, but is significantly more efficient than FAST-LIO2, with a similar trend in the number of map points maintained. These results indicate that, thanks to multi-scale voxel representation and a keyframe-based cyclic smoothing mechanism, DAC-LIO can enhance local spatial detail modeling capabilities, further reduce drift accumulation, and improve the consistency of long-term trajectories, while maintaining manageable computational costs.

[0171] In summary, this method maintains temporal statistics for each voxel at the map level using Time Voxel Stability Gated (TVSG), and applies this to both nearest neighbor retrieval and map writing. This achieves pre-suppression of moving voxels before residual construction, directly improving the discriminability of geometric constraints and estimation consistency. Simultaneously, TVSG is used to maintain time-stability counting and observation statistics for voxels, and these are used bidirectionally to avoid interference from being fixed into the map structure at low cost. This application proposes a pyramid-style adaptive voxel and neighborhood strategy, PAVEN, which introduces hierarchical-aware weights and robust thresholds in the observation modeling stage. Based on the measurement distance and local point density, it adaptively selects the retrieval level and neighborhood size, achieving adaptive perception adjustment of voxels and neighborhoods. This effectively alleviates the condition number deterioration of far-range normal estimation and the noise bias caused by excessive near-field density, stabilizing the average query complexity within an approximately constant range. Furthermore, lightweight loop closure smoothing (LLS) is used to perform intra-segment pose interpolation on the keyframe sequence, and an external global correction matrix is ​​applied to the published pose to obtain smoother trajectories at loop closures.

[0172] In some examples, the present invention also provides an inspection robot, including an inspection robot body, a memory, and a processor; the memory is used to store computer-executable instructions, and the processor is used to execute the computer-executable instructions to realize the mapping and inspection positioning method based on lidar inertial odometry proposed in the above embodiments.

[0173] The inspection robot described can be applied to municipal drainage pipelines, generating real-time three-dimensional coordinates including mileage, burial depth, and lateral offset. This allows for precise location of potential hazards such as cracks, subsidence, and blockages at branch pipe connections, effectively solving the problem of "not being able to find or digging the wrong spot" during hazard rectification. Precise positioning binds hazards to the pipeline network GIS system, with a marking error of ≤±5cm, ensuring targeted work during rectification and avoiding overlooking major risks, such as pipeline leaks near gas pipelines or subsidence hazards beneath roads. Through this inspection and positioning method and system, the robot can monitor displacement changes at hazard locations, such as crack expansion and pipeline settlement, by comparing positioning data from multiple inspections. It can also predict instability risks using pipeline network mechanical models; for example, if a section of pipeline experiences annual settlement exceeding 5mm, it allows staff to take reinforcement measures in advance, preventing sudden collapses that could lead to road interruptions or groundwater pollution.

[0174] This embodiment also provides a computer device applicable to mapping and inspection positioning methods based on lidar inertial odometers, including a memory and a processor; the memory is used to store computer-executable instructions, and the processor is used to execute the computer-executable instructions to realize the mapping and inspection positioning methods based on lidar inertial odometers proposed in the above embodiments.

[0175] This embodiment also provides a storage medium storing a computer program that, when executed by a processor, implements the mapping and inspection positioning method based on lidar inertial odometry as proposed in the above embodiments.

[0176] The computer device can be a terminal, comprising a processor, memory, communication interface, display screen, and input devices connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The communication interface is used for wired or wireless communication with external terminals; wireless communication can be achieved through Wi-Fi, carrier networks, NFC (Near Field Communication), or other technologies. The display screen can be an LCD screen or an e-ink screen. The input devices can be a touch layer covering the display screen, buttons, a trackball, or a touchpad on the computer device's casing, or an external keyboard, touchpad, or mouse.

[0177] If a function is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods of the various embodiments of the present invention.

[0178] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-including system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device.

[0179] More specific examples (a non-exhaustive list) of computer-readable media include: electrical connections (electronic devices) having one or more wires, portable computer disk drives (magnetic devices), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Furthermore, computer-readable media can even be paper or other suitable media on which programs can be printed, because programs can be obtained electronically, for example, by optically scanning the paper or other media, followed by editing, interpreting, or otherwise processing as necessary, and then stored in computer memory.

[0180] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0181] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A mapping and inspection positioning method based on lidar inertial odometry, characterized in that: Includes the following steps: Acquire the current frame point cloud data of the LiDAR and the IMU data, perform state propagation processing on the IMU data to obtain the predicted pose and covariance, and perform distortion correction processing on the current frame point cloud data of the LiDAR based on the predicted pose to obtain the distortion-corrected point set and obtain the predicted global point set based on the distortion-corrected point set. The target voxel level and target neighborhood scale of each predicted global point in the predicted global point set are adaptively selected by the pyramid-type adaptive voxel and neighborhood strategy PAVEN. Based on the target voxel level and target neighborhood scale, the neighborhood point set of each predicted global point in the predicted global point set is obtained. Based on the neighborhood point set of the predicted global point set, a residual vector from point to plane is constructed. The neighborhood point set is obtained by combining the time voxel stability gated TVSG. Based on the predicted pose, the covariance, and the residual vector, the error state is iteratively updated using the Iterative Extended Kalman Filter (IEKF) algorithm to obtain the updated state. The residual vector is then linearized based on the updated state to obtain the posterior state. Based on the posterior state and the distortion-free point set, a local map update is performed to obtain the updated local map. In the release branch of the lidar inertial odometry, a corrected transformation matrix is ​​obtained based on the keyframe pose, and a corrected release pose is obtained based on the corrected transformation matrix and the odometry pose.

2. The mapping and inspection positioning method based on lidar inertial odometry according to claim 1, characterized in that: The step of adaptively selecting the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set using the pyramid-style adaptive voxel and neighborhood strategy PAVEN, and obtaining the neighborhood point set of each predicted global point in the predicted global point set based on the target voxel level and target neighborhood scale, includes: The PAVEN adaptively selects the target voxel level and target neighborhood scale for each predicted global point in the predicted global point set; based on the target voxel level and target neighborhood scale, the neighborhood point set of each predicted global point in the predicted global point set is obtained; wherein, the PAVEN is used to explicitly maintain a multi-scale voxel pyramid, simultaneously selecting the voxel level and neighborhood scale for each predicted global point, in order to achieve a balance between the stability of the long-distance normal vector and the computational efficiency of the short-distance calculation.

3. The mapping and inspection positioning method based on lidar inertial odometry according to claim 2, characterized in that: The target voxel level and target neighborhood scale of each predicted global point in the predicted global point set are adaptively selected by PAVEN; Based on the target voxel level and the target neighborhood scale, the neighborhood point set of each predicted global point in the predicted global point set is obtained, including: For each predicted global point in the predicted global point set, a target voxel level is selected for the predicted global point from the multi-scale voxel pyramid based on the prior distance. Obtain the number of map points in the neighborhood voxels at different neighborhood scales of the predicted global point at the target voxel level, and determine whether there are any neighborhood voxels at different neighborhood scales that have a number of map points that meet the first neighborhood point requirement at the target neighborhood scale. If there is no neighborhood voxel sphere at the target neighborhood scale that meets the first neighborhood point requirement, return to the voxel level selection step, update the next level of the target voxel level to the target voxel level of the predicted global point, and continue execution. If there exists a target neighborhood voxel sphere with a map point count that meets the first neighborhood point count requirement, the first neighborhood point set corresponding to the target neighborhood voxel sphere is obtained. The stability count of each point in the first neighborhood point set at the target voxel level is obtained through the TVSG. Points whose stability count in the first neighborhood point set does not meet the first stability threshold are filtered out to obtain the filtered first neighborhood point set. The TVSG is used to continuously update two attribute values ​​for each voxel in the map. The two attribute values ​​include the frequency of the voxel being observed in the time series and whether the voxel is observed in the current frame. The stability count of each voxel is obtained based on the two attribute values. Determine whether the number of points in the filtered first neighborhood point set meets the first neighborhood point count threshold. If not, return to the voxel level selection step, update the next level of the target voxel level to the target voxel level of the predicted global point, and continue execution until the number of points in the filtered first neighborhood point set meets the first neighborhood point count threshold, thereby obtaining the neighborhood point set of the predicted global point, and thus obtaining the neighborhood point set of each predicted global point in the predicted global point set.

4. The mapping and inspection positioning method based on lidar inertial odometry according to claim 3, characterized in that: The step of constructing a residual vector from a point to a plane based on the neighborhood point set of the predicted global point set includes: Perform plane fitting on the neighborhood point set of each point in the predicted global point set to obtain the plane normal vector and representative point of each fitted plane; Based on the plane normal vectors and representative points of each fitted plane, multiple point-to-plane residuals are constructed, and the multiple residuals are fused to obtain the residual vector.

5. The mapping and inspection positioning method based on lidar inertial odometry according to claim 4, characterized in that: The process of fusing multiple residuals to obtain the residual vector includes: Obtain the level weight corresponding to the level where each residual is located, and weight the multiple residuals based on the level weight to obtain the aggregated residual; Obtain the stability weights assigned to each residual by the TVSG; The residual vector is obtained based on the stability weights and the aggregated residuals.

6. The mapping and inspection positioning method based on lidar inertial odometry according to claim 3, characterized in that: The step of updating the local map based on the posterior state and the distortion-free point set to obtain the updated local map includes: Based on the posterior state and the distortion-free point set, a posterior global point set is obtained, and the stability count of the voxel corresponding to each point in the posterior global point set is acquired. Based on the stability count, obtain the set of target points in the posterior global point set whose stability count is greater than or equal to the insertion threshold; The local map is updated based on the target point set to obtain the updated local map.

7. The mapping and inspection positioning method based on lidar inertial odometry according to claim 6, characterized in that: The method further includes: Obtain the first set of points in the posterior global point set whose stability count is less than the insertion threshold; The first point set is temporarily stored in the cache to update local statistics, but it is not marked as a queryable stable neighborhood. The voxels corresponding to the first point set are subsequently... Continuous hits in the point cloud and in subsequent frames If the stability count in a frame is greater than or equal to the insertion threshold, the voxel is updated to a retrievable voxel; The voxels corresponding to the first point set are subsequently... If a voxel is no longer observed in the frame point cloud and its stability count decays to 0, then the voxel and its corresponding cached content are discarded.

8. The mapping and inspection positioning method based on lidar inertial odometry according to claim 6, characterized in that: The TVSG uses frame-level occupancy indicators. Indicates whether each voxel is observed in the current frame; The TVSG records the number of times each voxel is observed in the time series using a stability counter, and normalizes the number of observations to obtain a stability count. It is expressed as follows: ； in, Voxel representation In the The number of times a frame is recorded by a stability counter. Voxel representation In the The number of times a frame is recorded by a stability counter. and These are the control of the count growth and decay rates, respectively. This is the upper limit saturation value; Stability count It is expressed as follows: 。 9. The mapping and inspection positioning method based on lidar inertial odometry according to claim 1, characterized in that: The process of obtaining a corrected transformation matrix based on the keyframe pose, and obtaining a corrected release pose based on the corrected transformation matrix and the odometry pose, includes: In the case of a loop closure detected between adjacent keyframes A and B, in a standard homogeneous matrix Constructing relative constraints and confidence level ,in, , ; For each loop A Lie algebra is defined by the current odometry pose and the observed relative pose. The six-dimensional residual vector is represented as follows: ； in, This represents the six-dimensional residual vector between the i-th frame and the j-th frame. This represents the predicted relative pose obtained from the current estimate. Represents the odometry pose of the j-th frame in the global coordinate system G; when Sometimes, ; Indicates that it is defined in The matrix logarithmic mapping on; , This represents the set of currently active loop edges. Represents a six-dimensional eigenvector space; Obtain the global correction in the form of left multiplication , The global correction aligns all loop edges in the least squares sense, while keeping the computational complexity within a certain range. Magnitude; Based on the six-dimensional residual vector, a single correction torque is obtained between the adjacent keyframes A and B. The objective function is minimized, and the objective function is expressed as follows: ； Among them, the weights of each loop edge It is directly proportional to its confidence level, that is , Let be the confidence level of each loop edge; Based on the weights of each loop edge and the six-dimensional residual vector, the weighted average torque is obtained using the following formula. To perform closed-loop updates: ； Based on the weighted average torque, the transformation matrix used for publishing will be... The global correction is gradually accumulated by updating the gain to obtain the corrected transformation matrix; ； in, , To correct the upper limit of torque, To update the gain, ; The odometry pose of the current frame in the global coordinate system G is corrected based on the modified transformation matrix to obtain the corrected publication pose. : ； in, Indicates the first The odometry pose of frame G in global coordinate system, the first frame The frame represents the current frame.

10. A mapping and inspection positioning system based on lidar inertial odometry, characterized in that: include: The data processing module is used to acquire the current frame point cloud data of the LiDAR and the IMU data, perform state propagation processing on the IMU data to obtain the predicted pose and covariance, and perform distortion correction processing on the current frame point cloud data of the LiDAR based on the predicted pose to obtain the distortion-corrected point set and obtain the predicted global point set based on the distortion-corrected point set. The residual construction module is used to adaptively select the target voxel level and target neighborhood scale of each predicted global point in the predicted global point set through the pyramid-type adaptive voxel and neighborhood strategy PAVEN, and obtain the neighborhood point set of each predicted global point in the predicted global point set based on the target voxel level and target neighborhood scale, and construct the point-to-plane residual vector based on the neighborhood point set of the predicted global point set, wherein the neighborhood point set is obtained by combining the time voxel stability gated TVSG; The error update module is used to iteratively update the error state based on the predicted pose, the covariance, and the residual vector using the iterative extended Kalman filter (IEKF) algorithm to obtain the updated state, and to linearize the residual vector based on the updated state to obtain the posterior state. The map update module is used to perform local map updates based on the posterior state and the distortion-free point set to obtain an updated local map. The pose publishing module is used to obtain a corrected transformation matrix based on the keyframe pose in the publishing branch of the lidar inertial odometry, and to obtain a corrected publishing pose based on the corrected transformation matrix and the odometry pose.

Images