Trajectory restoration and map reconstruction method for solving time drift error and application thereof

Abstract

The application discloses a trajectory restoration and map reconstruction method capable of solving time drift error and application, and relates to the technical field of data processing; by constructing a robot kinematics model constraint, fusing real-time synchronous data streams of multiple sensors, combining a dynamic sensor weight distribution function and a Kalman filter to generate an initial motion trajectory; in a feature-free area, adjusting the trajectory based on dead reckoning and a pipeline kinematics model, and calculating pose deviation by matching pipeline feature points through an ICP algorithm, and correcting trajectory error by using a Kalman filter; an optimal trajectory is obtained by adopting a sliding window graph optimization to fuse IMU, vision and pipeline geometric constraints; a three-dimensional digital twin model with defect mapping is generated by combining laser point cloud and Poisson reconstruction; the application solves the problems of large sensor cumulative error, feature missing positioning failure and low modeling precision, realizes high-precision positioning in a complex pipeline environment, and significantly improves detection efficiency and reduces artificial dependence.

Description

Technical Field

[0001] The present invention belongs to the field of data processing technology, and more specifically, relates to a trajectory restoration and map reconstruction method and application capable of resolving time drift errors. Background Art

[0002] As urban underground pipeline networks expand and age, the demand for pipeline inspection and maintenance surges. Traditional manual inspections are inefficient and dangerous, making pipeline robots a core solution for the industry. However, underground pipelines present complex environments (including straight pipes, elbows, and branches), low light levels, lack of GPS signals, and variable structures. Existing robotic positioning technologies are susceptible to sensor error accumulation and the sparsity of environmental features, leading to trajectory distortion and model deviation. The industry still needs a high-precision positioning and modeling method that integrates multi-source data and adapts to environmental changes to support digital twin-driven pipeline network operation and maintenance management.

[0003] In existing pipeline robot positioning and mapping technologies, single reliance on sensors can easily lead to cumulative errors, especially in complex pipeline structures (such as elbows and tees), where errors are significantly amplified. The lack of a dynamic weight distribution mechanism makes it difficult to adapt to the multimodal perception requirements of different pipeline environments (straight pipes and elbows). Traditional feature matching algorithms are prone to failure in areas without significant pipeline features (long straight pipe sections), resulting in trajectory drift. The failure to fully integrate pipeline geometric constraints (such as pipe diameter and curvature radius) results in insufficient three-dimensional reconstruction accuracy and an inability to generate high-fidelity digital twin models, restricting the intelligent operation and maintenance of urban underground pipeline networks. Summary of the Invention

[0004] (1) Technical problems solved

[0005] In response to the problems in the related art, the present invention provides a trajectory restoration and map reconstruction method that can solve the time drift error, so as to overcome the above-mentioned technical problems existing in the existing related art.

[0006] (2) Technical solution

[0007] To solve the above technical problems, the present invention is achieved through the following technical solutions:

[0008] S1. Construct robot kinematic model constraints; collect and process multi-source pipeline data to obtain real-time synchronized data streams;

[0009] S2. By combining real-time sensor weight data with real-time synchronous data streams, and using Kalman filtering and robot kinematic model constraints, the initial motion trajectory is obtained; a local sparse feature map is constructed;

[0010] S3. When the distance to the no-pipe feature point exceeds the no-pipe feature point distance threshold, the initial motion trajectory is adjusted until the pipeline feature point is detected, thereby obtaining an adjusted motion trajectory; otherwise, the initial motion trajectory is used as the adjusted motion trajectory;

[0011] S4. When a pipeline feature point is detected, the pipeline feature point is matched with the local sparse feature map, and the time drift error is calculated using the iterative closest point algorithm;

[0012] The motion trajectory is corrected according to the time drift error combined with Kalman filtering to obtain the optimized motion trajectory;

[0013] S5, combining the sliding window to perform global trajectory optimization on the optimized motion trajectory to obtain the optimal trajectory;

[0014] S6. Construct a three-dimensional mesh model of the pipeline based on the optimal trajectory; map the pipeline defects to the three-dimensional mesh model of the pipeline to generate a digital twin map;

[0015] The present invention generates globally consistent high-precision motion trajectories through dynamic weight allocation of multi-source sensors, Kalman filter fusion and robot kinematic constraints, combined with dead reckoning adjustment in featureless areas and feature matching error correction technology based on the ICP algorithm, sliding window graph and digital twin modeling technology. It realizes high-precision detection, full-factor modeling and intelligent operation and maintenance of underground pipelines, significantly reducing manual dependence and improving operation and maintenance efficiency.

[0016] Preferably, the S1 comprises the following steps:

[0017] S11, collecting pipeline environmental parameter data to obtain a pipeline environmental parameter data set;

