A high-precision positioning system for pipe network inspection equipment based on multi-sensor fusion

Abstract

The application relates to the technical field of navigation positioning of pipe network inspection equipment, and particularly discloses a high-precision positioning system for pipe network inspection equipment based on multi-sensor fusion. In view of the problem that no GNSS signal exists in the pipe network and long-distance inspection leads to serious cumulative drift of positioning, the system integrates IMU, a wheeled odometer and a three-dimensional laser radar, realizes space-time synchronization at the hardware level through a preprocessing module, uses an ESKF fusion model and IMU pre-integration for high-frequency pose estimation at the front end, corrects global errors through a factor graph optimization algorithm in combination with key node features and a global topological prior map at the back end, and introduces measurement weight adaptive adjustment logic to cope with environmental degradation. The application effectively solves the problem that the positioning accuracy is difficult to maintain for a long time in the complex environment of the underground pipe network, and significantly improves the reliability and trajectory consistency of the inspection operation.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology for pipeline inspection equipment, and specifically discloses a high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion. Background Technology

[0002] With the continuous expansion of urban underground pipe networks, the use of pipe network inspection equipment equipped with various sensors for internal detection and maintenance has become the industry mainstream. However, since the underground pipe network completely shields GNSS signals and has the physical characteristics of being enclosed, narrow, and lacking light, the inspection equipment can only rely on internal sensors for trajectory calculation and local autonomous positioning, which brings great technical challenges to high-precision navigation and positioning in complex environments.

[0003] Existing pipeline inspection and positioning systems have revealed numerous insurmountable technical defects in actual operations: the bottom of the pipeline is often accompanied by water accumulation, silt, or slippery moss, and relying solely on wheeled odometers is prone to wheel slippage or spinning, resulting in serious distortion of speed increments and mileage estimation; in long, straight cylindrical pipelines, 3D environmental perception sensors, due to the lack of rich geometric variations, are prone to degradation during point cloud registration, leading to the failure of constraints along the pipeline direction; most existing fusion positioning solutions lack a strict microsecond-level hardware spatiotemporal synchronization mechanism between heterogeneous sensors, and usually only focus on front-end data recursion, failing to effectively extract key nodes unique to the pipeline and combine them with global prior maps for back-end optimization, resulting in sensor measurement noise and local estimation errors accumulating over time.

[0004] In summary, existing pipeline inspection and positioning equipment cannot effectively address issues such as sensor slippage failure, environmental feature degradation, and coarse multi-sensor fusion mechanisms. This leads to irreversible cumulative drift and heading angle divergence during long-distance, long-duration pipeline operations. Such severe trajectory distortion and coordinate drift not only significantly distort the collected pipeline defect location information but also fail to meet the urgent need for high-precision, robust global navigation and positioning in current digital underground space construction. Summary of the Invention

[0005] The purpose of this invention is to provide a high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion. This system first achieves microsecond-level hardware spatiotemporal synchronization of heterogeneous sensors by distributing pulse-second signals through a unified microcontroller. In the front-end fusion stage, an error-state Kalman filter model is used to perform high-frequency local state estimation of IMU pre-integrated data, wheel odometer speed increments, and pipeline circular surface and edge feature points finely extracted by 3D lidar. The core breakthrough of this invention lies in constructing a deep back-end factor graph optimization mechanism, which not only transforms the front-end high-frequency pose, local trajectory, and IMU data into constraint factors... Furthermore, the system creatively extracts key features of pipeline network nodes such as manhole openings and T-junctions, and uses a normal distribution transformation registration algorithm to accurately match with a pre-loaded global topology prior map to trigger closed-loop detection, forcibly eliminating the cumulative heading angle drift caused by long-term system operation. The system also proposes an adaptive adjustment logic for measurement weights, which quantifies the degree of environmental degradation and wheel slippage by calculating the eigenvalues ​​of the Jacobian matrix and the sensor speed difference in real time, and dynamically adjusts the values ​​of the diagonal elements of the measurement noise covariance matrix, thereby greatly improving the global navigation and positioning accuracy and system robustness in complex and extreme pipeline network environments.