[0018] According to the geometric parameter data in the pipeline environment parameter data set, the robot kinematic model constraints are established;

[0019] S12: Start the pipeline robot and initialize the MEMS-IMU, binocular vision camera, wheel encoder odometer, and lidar;

[0020] The IMU outputs accelerometer and gyroscope data in real time, the wheel odometer records travel distance and speed, the binocular camera captures images of the pipeline inner wall, and the lidar obtains the pipeline cross-section point cloud, resulting in real-time multi-source data streams.

[0021] Align real-time multi-source data streams by timestamps to obtain real-time synchronized data streams;

[0022] The present invention establishes robot kinematic constraints by collecting parameters such as pipe diameter and elbow curvature, and initializes multiple sensors (IMU / vision / encoder / radar). After timestamp alignment, a synchronous data stream is generated, providing a basis for subsequent positioning and modeling.

[0023] Preferably, said S2 comprises the following steps:

[0024] S21, constructing a sensor weight distribution function;

[0025] According to the pipeline structure type, combined with the sensor weight allocation function formula, the sensor weight is dynamically allocated to obtain real-time sensor weight data;

[0026] S22, based on the inertial data of the IMU and the displacement information of the wheel odometer in the real-time synchronous data stream combined with the real-time sensor weight data, preliminarily estimate the robot posture through the state prediction equation of the Kalman filter and the robot kinematic model to obtain the initial motion trajectory;

[0027] S23, setting a pipeline feature point set;

[0028] Collect historical pipeline data; extract pipeline feature points from the historical pipeline data using binocular visual odometry combined with pipeline feature point sets to construct a local sparse feature map;

[0029] The present invention dynamically allocates sensor weights (focusing on IMU / odometer in straight pipe sections and increasing visual weight in elbow sections), fuses multi-source data, and generates an initial trajectory based on Kalman filtering and kinematic models. It also uses historical feature points to construct a local sparse map, providing a benchmark for subsequent error correction and improving positioning robustness in complex environments.

[0030] Preferably, the step S3 includes the following steps:

[0031] S31, setting the no-pipeline feature point distance threshold to g1, and when the no-pipeline feature point distance g2 ≥ the no-pipeline feature point distance threshold g1, adjusting the initial motion trajectory based on the wheel odometer and the IMU dead reckoning, combined with the pipeline kinematic model, until the pipeline feature point is detected, thereby obtaining an adjusted motion trajectory;

[0032] Otherwise, the initial motion trajectory is directly used as the adjusted motion trajectory;

[0033] The present invention sets a distance threshold for no feature points (e.g., 10 meters). If no feature points are detected for 10 consecutive meters, the trajectory is adjusted based on the wheel odometer, IMU, and kinematic model until feature point recognition is restored, thereby ensuring the continuity of positioning in long straight pipe sections.

[0034] Preferably, the S4 comprises the following steps:

[0035] S41. When the robot moves to a pipeline feature point, the vision system triggers high-precision image acquisition and extracts features using an ORB / SIFT algorithm to obtain pipeline feature point features, wherein the pipeline feature point features include three-dimensional point cloud features of the pipeline feature point;

[0036] S42, feature matching is performed on the pipeline feature point features and the local sparse feature map, and the ICP algorithm (Iterative Closest Point algorithm) is used to align the adjacent segment point clouds in the feature matching results to calculate the posture deviation;

[0037] S43. Based on the posture deviation and in combination with the real-time sensor weight data, a weighted average of the posture deviations of vision and laser is performed to obtain the position drift error and attitude error of the inertial navigation system;

[0038] The position drift error and attitude error are input as observation values ​​into the updated state equation of the Kalman filter to correct and adjust the motion trajectory to obtain the optimized motion trajectory;

[0039] When detecting pipeline feature points, the present invention extracts three-dimensional point cloud features through ORB / SIFT and matches them with the local map, calculates the pose deviation using the ICP algorithm, corrects the inertial navigation drift error in combination with the sensor weight, and finally updates the trajectory through Kalman filtering to eliminate accumulated errors and improve positioning accuracy.

[0040] Preferably, said S42 includes the following steps:

[0041] S421, setting the initial rotation matrix to the identity matrix, the initial translation vector to the zero vector, and setting the maximum number of iterations;

[0042] Set the three-dimensional point cloud in the pipeline feature point feature to obtain the pipeline feature point cloud;

[0043] Setting the 3D reference point cloud in the local sparse feature map;

[0044] S422. For each point in the pipeline feature point cloud, find the point closest to it in the local map reference point cloud to form a point pair;

[0045] S423, calculating the center point of the pipeline feature point cloud and the center point of the three-dimensional reference point cloud; using the point pair and the center point of the pipeline feature point cloud and the center point of the three-dimensional reference point cloud, constructing a matrix and performing SVD decomposition to obtain an SVD decomposition result;

[0046] Calculate the rotation matrix based on SVD decomposition and combined with the initial rotation matrix;

[0047] Calculate the translation vector based on the rotation matrix, the center point of the pipeline feature point cloud, the center point of the 3D reference point cloud, and the initial translation vector;

[0048] S424, using the calculated rotation matrix and translation vector to update the pose of the pipeline feature point cloud, and calculating the error between the pose of the pipeline feature point cloud and the three-dimensional reference point cloud to obtain a pose deviation;

[0049] S425, repeat S422, S423, S424, and when the maximum number of iterations is reached, stop the iteration and obtain the posture deviation;

[0050] The present invention realizes precise point cloud registration through the ICP algorithm. After initializing the rotation matrix and translation vector, the nearest neighbor point pairs of the feature point cloud and the local map point cloud are found through iteration, the point cloud center is calculated and the covariance matrix is ​​constructed. The optimal rotation matrix and translation vector are solved through SVD decomposition, the point cloud pose is updated and the error is calculated until the maximum number of iterations is reached. Finally, the pose deviation is output to eliminate the accumulated sensor error and correct the trajectory.

[0051] Preferably, the S5 comprises the following steps:

[0052] S51. Using a sliding window method based on graph optimization, the real-time synchronous data stream within the window is integrated with the corrected observations. Pipeline geometry constraints are introduced to solve the globally consistent optimal trajectory through the global trajectory optimization objective function.

[0053] The present invention uses sliding window graph optimization technology to integrate real-time data, corrected observations and pipeline geometric constraints (such as pipe diameter and elbow curvature), solves the global consistent optimal trajectory through the objective function, and eliminates cumulative errors.

[0054] Preferably, the S6 comprises the following steps:

[0055] S61, fusing the lidar point cloud in the real-time synchronous data stream with the optimal trajectory, and generating an initial pipeline 3D mesh model using a Poisson surface reconstruction algorithm; marking the pipeline topology and key nodes in the initial pipeline 3D mesh model to obtain the pipeline 3D mesh model;

[0056] S62, mapping the pipeline defects detected by the robot to the pipeline three-dimensional grid model to generate a digital twin map with attribute information;

[0057] Output the digital twin map into a standardized data format and connect it to the city's gas network operation system;

[0058] The present invention fuses laser point clouds with optimal trajectories, uses Poisson reconstruction to generate a three-dimensional model with a topological structure, maps pipeline defects to generate a digital twin map in a standardized format, and connects to the city's operation and maintenance platform to achieve pipeline network visualization and precise management.

[0059] A trajectory restoration and map reconstruction system capable of resolving temporal drift errors, used to implement the aforementioned trajectory restoration and map reconstruction method capable of resolving temporal drift errors, comprising a multi-source data acquisition and synchronization module, a dynamic weight fusion and initial trajectory generation module, a trajectory dynamic adjustment module, a trajectory correction and global optimization module, and a 3D modeling and digital twin generation module;

[0060] The multi-source data acquisition and synchronization module is responsible for collecting pipeline environmental parameters, initializing various sensors, and aligning real-time multi-source data streams through timestamps to generate synchronized data streams. It also establishes robot kinematic model constraints based on pipeline geometric parameters to ensure geometric consistency between the robot's posture and the pipeline structure, providing basic data and model support for subsequent processing.

[0061] The dynamic weight fusion and initial trajectory generation module is used to dynamically adjust the confidence of different sensors through the sensor weight distribution function, combine the Kalman filter and the robot kinematic model to preliminarily estimate the robot's position and generate the initial motion trajectory; use the binocular visual odometry to extract pipeline feature points and construct a local sparse feature map to provide a reference benchmark for subsequent trajectory optimization;

[0062] The dynamic trajectory adjustment module is used to adjust the initial trajectory in real time when the robot's continuous travel distance exceeds the threshold for no feature points. This module uses the wheel odometer, IMU dead reckoning, and pipeline kinematic model to adjust the initial trajectory until a new pipeline feature point is detected. If the threshold is not triggered, the initial trajectory is retained to ensure the robot's motion continuity in the feature-missing area.

[0063] The trajectory correction and global optimization module is used to calculate the pose deviation through feature matching and ICP algorithm when pipeline feature points are detected, and correct the trajectory error by combining the Kalman filter update equation. It further adopts the sliding window method to fuse multi-source data and solves the optimal trajectory through the global trajectory optimization objective function, eliminating cumulative errors and improving the global consistency of the trajectory.

[0064] The 3D modeling and digital twin generation module is used to combine the optimized trajectory with the lidar point cloud, generate a 3D mesh model of the pipeline with a topological structure through a Poisson surface reconstruction algorithm, map the detected pipeline defects into the model, and finally output a standardized digital twin map to provide visualization and decision support for the operation and maintenance of urban underground pipeline networks.