[0006] The objective of this invention can be achieved through the following technical solutions: A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion includes the following modules: A multi-sensor data acquisition module is installed on the pipeline inspection equipment body to collect real-time data on the equipment's own motion status within the pipeline network and external environmental perception data. The multi-sensor data acquisition module includes an inertial measurement unit, a wheeled odometer, and a three-dimensional environmental perception sensor. The preprocessing and spatiotemporal synchronization module is communicatively connected to the multi-sensor data acquisition module. It is used to perform anomaly removal and noise reduction filtering on the acquired motion state data and environmental perception data, and to complete the timestamp alignment and spatial coordinate system calibration transformation between heterogeneous sensors based on a hardware triggering mechanism, and output the spatiotemporally synchronized observation data sequence. The front-end fusion and local state estimation module, connected to the preprocessing and spatiotemporal synchronization module, is used to input the observation data sequence into a preset filtering fusion model. The integral data of the inertial measurement unit is used as the system state prediction input, and the speed increment of the wheel odometer and the pipeline geometric feature points extracted by the three-dimensional environmental perception sensor are used as the measurement update input to calculate and obtain the front-end high-frequency pose and local motion trajectory of the pipeline inspection equipment in the local coordinate system. The backend optimization and high-precision positioning output module is connected to the frontend fusion and local state estimation module. It is used to construct a pose node graph from the high-frequency pose, local motion trajectory and extracted key node features of the pipeline network. Combined with the preloaded global topology prior map of the pipeline network, the factor graph optimization algorithm is used to minimize the global error of the pose node graph, eliminate the cumulative drift error of the local state estimation, and output the high-precision positioning information of the pipeline inspection equipment in the global coordinate system.

[0007] Preferably, in the front-end fusion and local state estimation module, the preset filtering fusion model is specifically an error state Kalman filter model; The front-end fusion and local state estimation module is specifically used for: During the state prediction phase, the continuous-time kinematic equations of the system are recursively derived using the angular velocity and linear acceleration of the inertial measurement unit, outputting the prior state containing nominal state variables and error state variables, and propagating and updating the error state covariance matrix. During the measurement update phase, the speed increment of the wheel odometer and the pipeline geometric feature points extracted by the three-dimensional environmental perception sensor are used as observation values ​​to calculate the Kalman gain. The Kalman gain is then used to correct the error state variable in order to update the nominal state variable, thereby outputting the front-end high-frequency pose.

[0008] Preferably, in the front-end fusion and local state estimation module, the integral data of the inertial measurement unit is specifically obtained through an IMU pre-integration algorithm: Between two adjacent observation frames output by the three-dimensional environment perception sensor, the high-frequency sampling data of the inertial measurement unit is continuously integrated to calculate the relative rotation change, relative translation change, and velocity change between the two frames. The relative rotation change, relative translation change, and velocity change are then used as pre-integrated observations to construct the high-frequency relative motion constraints of the pipeline inspection equipment in the local coordinate system.

[0009] Preferably, in the multi-sensor data acquisition module, the three-dimensional environment perception sensor is specifically limited to a three-dimensional lidar; The front-end fusion and local state estimation module extracts the geometric feature points of the pipeline network, specifically including: The raw point cloud data acquired by the three-dimensional lidar is subjected to voxel mesh filtering, and the local curvature and surface normal vector of each spatial point are calculated. Based on the distribution characteristics of the local curvature and surface normal vector, surface feature points characterizing the cylindrical pipe structure inside the pipeline network and edge feature points characterizing the joints of the inner wall of the pipeline network are extracted. The surface feature points and the edge feature points together constitute the geometric feature points of the pipeline network.

[0010] Preferably, in the preprocessing and spatiotemporal synchronization module, the hardware triggering mechanism specifically includes: Using a single microcontroller within the system as a unified clock source, pulse-second signals are synchronously sent to the inertial measurement unit, the wheeled odometer, and the 3D LiDAR at a preset fixed frequency. The underlying data acquisition drivers of the inertial measurement unit, the wheeled odometer, and the 3D LiDAR respond to the pulse-second signals, adding an absolute hardware timestamp based on the unified clock source to the acquired motion state data and environmental perception data, thereby achieving microsecond-level timestamp alignment between heterogeneous sensors.

[0011] Preferably, in the preprocessing and spatiotemporal synchronization module, the spatial coordinate system calibration transformation specifically includes: Based on the translation vector and rotation matrix of the external parameters between the inertial measurement unit, the wheel odometer, and the 3D lidar obtained through pre-offline calibration, the wheel odometer data and the 3D lidar point cloud data after the timestamp alignment are uniformly transformed into the body coordinate system of the inertial measurement unit to generate the observation data sequence in the same spatial coordinate system.

[0012] Preferably, in the back-end optimization and high-precision positioning output module, the specific process of using the factor graph optimization algorithm to minimize the global error of the pose node graph includes: The high-frequency pose of the pipeline inspection equipment at different times is defined as a variable node in the factor graph; An IMU pre-integration factor is constructed based on the integral data of the inertial measurement unit; an odometry relative pose factor is constructed based on the local motion trajectory in the high-frequency pose of the front end; and an environmental feature matching factor is constructed based on the geometric feature points of the pipeline network. The IMU pre-integration factor, the odometry relative pose factor, and the environmental feature matching factor are connected to the corresponding variable nodes to construct a target cost function. The target cost function is then iteratively solved using the Gauss-Newton nonlinear least squares algorithm to achieve smoothing and minimization of local state estimation errors.

[0013] Preferably, the key node features of the pipeline network extracted by the back-end optimization and high-precision positioning output module are specifically limited to manhole features and tee pipe features; The factor graph optimization algorithm also includes the process of constructing closed-loop detection factors: when the manhole features extracted at the current moment and the tee pipe features are successfully matched with the corresponding features recorded in the historical observation trajectory, it is determined that the system has triggered a closed loop. The closed-loop detection factors are established between the corresponding historical variable nodes and the current variable nodes in the factor graph to forcibly constrain and eliminate the cumulative heading angle drift generated by the system during long-term operation in the pipeline network.

[0014] Preferably, the specific process of solving the backend optimization and high-precision positioning output module by combining the preloaded global topology prior map of the pipeline network includes: The pipeline global topology prior map includes pre-established structural topology nodes in an absolute geographic coordinate system; The extracted manhole features and tee pipe features are geometrically registered with the structural topology nodes in the global topology prior map of the pipeline network using the normal distribution transformation registration algorithm. The absolute pose transformation matrix is ​​calculated and added as an absolute pose prior factor to the factor graph optimization algorithm to output high-precision positioning information aligned to the global coordinate system.

[0015] Preferably, the front-end fusion and local state estimation module also includes measurement weight adaptive adjustment logic: The front-end fusion and local state estimation module determines the degree of environmental feature degradation by calculating the eigenvalues ​​of the Jacobian matrix during the registration process of 3D environmental perception sensor data, and determines the degree of wheel slippage by calculating the difference between the output speed of the wheel odometer and the integral speed of the inertial measurement unit. Based on the degree of environmental feature degradation and the degree of wheel slippage, the module increases the diagonal elements of the measurement noise covariance matrix of the corresponding sensor in the error state Kalman filter model in real time in a proportional manner, thereby reducing the contribution rate of the degraded and slipping sensors to the system state update during the measurement update phase.

[0016] The beneficial effects of this invention are: This invention solves the problems of asynchronous data sampling and inconsistent spatial coordinate systems among heterogeneous multi-sensor systems by constructing a hardware-level spatiotemporal synchronization mechanism based on a unified clock source of a single microcontroller. This provides a high-quality observation sequence with absolute alignment for multi-source data fusion. At the same time, by combining the error state Kalman filter model and the IMU pre-integration algorithm, it can give full play to the measurement advantages of each sensor in different frequency bands. This effectively ensures that pipeline inspection equipment can still output continuous, smooth and high-frequency local high-precision pose information in a claustrophobic and complex environment with no GNSS signal, greatly consolidating the reliability foundation of front-end trajectory estimation.

[0017] This invention introduces a back-end factor graph optimization framework based on the unique key node features of the pipeline network and a global topological prior map, completely breaking the technical bottleneck that traditional pure continuous calculations inevitably lead to cumulative error divergence. By accurately extracting structural features such as manhole openings and T-junctions for geometric registration and closed-loop detection, the system can automatically trigger global constraints during long-distance and long-duration inspection operations, forcibly correct heading angle drift, and align local trajectories to the absolute geographic coordinate system, thereby ensuring the absolute accuracy of equipment global positioning and the high consistency of the inspection trajectory throughout the entire process.