[0065] (3) Beneficial effects

[0066] The present invention has the following beneficial effects:

[0067] The present invention generates globally consistent high-precision trajectories through dynamic weight allocation of multi-source sensors, Kalman filter fusion and robot kinematic constraints, combined with dead reckoning adjustment in featureless areas and feature matching error correction technology based on the ICP algorithm, sliding window graph and digital twin modeling technology. It realizes high-precision detection, full-factor modeling and intelligent operation and maintenance of underground pipelines, significantly reducing manual dependence and improving operation and maintenance efficiency.

[0068] The present invention realizes multi-source data fusion and dynamic weight optimization; by constructing a sensor weight distribution function, the confidence of sensors such as IMU and visual odometry is dynamically adjusted, significantly suppressing the cumulative error of a single sensor and improving the positioning robustness in complex environments; combining Kalman filtering with kinematic model constraints, efficient fusion of multi-source heterogeneous data is achieved, ensuring a balance between high-speed travel in straight pipe sections and precise steering in elbow sections.

[0069] The present invention realizes adaptive trajectory adjustment and error correction. By introducing a distance threshold for pipeline-free feature points, the trajectory is adaptively adjusted through dead reckoning and kinematic model in long straight pipe sections to avoid positioning failure caused by missing features. The ICP algorithm is used to align the feature point cloud and calculate the posture deviation. In combination with the Kalman filter, the trajectory is corrected in real time, effectively eliminating time drift errors and ensuring global trajectory consistency.

[0070] Based on the sliding window graph optimization, the present invention integrates IMU, vision and pipeline geometric constraints to solve the global optimal trajectory and provide a high-precision pose benchmark for lidar point cloud registration. It generates a three-dimensional mesh model with topological attributes through Poisson surface reconstruction, maps pipeline defect information, and outputs a digital twin map in a standardized BIM format to achieve pipeline network status visualization and precise operation and maintenance decision support.

[0071] This invention has engineering applicability; by adopting a combination of low-cost sensors such as MEMS-IMU and binocular cameras, industrial-grade precision is achieved through algorithm optimization; the modular design adapts to different pipe diameters and materials, reducing deployment complexity; the generated digital twin map can be directly connected to the urban gas operation and maintenance platform, promoting the transformation of pipeline network detection from "manual inspection" to "intelligent twin drive", greatly improving safety and operation and maintenance efficiency.

[0072] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] In order to more clearly illustrate the technical solutions of the embodiments of the invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the invention. For ordinary technicians in this field, they can also obtain drawings based on these drawings without paying any creative work.

[0074] Figure 1 Schematic diagram of the flow of the trajectory restoration and map reconstruction method of the present invention that can solve the time drift error;

[0075] Figure 2 This is a module diagram of the trajectory restoration and map reconstruction system of the present invention that can solve time drift errors. DETAILED DESCRIPTION

[0076] The following will clearly and completely describe the technical solutions in the embodiments of the invention in conjunction with the accompanying drawings. Obviously, the embodiments described are only part of the embodiments of the invention, not all of them. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0077] In the description of the present invention, it should be understood that the terms "opening", "upper", "lower", "top", "middle", "inside" and the like indicating orientation or positional relationship are only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the components or elements referred to must have a specific orientation, be constructed and operate in a specific orientation, and therefore cannot be understood as limiting the invention.

[0078] Example 1:

[0079] See also Figure 1 The present invention discloses a trajectory restoration and map reconstruction method that can solve time drift errors, comprising the following steps:

[0080] S1. Construct robot kinematic model constraints; collect and process multi-source pipeline data to obtain real-time synchronized data streams;

[0081] Said S1 comprises the following steps:

[0082] S11, collecting pipeline environmental parameter data, and obtaining a pipeline environmental parameter data set: , where ai represents the i-th pipeline environmental parameter, and b represents the total number of pipeline environmental parameters; the pipeline environmental parameters include pipe diameter, material, elbow curvature radius, and inclined pipe inclination angle;

[0083] According to the geometric parameter data in the pipeline environment parameter data set, the robot kinematic model constraint is established; the robot kinematic model constraint formula is as follows:

[0084] ;

[0085] Among them, pk represents the position coordinates of the robot in the pipeline, ck represents the centerline coordinates of the pipeline, r represents the pipe diameter, α represents the safety margin, qk represents the angular velocity vector of the robot, and nk represents the normal vector of the elbow plane;

[0086] S12: Start the pipeline robot and initialize the MEMS-IMU (micro-electromechanical system inertial measurement unit), binocular vision camera, wheel encoder odometer, and lidar;

[0087] The IMU outputs accelerometer and gyroscope data in real time, the wheel odometer records travel distance and speed, the binocular camera captures images of the pipeline inner wall, and the lidar obtains the pipeline cross-section point cloud, resulting in real-time multi-source data streams.

[0088] Align real-time multi-source data streams by timestamps to obtain real-time synchronized data streams;

[0089] S2. By combining real-time sensor weight data with real-time synchronous data streams, and using Kalman filtering and robot kinematic model constraints, the initial motion trajectory is obtained; a local sparse feature map is constructed;

[0090] The S2 comprises the following steps:

[0091] S21, constructing a sensor weight distribution function; the sensor weight distribution function formula is as follows:

[0092] ;

[0093] Where w represents the weight of the sensor, Ci represents the confidence of sensor i in a specific pipeline environment (straight pipe, elbow), Ei(t) represents the cumulative error estimate of sensor i at time t, λ represents the attenuation coefficient, N represents the total number of sensors, and j represents the sensor index. Represents the product of the confidence of sensor i and its cumulative error estimate, and then undergoes exponential decay. If the cumulative error of sensor i is large, its weight will be reduced accordingly; This means that similar items from all sensors are summed for normalization to ensure that the sum of all weights is 1. For example, in straight pipe sections, the IMU and wheel odometry are used as primary weights, with the visual odometry as a supplementary weight. In elbow / tee sections, the visual odometry weight is increased to suppress the accumulated error of the IMU.

[0094] According to the pipeline structure type (straight pipe, elbow, tee), combined with the sensor weight distribution function formula, the sensor weight is dynamically allocated to obtain real-time sensor weight data;

[0095] S22, based on the inertial data of the IMU in the real-time synchronous data stream and the displacement information of the wheel odometer combined with the real-time sensor weight data, the robot posture is preliminarily estimated through the state prediction equation of the Kalman filter and combined with the robot kinematic model, and the initial motion trajectory is obtained as , where xi represents the robot's position at the i-th moment, xt represents the robot's position at the current moment t, and each xi represents a vector matrix containing position and posture information; the state prediction equation of the Kalman filter is as follows,

[0096] ;