[0018] This invention proposes an adaptive adjustment logic for measurement weights, which endows the positioning system with strong fault tolerance and dynamic correction capabilities in the face of extremely harsh pipeline network environments. By monitoring the degree of degradation of environmental geometric features and abnormal wheel slippage of the wheel odometer during the three-dimensional point cloud registration process in real time, the system can dynamically and accurately reduce the interference of poor or failed observation data on the overall state estimation. This effectively avoids the collapse of the overall positioning system or sudden trajectory changes due to the partial failure of a single sensor, and greatly improves the robustness and continuous safe operation capability of the system in harsh working conditions such as slippery mud and long straight pipelines lacking features. Attached Figure Description

[0019] Figure 1 This is a schematic diagram of the structure of a high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion according to the present invention; Figure 2 This is a hardware-triggered spatiotemporal synchronization logic diagram for the present invention; Figure 3 This is a flowchart of the feature extraction and processing of the present invention; Figure 4 This is the measurement weight adaptive adjustment logic diagram of the present invention. Detailed Implementation

[0020] The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0021] Example: Figures 1-4 As shown, a high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion is presented. This system is mainly mounted on the pipeline robot equipment body. The system is divided into four core modules in terms of logical architecture and data flow: multi-sensor data acquisition module, preprocessing and spatiotemporal synchronization module, front-end fusion and local state estimation module, and back-end optimization and high-precision positioning output module.

[0022] The multi-sensor data acquisition module is physically mounted on the pipeline robot chassis and is mainly used for real-time and continuous acquisition of the device's kinematic parameters within the pipeline network and three-dimensional perception data of the external spatial environment. In this embodiment, the module specifically includes the following heterogeneous sensor group: Inertial Measurement Unit (IMU): Employing a six-degree-of-freedom industrial-grade microelectromechanical system (MEMS), it is rigidly mounted at the center of mass of the pipeline inspection equipment. It operates at an extremely high sampling frequency, typically... It outputs a sequence of three-axis acceleration measurements and a sequence of three-axis angular velocity measurements.

[0023] Wheel-type odometer: Installed on the main drive wheel shaft of the inspection equipment, it records the number of rotation pulses of the wheel in real time through a high-resolution photoelectric encoder at a fixed frequency. Calculate and output the incremental data of the forward linear velocity and angular velocity of the device in the chassis coordinate system.

[0024] 3D environmental perception sensor: In this embodiment, it is specifically defined as a mechanically rotating 3D LiDAR with a horizontal field of view of 360° and a vertical field of view including multiple laser beams. It is fixed to the top of the inspection equipment and operates at a set scanning frame rate, such as... A laser beam is emitted toward the inner wall of the pipeline network, and dense spatial three-dimensional point cloud data is obtained through the time-of-flight ranging principle.

[0025] The primary challenge in locating underground pipeline networks is the asynchronous data transmission from multiple heterogeneous sensors. This module, based on a hardware triggering mechanism and a rigorous calibration model, completes timestamp alignment and unified spatial coordinate system transformation.

[0026] Hardware-level microsecond-level time synchronization mechanism: This system abandons the traditional operating system-based software timestamp method and adopts hardware synchronization logic based on a single microcontroller as the system's "globally unified clock source." Specifically, the high-precision timer inside the MCU operates at a preset fixed frequency that is completely consistent with the scanning frame rate of the 3D LiDAR (i.e.,... It synchronously sends pulse-second signals to the external hardware trigger pins of the IMU, wheel odometer, and 3D LiDAR.

[0027] When the underlying analog-to-digital converters or data acquisition drivers of each sensor capture the rising edge of the PPS signal, they immediately generate a high-priority hardware interrupt, forcibly latching the physical motion state data and environmental perception data acquired at that instant. The MCU then appends an absolute hardware timestamp based on its internal clock to this latched data. This enables the physical-level microsecond-level time alignment of previously independently sampled asynchronous data sequences.

[0028] Rigid body transformation calibration of spatial coordinate system: In order to fuse multi-source data, they must be mapped to the same reference system. This system defines three independent Cartesian coordinate systems: Inertial navigation body coordinate system System: The origin is located at the IMU measurement center.

[0029] Radar coordinate system L: The origin is located at the optical scanning center of the three-dimensional lidar.

[0030] O-frame coordinate system of the odometer chassis: The origin is located at the center point of the two rear drive shafts of the inspection equipment.