[0097] Where xi represents the prior state at time i (i.e., the robot's position and posture data), f() represents the kinematic model of the pipeline robot, ui represents the control input at time i (i.e., wheel odometer speed, IMU angular velocity), and wi represents the process noise at time i;

[0098] S23. Set pipeline feature point set , where pi represents the pipeline feature point in the i-th position, and q represents the total number of pipeline feature points;

[0099] Collect historical pipeline data; extract pipeline feature points from the historical pipeline data using binocular visual odometry combined with pipeline feature point sets to construct a local sparse feature map;

[0100] S3. When the distance to the no-pipe feature point exceeds the no-pipe feature point distance threshold, the initial motion trajectory is adjusted until the pipeline feature point is detected, thereby obtaining an adjusted motion trajectory; otherwise, the initial motion trajectory is used as the adjusted motion trajectory;

[0101] The S3 includes the following steps:

[0102] S31. Set the no-pipeline feature point distance threshold to g1. When the no-pipeline feature point distance g2 ≥ the no-pipeline feature point distance threshold g1, adjust the initial motion trajectory based on the wheel odometer and the IMU dead reckoning, combined with the pipeline kinematic model, until the pipeline feature point is detected, thereby obtaining an adjusted motion trajectory. For example, if the no-pipeline feature point distance threshold is set to 10 meters, if the robot does not detect the pipeline feature point for 10 consecutive meters, adjust the initial motion trajectory.

[0103] Otherwise, the initial motion trajectory is directly used as the adjusted motion trajectory;

[0104] S4. When a pipeline feature point is detected, the pipeline feature point is matched with the local sparse feature map, and the time drift error is calculated using the iterative closest point algorithm;

[0105] The motion trajectory is corrected according to the time drift error combined with Kalman filtering to obtain the optimized motion trajectory;

[0106] The S4 comprises the following steps:

[0107] S41. When the robot moves to a pipeline feature point (such as a tee interface or a valve), the vision system triggers high-precision image acquisition and extracts features using an ORB / SIFT algorithm to obtain pipeline feature point features, including three-dimensional point cloud features of the pipeline feature point.

[0108] S42, feature matching is performed on the pipeline feature point features and the local sparse feature map, and the adjacent segment point clouds in the feature matching results are aligned using the ICP algorithm to calculate the posture deviation;

[0109] The S42 includes the following steps:

[0110] S421, set the initial rotation matrix R0 to the unit matrix, the initial translation vector Z0 to the zero vector, and set the maximum number of iterations;

[0111] Set the 3D point cloud in the pipeline feature point feature to obtain the pipeline feature point cloud , where ri represents the i-th point of the pipeline feature point cloud, and N represents the total number of pipeline feature point clouds;

[0112] Set the 3D reference point cloud in the local sparse feature map to be , where vi represents the i-th point of the three-dimensional reference point cloud, and M represents the total number of three-dimensional reference point clouds;

[0113] S422. For each point in the pipeline feature point cloud, find the point closest to it in the local map reference point cloud to form a point pair (ri, vi);

[0114] S423. Calculate the center point of the pipeline feature point cloud and the center point of the three-dimensional reference point cloud; construct a matrix using the point pair and the center point of the pipeline feature point cloud and the center point of the three-dimensional reference point cloud, and perform SVD (Singular Value Decomposition) decomposition to obtain an SVD decomposition result;

[0115] Calculate the rotation matrix based on SVD decomposition and combined with the initial rotation matrix R0;

[0116] Calculate the translation vector based on the rotation matrix, the center point of the pipeline feature point cloud, the center point of the 3D reference point cloud, and the initial translation vector Z0;

[0117] S424, using the calculated rotation matrix and translation vector to update the pose of the pipeline feature point cloud, and calculating the error between the pose of the pipeline feature point cloud and the three-dimensional reference point cloud to obtain a pose deviation;

[0118] S425, repeat S422, S423, S424, and when the maximum number of iterations is reached, stop the iteration and obtain the posture deviation;

[0119] S43. Based on the posture deviation and in combination with the real-time sensor weight data, a weighted average of the posture deviations of vision and laser is performed to obtain the position drift error and attitude error of the inertial navigation system;

[0120] The position drift error and attitude error are input as observation values ​​into the updated state equation of the Kalman filter to correct and adjust the motion trajectory to obtain the optimized motion trajectory; the updated state equation formula of the Kalman filter is as follows:

[0121] ;

[0122] Where zi represents the observation value, h() represents the observation model, Hi represents the Jacobian of the observation matrix, Ri represents the observation noise covariance matrix, which is used to describe the noise characteristics of the observation value zi; Ki represents the Kalman gain matrix, yi represents the state vector (i.e., the corrected position and posture data of the robot at the i-th moment), Pi represents the state covariance matrix, T represents the matrix transpose, and I represents the identity matrix;

[0123] S5, combining the sliding window to perform global trajectory optimization on the optimized motion trajectory to obtain the optimal trajectory;

[0124] The S5 comprises the following steps:

[0125] S51. Using a sliding window method based on graph optimization, the real-time synchronous data stream within the window is integrated with the corrected observation value, and pipeline geometric constraints (such as pipe diameter restrictions and elbow curvature radius) are introduced to solve the globally consistent optimal trajectory through the global trajectory optimization objective function; the global trajectory optimization objective function is as follows:

[0126] ;

[0127] Where X represents all state variables (i.e., position and attitude) in the sliding window, eIMU, eVO, and epipe represent the residuals of IMU, visual odometry, and pipeline geometric constraints, respectively; Σ represents the covariance matrix of each residual, A, B, and C represent the number of IMU residuals, visual odometry residuals, and pipeline geometric constraint residuals, respectively (i.e., the number of IMU data points, visual odometry data points, and pipeline geometric constraint data points considered in the sliding window), eIMU(i), eVO(j), and epipe(o) represent the IMU residuals, visual odometry residuals, and pipeline geometric constraint residuals at time i, time j, and time o, respectively;

[0128] S6. Construct a three-dimensional mesh model of the pipeline based on the optimal trajectory; map the pipeline defects to the three-dimensional mesh model of the pipeline to generate a digital twin map;

[0129] The S6 comprises the following steps:

[0130] S61, the lidar point cloud in the real-time synchronous data stream is compared with the optimal trajectory, and an initial pipeline three-dimensional mesh model is generated by a Poisson surface reconstruction algorithm;

[0131] Mark the pipeline topology (branches, inclined pipe inclination) and key nodes (tees, valve coordinates) in the initial pipeline 3D mesh model to obtain the pipeline 3D mesh model;

[0132] S62. Mapping pipeline defects (such as corrosion areas and cracks) detected by the robot to a three-dimensional mesh model of the pipeline to generate a digital twin map with attribute information;

[0133] The digital twin map is exported into a standardized data format (such as a BIM model) and connected to the city's gas network operation system.

[0134] Example 2:

[0135] See also Figure 2 , a trajectory restoration and map reconstruction system capable of resolving time drift errors, used to implement the above-mentioned trajectory restoration and map reconstruction method capable of resolving time drift errors, including a multi-source data acquisition and synchronization module, a dynamic weight fusion and initial trajectory generation module, a trajectory dynamic adjustment module, a trajectory correction and global optimization module, and a 3D modeling and digital twin generation module;

[0136] The multi-source data acquisition and synchronization module is responsible for collecting pipeline environmental parameters, initializing various sensors, and aligning real-time multi-source data streams through timestamps to generate synchronized data streams. It also establishes robot kinematic model constraints based on pipeline geometric parameters to ensure geometric consistency between the robot's posture and the pipeline structure, providing basic data and model support for subsequent processing.

[0137] The dynamic weight fusion and initial trajectory generation module is used to dynamically adjust the confidence of different sensors through the sensor weight distribution function, combine the Kalman filter and the robot kinematic model to preliminarily estimate the robot's position and generate the initial motion trajectory; use the binocular visual odometry to extract pipeline feature points and construct a local sparse feature map to provide a reference benchmark for subsequent trajectory optimization;

[0138] The dynamic trajectory adjustment module is used to adjust the initial trajectory in real time when the robot's continuous travel distance exceeds the threshold for no feature points. This module uses the wheel odometer, IMU dead reckoning, and pipeline kinematic model to adjust the initial trajectory until a new pipeline feature point is detected. If the threshold is not triggered, the initial trajectory is retained to ensure the robot's motion continuity in the feature-missing area.

[0139] The trajectory correction and global optimization module is used to calculate the pose deviation through feature matching and ICP algorithm when pipeline feature points are detected, and correct the trajectory error by combining the Kalman filter update equation. It further adopts the sliding window method to fuse multi-source data and solves the optimal trajectory through the global trajectory optimization objective function, eliminating cumulative errors and improving the global consistency of the trajectory.

[0140] The 3D modeling and digital twin generation module is used to combine the optimized trajectory with the lidar point cloud, generate a 3D mesh model of the pipeline with a topological structure through a Poisson surface reconstruction algorithm, map the detected pipeline defects into the model, and finally output a standardized digital twin map to provide visualization and decision support for the operation and maintenance of urban underground pipeline networks.

[0141] Throughout this specification, references to terms such as "one embodiment," "example," or "specific example" indicate that the specific features, structures, materials, or characteristics described in conjunction with that embodiment or example are included in at least one embodiment or example of the invention. In this specification, schematic representations of these terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0142] The preferred embodiments of the invention disclosed above are intended only to help illustrate the invention. These preferred embodiments do not exhaust all details, nor do they limit the invention to the specific embodiments described. Obviously, many modifications and variations are possible based on the content of this specification. These embodiments are selected and described in detail in this specification to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention.

Claims

1. A trajectory restoration and map reconstruction method that can resolve temporal drift errors, characterized by: The following steps are involved: S1. Construct robot kinematic model constraints; collect and process multi-source pipeline data to obtain real-time synchronized data streams; S2. By combining real-time sensor weight data with real-time synchronous data streams, and using Kalman filtering and robot kinematic model constraints, the initial motion trajectory is obtained; a local sparse feature map is constructed; S3. When the distance to the no-pipe feature point exceeds the no-pipe feature point distance threshold, the initial motion trajectory is adjusted until the pipeline feature point is detected, thereby obtaining an adjusted motion trajectory; otherwise, the initial motion trajectory is used as the adjusted motion trajectory; S4. When a pipeline feature point is detected, the pipeline feature point is matched with the local sparse feature map, and the time drift error is calculated using the iterative closest point algorithm; The motion trajectory is corrected according to the time drift error combined with Kalman filtering to obtain the optimized motion trajectory; S5, combining the sliding window to perform global trajectory optimization on the optimized motion trajectory to obtain the optimal trajectory; S6. Build a three-dimensional mesh model of the pipeline based on the optimal trajectory; map the pipeline defects to the three-dimensional mesh model of the pipeline to generate a digital twin map.

2. The trajectory restoration and map reconstruction method capable of resolving time drift errors according to claim 1, characterized in that: Said S1 comprises the following steps: S11, collecting pipeline environmental parameter data to obtain a pipeline environmental parameter data set; According to the geometric parameter data in the pipeline environment parameter data set, the robot kinematic model constraints are established; S12: Start the pipeline robot and initialize the MEMS-IMU, binocular vision camera, wheel encoder odometer, and lidar; The IMU outputs accelerometer and gyroscope data in real time, the wheel odometer records travel distance and speed, the binocular camera captures images of the pipeline inner wall, and the lidar obtains the pipeline cross-section point cloud to obtain real-time multi-source data streams; the real-time multi-source data streams are aligned by timestamps to obtain real-time synchronized data streams.