[0031] The system is based on The system serves as a global reference benchmark for local state estimation. Through an offline hand-eye calibration algorithm, the extrinsic parameter matrix of the radar relative to the IMU is pre-solved, including: the rotation matrix. , indicating from L series to The three-dimensional rotation and translation vector of the system , indicating that the origin of the L series is at The three-dimensional coordinates under the system. Similarly, the extrinsic parameters of the odometer relative to the IMU are calibrated: the rotation matrix. Translation vector .

[0032] In actual operation, for Any three-dimensional spatial environment point acquired in the radar coordinate system at any time Convert it to The mathematical equation under the system is: .

[0033] for The forward velocity vector of the chassis measured by the wheel-mounted odometer in the O-frame. Convert it to The mathematical equation under the system is: in, These are the coordinates of the point cloud in the I-system after transformation; This represents the chassis linear velocity in the I-series after conversion. This represents the three-axis angular velocity vector measured in real time by the IMU at the current moment; the symbol × indicates the cross product operation of the three-dimensional vector, i.e., compensating for the tangential velocity caused by the lever arm effect. After coordinate transformation, the output is a sequence of observation data under the same spatiotemporal reference.

[0034] The front-end fusion and local state estimation module is the core of realizing high-frequency trajectory extrapolation in a GNSS-free environment. The key is to use the error state Kalman filter model to process IMU pre-integration, point cloud feature extraction, and adaptive weight allocation of sensor measurements.

[0035] The large amount of redundant point cloud in long, straight underground pipe networks not only consumes computational resources but also easily leads to mismatches. This system first utilizes a voxel grid filter to... Spatial downsampling is performed on the ensemble. Subsequently, principal component analysis is used to calculate the center point of each voxel. and its local neighborhood point set Local curvature , It contains N nearest neighbors. Its definition is based on the eigenvalues ​​of the covariance matrix of the neighborhood point set. Let the eigenvalues ​​of the covariance matrix be arranged in ascending order as follows: Then the formula for calculating the local curvature is: .

[0036] The system sets an empirical curvature determination threshold. : when When the local area is extremely flat, the system extracts it as surface feature points. In the pipeline network, these points represent the inner wall surface of the cylindrical pipe over a large area.

[0037] when When this occurs, it indicates a sudden change in curvature in the region. The system extracts these points as edge feature points, which accurately characterize pipe joint gaps, flange edges, or obstacle outlines in the pipeline network.

[0038] To constrain and align high-frequency IMU data with low-frequency radar frame data on the time axis without re-integrating the IMU data after each pose update, the system introduces IMU pre-integration theory. Assuming the system is in continuous operation, at two adjacent radar keyframe moments... and In between, the IMU generated a series of high-frequency sampling moments. ,satisfy .exist At time t, the angular velocity reading measured by the IMU is defined as The linear acceleration reading is defined as Considering the sensor's own zero bias and Gaussian white noise, the true angular velocity... and real linear acceleration They are respectively: , in, and The gyroscope and accelerometer are respectively located in The zero-bias vector of the random walk at time step; and This is the corresponding Gaussian white noise vector.

[0039] exist arrive time span Internally, constructing with Pre-integrated observations based on the body coordinate system at any given time. Includes relative rotational changes. Relative velocity change With relative translation change The recursive formula in discrete time is: , , ,in, Represents the Lie algebra ( ) to Li Qun ( The Rodriguez exponential mapping function; The minimum time interval between adjacent sampling data points of the IMU; during the integration period, assume a zero bias constant, i.e. and The above three sets of data constitute the high-frequency relative motion constraints of the pipeline inspection equipment in the local coordinate system.

[0040] The error-state Kalman filter model separates the system's total state into nominal and error states. By filtering, it specifically estimates small error states, thus avoiding nonlinear singularities caused by large variations in attitude angles. The true state vector is defined. Nominal state vector and error state vector Its dimensions are set to 18: ,in For translation error, For speed error, This refers to the attitude angle error. and This is the zero-bias error vector.

[0041] State prediction phase: Using the current input from the IMU, update the nominal state according to the continuous-time kinematic differential equation. Simultaneously, based on the state-space equation of the error state, the prior covariance matrix of the error state is updated. : in, Let be the error state transition matrix composed of the partial derivatives of the kinematic model; Let be the posterior covariance matrix of the previous time step; The noise-driven Jacobian matrix; Let be the system process noise covariance matrix.

[0042] Measurement weight adaptive adjustment logic: Before introducing odometer and radar feature points into the measurement update, the system performs degradation and slip detection in real time: For 3D LiDAR, it calculates the measurement Jacobian matrix when the currently extracted surface feature points / edge feature points are registered with the previous frame or local sub-map. Construct the registration information matrix and approximate the Hessian matrix. .right Perform singular value decomposition to extract its 6 eigenvalue sets. If the smallest eigenvalue in the set Less than the preset environmental degradation threshold The radar was determined to have experienced environmental degradation in a featureless straight pipe section. For wheeled odometers, the forward velocity after the coordinate transformation was calculated. Forward velocity derived from pure IMU integration absolute difference .like Greater than the preset slip threshold If the chassis is found to have wheel slippage in the silt of the pipeline network, it is determined that the chassis has experienced wheel slippage. If degradation is determined, the system introduces an exponential penalty coefficient. For radar-specific measurement noise covariance matrix Dynamically enlarge diagonal elements: Similarly, if slippage is detected, a penalty coefficient is introduced. Amplified odometer measurement noise covariance matrix: ,in To adjust the gain constant, the values ​​of the diagonal elements of the noise matrix are dynamically increased. Mathematically, this directly reduces the contribution weight of malfunctioning sensors during final data fusion, thus ensuring the robustness of the system.

[0043] Measurement update phase: Constructing the joint observation matrix With joint observation noise covariance matrix Calculate the Kalman gain matrix : Calculate the posterior error state vector: ,in This is the actual observation vector, including the odometer velocity increment and radar registration residual. This is a nonlinear observation function. Finally, the calculated error state is injected into the nominal state for error correction: ,symbol This represents the generalized state addition in the Lie manifold space, followed by resetting the error state. The zero vector is used to output the corrected high-frequency pose of the front end.

[0044] To completely eliminate the cumulative error caused by continuous calculations from local states, the system introduces factor graph optimization theory in the backend and combines it with the pipeline topology prior map to achieve global absolute coordinate alignment.

[0045] The system abstracts the high-frequency pose of the inspection equipment at different keyframe moments into variable nodes in a factor graph. Let the factor graph contain a set of state nodes. Based on sensor observations, various constraint factors are constructed among these nodes: IMU pre-integration factor Constraining adjacent nodes and Its residual vector The formula for calculating the position components is: ,in, The pose and velocity states to be optimized; It is the gravity vector; This refers to the pre-integral quantity of the aforementioned IMU.

[0046] Odometer relative pose factor : Generates a relative displacement residual .

[0047] Environmental feature matching factor Generates geometrically constrained residuals for point-to-line / point-to-surface distances. .

[0048] Key node feature extraction and global topology constraints: The system specifically extracts key node features of underground pipe networks based on their fixed geometric structure, which are defined as: manhole features that are cylindrical cavities extending to the ground, and tee pipe features that are cylindrical surfaces of different diameters intersecting perpendicularly.

[0049] Closed-loop detection factor When the manhole / te-gate features extracted at the current moment, determined based on the spatial geometric descriptor, highly match a node previously scanned, a closed-loop mechanism is triggered. The system establishes connection edges between the current node and historical distant nodes in the factor graph, generating closed-loop residuals. Forced constraint on heading angle drift.

[0050] Pipeline network global topology prior map matching: The system preloads a global topology map stored in absolute geographic coordinates. When equipment travels to key nodes such as manholes, the absolute pose is obtained using a normal distribution transformation registration algorithm. The NDT algorithm divides the global topology point cloud into a three-dimensional grid and calculates the mean vector of the point set within each grid. With covariance matrix By maximizing the current extracted feature point cloud Based on the Gaussian probability density distribution in the target mesh, construct the objective function and obtain the absolute pose transformation matrix. : ,in Let be the transformed point. The obtained absolute pose transformation matrix is... Converted to absolute position and absolute attitude, serving as absolute pose prior factors. Added to the factor plot, it generates absolute coordinate residuals. .

[0051] Nonlinear solution for minimizing global error: By simultaneously solving all the residual factors mentioned above, a nonlinear least-squares objective cost function based on Mahalanobis distance is constructed. : in, Represents the weighted matrix The constrained square norm, where each information matrix This is equal to the inverse of the corresponding sensor observation noise covariance matrix. To obtain the target cost function... The optimal set of state variables that achieves the global minimum The system employs a Gauss-Newton iterative algorithm for optimization. First, for all residual functions... Perform a first-order Taylor series expansion at the current state iteration point to calculate the system-level Jacobian matrix of each residual with respect to the set of state variables. Next, a linearized incremental equation based on the Gauss-Newton method is constructed: Let the large sparse information matrix on the left side of the equation be... The bias vector on the right is Then the equation simplifies to: .because The matrix has a naturally blocky and sparse structure. The system uses the Schur complement elimination technique and the Cholesky matrix factorization algorithm to accelerate the solution of the equation, and calculates the state increment vector containing position and attitude corrections. Finally, the increment is updated in the original variable node, i.e., execution is performed. Repeat the linearization, solution, and update steps described above until... When the norm approaches a minimum threshold, the algorithm converges. This optimization process, from a global perspective, not only fully smooths and eliminates the cumulative drift error caused by front-end filtering, but also strictly binds and aligns the three-dimensional motion trajectory of the inspection equipment to the absolute geographic coordinate system of the pipeline network, ultimately outputting high-precision positioning information of the pipeline inspection equipment in real time and stably.

[0052] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0053] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion, characterized in that, Includes the following modules: A multi-sensor data acquisition module is installed on the pipeline inspection equipment body to collect real-time data on the equipment's own motion status within the pipeline network and external environmental perception data. The multi-sensor data acquisition module includes an inertial measurement unit, a wheeled odometer, and a three-dimensional environmental perception sensor. The preprocessing and spatiotemporal synchronization module is communicatively connected to the multi-sensor data acquisition module. It is used to perform anomaly removal and noise reduction filtering on the acquired motion state data and environmental perception data, and to complete the timestamp alignment and spatial coordinate system calibration transformation between heterogeneous sensors based on a hardware triggering mechanism, and output the spatiotemporally synchronized observation data sequence. The front-end fusion and local state estimation module, connected to the preprocessing and spatiotemporal synchronization module, is used to input the observation data sequence into a preset filtering fusion model. The integral data of the inertial measurement unit is used as the system state prediction input, and the speed increment of the wheel odometer and the pipeline geometric feature points extracted by the three-dimensional environmental perception sensor are used as the measurement update input to calculate and obtain the front-end high-frequency pose and local motion trajectory of the pipeline inspection equipment in the local coordinate system. The backend optimization and high-precision positioning output module is connected to the frontend fusion and local state estimation module. It is used to construct a pose node graph from the high-frequency pose, local motion trajectory and extracted key node features of the pipeline network. Combined with the preloaded global topology prior map of the pipeline network, the factor graph optimization algorithm is used to minimize the global error of the pose node graph, eliminate the cumulative drift error of the local state estimation, and output the high-precision positioning information of the pipeline inspection equipment in the global coordinate system.