3. The trajectory restoration and map reconstruction method capable of resolving time drift errors according to claim 1, characterized in that: The S2 comprises the following steps: S21, constructing a sensor weight distribution function; According to the pipeline structure type, combined with the sensor weight allocation function formula, the sensor weight is dynamically allocated to obtain real-time sensor weight data; S22, based on the inertial data of the IMU and the displacement information of the wheel odometer in the real-time synchronous data stream combined with the real-time sensor weight data, preliminarily estimate the robot posture through the state prediction equation of the Kalman filter and the robot kinematic model to obtain the initial motion trajectory; S23, setting a pipeline feature point set; Collect historical pipeline data; extract pipeline feature points from the historical pipeline data through binocular visual odometry combined with pipeline feature point sets, and construct a local sparse feature map.

4. The trajectory restoration and map reconstruction method capable of resolving time drift errors according to claim 1, characterized in that: The S3 includes the following steps: S31, set the distance threshold of the no-pipeline feature point to g 1. When there is no pipeline feature point distance g 2≥No pipeline feature point distance threshold is g At 1, the initial motion trajectory is adjusted based on the dead reckoning of the wheel odometer and IMU and combined with the pipeline kinematic model until the pipeline feature points are detected, and the adjusted motion trajectory is obtained; Otherwise, the initial motion trajectory is directly used as the adjusted motion trajectory.

5. The trajectory restoration and map reconstruction method capable of resolving time drift errors according to claim 1, characterized in that: The S4 comprises the following steps: S41. When the robot moves to a pipeline feature point, the vision system triggers high-precision image acquisition and extracts features using an ORB / SIFT algorithm to obtain pipeline feature point features, wherein the pipeline feature point features include three-dimensional point cloud features of the pipeline feature point; S42, feature matching is performed on the pipeline feature point features and the local sparse feature map, and the adjacent segment point clouds in the feature matching results are aligned using the ICP algorithm to calculate the posture deviation; S43. Based on the posture deviation and in combination with the real-time sensor weight data, a weighted average of the posture deviations of vision and laser is performed to obtain the position drift error and attitude error of the inertial navigation system; The position drift error and attitude error are input as observation values ​​into the updated state equation of the Kalman filter to correct and adjust the motion trajectory to obtain the optimized motion trajectory.

6. The trajectory restoration and map reconstruction method capable of resolving time drift errors according to claim 5, characterized in that: The S42 includes the following steps: S421, setting the initial rotation matrix to the identity matrix, the initial translation vector to the zero vector, and setting the maximum number of iterations; Set the three-dimensional point cloud in the pipeline feature point feature to obtain the pipeline feature point cloud; Setting the 3D reference point cloud in the local sparse feature map; S422. For each point in the pipeline feature point cloud, find the point closest to it in the local map reference point cloud to form a point pair; S423, calculating the center point of the pipeline feature point cloud and the center point of the three-dimensional reference point cloud; using the point pair and the center point of the pipeline feature point cloud and the center point of the three-dimensional reference point cloud, constructing a matrix and performing SVD decomposition to obtain an SVD decomposition result; Calculate the rotation matrix based on SVD decomposition and combined with the initial rotation matrix; Calculate the translation vector based on the rotation matrix, the center point of the pipeline feature point cloud, the center point of the 3D reference point cloud, and the initial translation vector; S424, using the calculated rotation matrix and translation vector to update the pose of the pipeline feature point cloud, and calculating the error between the pose of the pipeline feature point cloud and the three-dimensional reference point cloud to obtain a pose deviation; S425. Repeat S422, S423, and S424. When the maximum number of iterations is reached, stop the iteration and obtain the posture deviation.

7. The trajectory restoration and map reconstruction method capable of resolving time drift errors according to claim 1, characterized in that: The S5 comprises the following steps: S51. A sliding window method based on graph optimization is used to fuse the real-time synchronous data stream within the window with the corrected observation value, and pipeline geometric constraints are introduced to solve the globally consistent optimal trajectory through the global trajectory optimization objective function.

8. The trajectory restoration and map reconstruction method capable of resolving time drift errors according to claim 1, characterized in that: The S6 comprises the following steps: S61, fusing the lidar point cloud in the real-time synchronous data stream with the optimal trajectory, and generating an initial pipeline 3D mesh model using a Poisson surface reconstruction algorithm; marking the pipeline topology and key nodes in the initial pipeline 3D mesh model to obtain the pipeline 3D mesh model; S62, mapping the pipeline defects detected by the robot to the pipeline three-dimensional grid model to generate a digital twin map with attribute information; The digital twin map is exported into a standardized data format and connected to the city's gas pipeline operation system.

9. A trajectory restoration and map reconstruction system that can resolve temporal drift errors, characterized by: A trajectory restoration and map reconstruction method capable of resolving time drift errors as described in any one of claims 1 to 8 is implemented, wherein the system includes a multi-source data acquisition and synchronization module, a dynamic weight fusion and initial trajectory generation module, a trajectory dynamic adjustment module, a trajectory correction and global optimization module, and a three-dimensional modeling and digital twin generation module.

Images