2. The high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion according to claim 1, characterized in that, In the front-end fusion and local state estimation module, the preset filtering fusion model is specifically an error state Kalman filter model; The front-end fusion and local state estimation module is specifically used for: During the state prediction phase, the continuous-time kinematic equations of the system are recursively derived using the angular velocity and linear acceleration of the inertial measurement unit, outputting the prior state containing nominal state variables and error state variables, and propagating and updating the error state covariance matrix. During the measurement update phase, the speed increment of the wheel odometer and the pipeline geometric feature points extracted by the three-dimensional environmental perception sensor are used as observation values ​​to calculate the Kalman gain. The Kalman gain is then used to correct the error state variable in order to update the nominal state variable, thereby outputting the front-end high-frequency pose.

3. The high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion according to claim 2, characterized in that, In the front-end fusion and local state estimation module, the integral data of the inertial measurement unit is specifically obtained through the IMU pre-integration algorithm: Between two adjacent observation frames output by the three-dimensional environment perception sensor, the high-frequency sampling data of the inertial measurement unit is continuously integrated to calculate the relative rotation change, relative translation change, and velocity change between the two frames. The relative rotation change, relative translation change, and velocity change are then used as pre-integrated observations to construct the high-frequency relative motion constraints of the pipeline inspection equipment in the local coordinate system.

4. The high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion according to claim 1, characterized in that, In the multi-sensor data acquisition module, the three-dimensional environment perception sensor is specifically defined as a three-dimensional lidar; The front-end fusion and local state estimation module extracts the geometric feature points of the pipeline network, specifically including: The raw point cloud data acquired by the three-dimensional lidar is subjected to voxel mesh filtering, and the local curvature and surface normal vector of each spatial point are calculated. Based on the distribution characteristics of the local curvature and surface normal vector, surface feature points characterizing the cylindrical pipe structure inside the pipeline network and edge feature points characterizing the joints of the inner wall of the pipeline network are extracted. The surface feature points and the edge feature points together constitute the geometric feature points of the pipeline network.

5. A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion as described in claim 4, characterized in that, In the preprocessing and spatiotemporal synchronization module, the hardware triggering mechanism specifically includes: Using a single microcontroller within the system as a unified clock source, pulse-second signals are synchronously sent to the inertial measurement unit, the wheeled odometer, and the 3D LiDAR at a preset fixed frequency. The underlying data acquisition drivers of the inertial measurement unit, the wheeled odometer, and the 3D LiDAR respond to the pulse-second signals, adding an absolute hardware timestamp based on the unified clock source to the acquired motion state data and environmental perception data, thereby achieving microsecond-level timestamp alignment between heterogeneous sensors.

6. A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion according to claim 4, characterized in that, In the preprocessing and spatiotemporal synchronization module, the spatial coordinate system calibration transformation specifically includes: Based on the translation vector and rotation matrix of the external parameters between the inertial measurement unit, the wheel odometer, and the 3D lidar obtained through pre-offline calibration, the wheel odometer data and the 3D lidar point cloud data after the timestamp alignment are uniformly transformed into the body coordinate system of the inertial measurement unit to generate the observation data sequence in the same spatial coordinate system.

7. A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion as described in claim 1, characterized in that, In the back-end optimization and high-precision positioning output module, the specific process of using the factor graph optimization algorithm to minimize the global error of the pose node graph includes: The high-frequency pose of the pipeline inspection equipment at different times is defined as a variable node in the factor graph; An IMU pre-integration factor is constructed based on the integral data of the inertial measurement unit; an odometry relative pose factor is constructed based on the local motion trajectory in the high-frequency pose of the front end; and an environmental feature matching factor is constructed based on the geometric feature points of the pipeline network. The IMU pre-integration factor, the odometry relative pose factor, and the environmental feature matching factor are connected to the corresponding variable nodes to construct a target cost function. The target cost function is then iteratively solved using the Gauss-Newton nonlinear least squares algorithm to achieve smoothing and minimization of local state estimation errors.

8. A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion as described in claim 7, characterized in that, The key node features of the pipeline network extracted by the back-end optimization and high-precision positioning output module are specifically limited to manhole features and tee pipe features. The factor graph optimization algorithm also includes the process of constructing closed-loop detection factors: when the manhole features extracted at the current moment and the tee pipe features are successfully matched with the corresponding features recorded in the historical observation trajectory, it is determined that the system has triggered a closed loop. The closed-loop detection factors are established between the corresponding historical variable nodes and the current variable nodes in the factor graph to forcibly constrain and eliminate the cumulative heading angle drift generated by the system during long-term operation in the pipeline network.

9. A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion as described in claim 8, characterized in that, The specific process of solving the backend optimization and high-precision positioning output module by combining the preloaded global topology prior map of the pipeline network includes: The pipeline global topology prior map includes pre-established structural topology nodes in an absolute geographic coordinate system; The extracted manhole features and tee pipe features are geometrically registered with the structural topology nodes in the global topology prior map of the pipeline network using the normal distribution transformation registration algorithm. The absolute pose transformation matrix is ​​calculated and added as an absolute pose prior factor to the factor graph optimization algorithm to output high-precision positioning information aligned to the global coordinate system.

10. A high-precision positioning system for pipeline inspection equipment based on multi-sensor fusion according to claim 2, characterized in that, The front-end fusion and local state estimation module also includes measurement weight adaptive adjustment logic: The front-end fusion and local state estimation module determines the degree of environmental feature degradation by calculating the eigenvalues ​​of the Jacobian matrix during the registration process of 3D environmental perception sensor data, and determines the degree of wheel slippage by calculating the difference between the output speed of the wheel odometer and the integral speed of the inertial measurement unit. Based on the degree of environmental feature degradation and the degree of wheel slippage, the module increases the diagonal elements of the measurement noise covariance matrix of the corresponding sensor in the error state Kalman filter model in real time in a proportional manner, thereby reducing the contribution rate of the degraded and slipping sensors to the system state update during the measurement update phase.

Images