A simultaneous localization and mapping method for underwater pipeline inspection

Abstract

The application provides a simultaneous localization and mapping method for underwater pipeline detection, and belongs to the technical field of underwater robot navigation and positioning; aiming at the problems of limited light, strong echo interference and high structure repeatability in the seawater pipeline environment, a tightly coupled pose estimation framework based on factor graph optimization is proposed by fusing a two-dimensional imaging sonar, a Doppler velocity log (DVL) and an inertial measurement unit (IMU). Through joint constraint and optimization of multi-source sensing information, the method can realize high-precision positioning and continuous mapping of the ROV in an environment with extremely low visibility and complex pipeline structure, effectively improving the autonomy and data reliability of the underwater inspection process. The method has clear structure, simple implementation and high calculation efficiency, can stably run on a conventional ROS platform, and is suitable for various types of underwater pipeline detection and inspection tasks.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of underwater robot navigation and positioning, and particularly relates to a simultaneous localization and mapping method for underwater pipeline detection. BACKGROUND

[0002] Underwater pipeline inspection is extremely complex due to factors such as narrow space, insufficient light, and turbid water. Traditional manual inspection methods not only need to suspend pipeline water supply, resulting in equipment interruption and high cost, but also have operation risks. In contrast, using ROV (underwater robot) to replace manual detection inside the pipeline is a more efficient and safe solution. However, the internal structure of the pipeline is often complex and variable, and external positioning systems such as GPS cannot be used underwater, making the autonomous navigation and positioning of ROV extremely challenging. Therefore, how to achieve high-precision navigation and positioning without external positioning conditions has become a key problem in underwater pipeline inspection technology.

[0003] SLAM (Simultaneous Localization and Mapping) is a key technology that enables mobile robots to complete localization and mapping simultaneously in unknown environments through their own sensors. This technology does not require external positioning systems such as GPS to achieve autonomous navigation, environmental perception, and path planning, and has wide application value in fields such as autonomous driving, mobile robots, underwater exploration, and industrial inspection.

[0004] However, traditional SLAM methods face significant limitations in underwater pipeline environments. On the one hand, the narrow space, weak light, and turbid water of the pipeline make it difficult for vision-based SLAM algorithms such as ORB-SLAM, VINS-Mono, etc. to extract stable feature points, or even completely fail. On the other hand, the highly repetitive internal structure of the pipeline, smooth surface, and lack of obvious texture make it difficult for SLAM systems based on geometric matching or laser scanning to distinguish pose loops, leading to drift or incorrect registration. In addition, underwater sonar images are greatly affected by multi-path echoes and scattering noise, resulting in blurred and discontinuous echo features, further increasing the difficulty of feature extraction and registration. Therefore, existing SLAM systems are difficult to ensure long-term stable operation in such low-visibility, high-noise, and strong-echo environments. SUMMARY

[0005] To solve the above problems, the application provides a simultaneous localization and mapping method for underwater pipeline detection, comprising the following steps:

[0006] S1, based on the two-dimensional imaging sonar, Doppler velocity log (DVL), and inertial measurement unit (IMU) arranged on the ROV body, raw echo images, velocity information, and direction information are obtained respectively;

[0007] S2, pre-processing the obtained original echo image, suppressing multi-path echo, and then correcting the sonar polar coordinate projection error according to the obtained speed and direction information, and mapping into a Cartesian sonar mapping image;

[0008] S3, extracting feature points based on the Cartesian sonar mapping image, establishing a candidate matching set using adjacent key frame templates, and screening out false matches based on cylindrical geometric constraints and structural repeatability detection rules;

[0009] S4, inputting the candidate matching set output by S3 into a registration module, performing coarse registration to obtain an initial pose estimation, and then performing fine registration to realize registration residual self-calibration; simultaneously, calling the speed and direction information of DVL and IMU, constructing a multi-element fusion constraint and forming a factor graph;

[0010] S5, based on the factor graph obtained in S4, performing incremental optimization under a sliding window optimization strategy; when detecting that the appearance similarity is higher than a threshold value and the point cloud registration algorithm geometric residual is lower than a set value within the window, automatically injecting a loop factor and triggering local to global re-optimization to obtain a key frame pose;

[0011] S6, according to the key frame pose obtained in S5, performing coordinate unification and occupancy probability fusion on the point cloud data; constructing a confidence weight field in the overlapping area, adjusting the contribution degree of each frame, and generating a globally consistent motion trajectory and point cloud map.

[0012] Preferably, the pre-processing process in S2 comprises:

[0013] First, the original sonar echo image from S1 is subjected to near-field rejection and beam direction intensity normalization, and then two-dimensional discrete wavelet transform is used to suppress high-frequency noise, and the wavelet transform coefficients satisfy:

[0014]

[0015] wherein, represents an image signal, is a wavelet basis function, is a scale parameter, is a spatial index.

[0016] Preferably, the geometric consistency correction of the sonar polar coordinate projection error in S2 is as follows:

[0017] Firstly, the angular velocity and linear acceleration of the IMU are used to estimate the attitude change between adjacent frames, and then the displacement is calculated by combining the velocity information measured by the DVL, so as to obtain the predicted pose transformation between sonar frames; by comparing the predicted pose with the actual projection position of the sonar image, the displacement and attitude deviation are obtained, and the sonar projection point is geometrically compensated according to the deviation, and the calculation formula is modified as:

[0018]

[0019] wherein, is the sonar projection point coordinate, is the displacement deviation of the DVL and IMU predicted coordinates and the projection point coordinate, is the attitude rotation matrix, is the modified projection point coordinate.

[0020] Preferably, the feature point extraction based on the Cartesian sonar mapping image in S3 is specifically:

[0021] Firstly, the gradient amplitude of the Cartesian sonar mapping image is calculated, and then the threshold screening method is used to extract the feature points, and the threshold range is 0.05-0.2; the extracted feature points are re-projected in polar coordinates according to the pipeline axial direction to generate a pipeline geometry template, so as to reduce the feature drift caused by the change of viewing angle:

[0022]

[0023] wherein, is the Cartesian sonar mapping image, , is the image coordinate, is the gradient amplitude; in the local maximum value detection, only the pixel points higher than the set threshold are reserved as the feature points.

[0024] Preferably, in S3, when the false matches are screened out, the cylindrical geometry constraint is used to match the feature templates of adjacent key frames, and when the radial distance difference and the azimuth angle difference meet the following conditions, it is considered that the matching is valid:

[0025]

[0026] wherein, and represent the radial distance of the feature points of the adjacent key frames, and represent the corresponding azimuth angle, represents the radius tolerance, represents the azimuth angle tolerance; other matches are invalid matches and are deleted.

[0027] ​Preferably, in the S4, the registration process adopts a two-stage optimization strategy, first uses the least squares method to calculate the initial rigid body transformation pose, and then executes a point-to-plane iterative registration algorithm, and the error energy function is:

[0028]

[0029] wherein, is the source point, is the target point, is the normal vector of the target point, is the rotation matrix, is the translation vector, is the point-to-plane registration error energy; the weight matrix is updated according to the residual size after each iteration to realize the residual-driven self-calibration, and the registration residual and covariance information are output.

[0030] Preferably, in the S4, a factor graph is constructed, specifically:

[0031] In a unified coordinate system, the IMU pre-integration factor, the DVL velocity factor and the sonar registration factor are constructed into a joint observation equation, the IMU pre-integration factor is generated by the IMU observation accumulation in S1, providing short-time attitude and velocity prior; the DVL velocity factor is projected to the same coordinate system by the DVL observation in S1, constraining the translational velocity; the sonar registration factor is composed of the residual and the pose estimation, and the sonar registration factor is a constraint term constructed by the residual calculated in the S4 registration process; the three are jointly fused and the influence of different sensor noise covariances is balanced through weight normalization.

[0032] Preferably, in the S5, incremental optimization is performed under a sliding window optimization strategy, and the specific process is:

[0033] Based on the factor graph model constructed in S4, incremental optimization is performed within a sliding window, and the window size is dynamically adjusted according to the average residual and the covariance spectral radius of the last period, the window is expanded when the residual is higher than the threshold, and the window is contracted when the residual is lower than the threshold, so as to balance the requirements of real-time and accuracy; the optimization output includes key frame pose trajectory and marginalized factors.

[0034] Preferably, in the S5, loop closure factors are automatically injected and local-to-global re-optimization is triggered, and the specific process is:

[0035] An appearance and geometry dual mechanism is adopted, first, the appearance similarity is calculated based on the polar intensity histogram of the key frame, and when the cosine similarity is higher than 0.75, it is marked as a candidate loop closure; then, a point-to-plane iterative registration verification is performed, and when the root mean square error is less than 0.2m, the loop closure is confirmed to be established, the system injects the loop closure factor into the factor graph and triggers the global re-optimization to eliminate the cumulative error.

[0036] Preferably, the S6 specific process is:

[0037] The confidence weighted fusion strategy is adopted for unified coordinate conversion and occupancy probability fusion of the key frame point cloud, and the confidence weights of different frames The pose covariance matrix Determination, the calculation formula is:

[0038]

[0039] Among them, The confidence weight of the i-th key frame point cloud, The pose covariance matrix of the frame, The normalized coefficient; The weighted fusion result is subjected to consistency constraint and confidence smoothing in the overlapping area, and finally a globally consistent motion trajectory and point cloud map are generated.

[0040] Compared with the prior art, the present application has the following beneficial effects:

[0041] The present application proposes a high-robustness simultaneous localization and mapping method and system based on an ROV platform for the problems of limited light, strong acoustic interference and repeated geometric features in underwater pipeline detection. The method realizes joint constraint and dynamic optimization of multi-source observation information under a unified factor graph optimization framework, thereby maintaining high-precision positioning and continuous mapping capability in low visibility and complex acoustic environment. The system as a whole has good real-time performance and stability, and can realize autonomous navigation of the ROV without relying on optical imaging.

[0042] Through multiple sets of experiments, the present application shows excellent stability and precision under various working conditions such as long-distance inspection, complex water flow and sensor interference. Compared with existing single sonar or weakly coupled methods, the system can significantly reduce cumulative drift and enhance the continuity of attitude estimation and the consistency of the map. Especially when the sonar noise is large or the DVL measurement fluctuates, the system can still run smoothly, showing strong anti-interference ability and environmental adaptability.

[0043] The method of the present application has clear structure, simple implementation, high calculation efficiency, and can stably run on a conventional ROS platform, and is suitable for various types of underwater pipeline detection and inspection tasks. It has significant advantages in improving inspection accuracy, reducing manual intervention and enhancing task continuity, and provides a feasible and highly reliable solution for navigation implementation of automatic detection in complex underwater environments. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 It is the overall method logic diagram of the present application.

[0045] Figure 2The schematic diagram of the sonar signal collection and preprocessing process of the application.

[0046] Figure 3 The theoretical diagram of the sliding window optimization process of the application.

[0047] Figure 4 The schematic diagram of generating point cloud map and trajectory of the application, and the experimental scene is a pool.

[0048] Figure 5 The schematic diagram of generating point cloud map and trajectory of the application, and the experimental scene is a pipeline. DETAILED DESCRIPTION

[0049] The application will be further described below in combination with specific embodiments.

[0050] 1. Overall process

[0051] In order to realize high-precision autonomous positioning and mapping of the underwater pipeline ROV, the application designs a tightly coupled SLAM system based on a two-dimensional imaging sonar and DVL / IMU. The system takes a two-dimensional imaging sonar as the main sensor, supplemented by DVL to provide relative speed information and IMU to provide high-frequency inertial measurement, and performs joint state estimation on multi-source data through factor graph optimization. As shown in Figure 1 The logic diagram of the overall SLAM system of the application.

[0052] The system first synchronizes and preprocesses the sensor data. Then, significant feature points are extracted from the sonar image to establish the matching relationship between images and form the inter-frame pose constraint. At the same time, the IMU data is pre-integrated to generate inertial factors, and the DVL velocity observation is constructed to form velocity factors, both of which are included in the factor graph. Finally, a sliding window optimization strategy is adopted to incrementally estimate the trajectory and point cloud map of the ROV, ensuring the real-time performance and stability of the system.

[0053] 2. Sonar image processing

[0054] The sonar image in the underwater pipeline environment shows obvious structural features, especially two significant band-shaped reflection signals near the center of the image, representing the echo of the pipeline wall. At the same time, other areas of the image contain a large amount of echo noise, mainly caused by water clutter, multipath reflection and bubble interference. In order to effectively extract the structural features of the pipeline, the application designs a preprocessing and feature point extraction process for this environment. As shown in Figure 2 The schematic diagram of the sonar signal collection and preprocessing process of the application.

[0055] Firstly, the original sonar image in polar coordinates is preprocessed. Considering that the pixels within 0.8 m from the sonar sensor usually have strong near-field interference, and low-intensity noise will affect the subsequent detection, the pixels with a distance less than 0.8 m or an intensity lower than 5% of the maximum value are set to zero to remove direct interference and invalid regions.

[0056] Then, to suppress echo noise, the average intensity of each sonar beam (each column of the polar coordinate image) is calculated, and the pixels with an intensity lower than the average value are set to zero, retaining only the strong signal region, which greatly improves the signal-to-noise ratio.

[0057] Subsequently, the polar coordinate image is denoised using Discrete Wavelet Transform (DWT) at multiple scales, further reducing high-frequency noise while preserving edge and structural information. After denoising, the polar coordinate image is converted to Cartesian coordinates for subsequent feature extraction.

[0058] In the denoised Cartesian image, pixels with an intensity lower than 30% of the maximum value are considered invalid and removed. Morphological closing operations are applied to the two band-shaped tube wall signal regions to fill in cracks and holes, ensuring the continuity of the features:

[0059]

[0060] where, and represent the dilation and erosion operations, is the Cartesian sonar image, is the closing operation result, is the structural element.

[0061] The sonar image processed by this strategy and its corresponding feature point extraction results. It can be observed that the echo noise is greatly suppressed, and the number of extracted feature points is significantly increased.

[0062] Next, feature point extraction is performed based on gradient magnitude and local maximum detection. The gradient magnitude is defined as follows:

[0063]

[0064] where, is the result of the previous morphological closing operation, and are the image coordinates, is the gradient magnitude. Set the gradient threshold where the local maximum point is selected as a feature point that needs to meet the following conditions:

[0065]

[0066] where, is the set of feature points, is the pixel coordinate, is the gradient magnitude, is the gradient threshold.

[0067] Finally, the extracted feature points are converted to a 2D point cloud in the robot coordinate system. Isolated outliers are removed based on local density statistics to ensure the stability and accuracy of the point cloud. This method effectively improves the number and positioning accuracy of sonar features in the underwater pipeline environment, making it suitable for subsequent registration and positioning tasks.

[0068] 3. Feature point matching

[0069] In the pipeline environment, sonar point clouds exhibit highly repetitive geometric features, typically in the form of parallel curves representing the left and right walls of the corresponding pipeline. While this regularity helps to extract stable geometric information, it can also lead to confusion between the two walls, affecting the reliability of registration. To overcome this problem, the invention proposes a strategy that combines geometric constraints, RANSAC verification, and two-stage registration to improve computational stability while ensuring accuracy.

[0070] First, from the pre-processed Cartesian sonar image, high-confidence edge points are extracted using the gradient magnitude and local maximum detection operator. Feature points located on the pipeline wall form a sparse feature set. For adjacent keyframes, candidate matching point pairs are established, and preliminary screening is performed through cylindrical geometric constraints:

[0071]

[0072] where, , are the two frames of candidate points, , is the azimuth angle, is the radius tolerance, is the azimuth angle tolerance.

[0073] Next, RANSAC is used to verify the candidate matches by minimizing the reprojection error of the rigid transformation to eliminate outliers:

[0074]

[0075] where, and are the rotation matrix and translation vector, respectively, from which a set of high-confidence matching points, is the source point, is the target point, is the reprojection error.

[0076] To further improve the registration accuracy, a two-stage registration strategy is adopted. In the first stage, the least squares estimation is performed on the matching points to achieve the coarse registration of the initial pose:

[0077]

[0078] wherein, is the initial rigid transformation estimation, is the candidate matching set, and is the matching point pair, is the optimization variable.

[0079] The initial estimation serves as the initial guess for the subsequent point cloud registration algorithm, significantly reducing the number of iterations and reducing the risk of falling into a local minimum. In the second stage, the point-to-plane point cloud registration algorithm is used to optimize the registration result for fine adjustment:

[0080]

[0081] wherein, is the target point normal vector, is the source point and target point, is the rotation and translation variable. The introduction of normal vector information makes the point cloud registration algorithm more accurate in convergence on curved pipelines.

[0082] This combined strategy performs well in highly repetitive sonar data inside the pipeline, improving the correct feature matching rate and providing reliable local registration constraints for subsequent global consistency graph optimization.

[0083] 4. Factor graph optimization

[0084] To effectively suppress the cumulative error in the long-distance pipeline inspection process, the present application constructs a SLAM system based on factor graph optimization (FGO), modeling the DVL / IMU dead reckoning and sonar registration results as unified constraint factors. The factor graph of the system consists of pose nodes ∈ SE(2) and constraint edges including motion factors (derived from DVL / IMU dead reckoning), sonar registration factors (derived from point cloud registration algorithm registration results), and loop closure factors (derived from subsequent loop detection). The global optimization objective is:

[0085]

[0086] wherein, is the optimal pose set, is the pose node, is the observation constraint, is the prediction function, is the information matrix, is the set of constraint edges. Compared with the recursive filter based on extended Kalman filter (EKF), factor graph optimization can solve the batch problem with global constraints and re-adjust the historical poses when loop closure is detected, thus significantly reducing the cumulative drift.

[0087] This global consistency optimization is particularly crucial in the pipeline environment, because the length of the pipeline far exceeds the single frame field of view of the sonar, and if not controlled, the cumulative error will grow rapidly. For example, Figure 3 is the theoretical diagram of the sliding window optimization process of the invention.

[0088] 5. Loop detection and point cloud map construction

[0089] In the pipeline environment, the high repeatability of geometric features makes the reliability of loop detection directly affect the global consistency of the point cloud map. The invention adopts a dual verification mechanism combining appearance and geometry for loop detection.

[0090] First, at the appearance level, the similarity between key frames is calculated using a global sonar descriptor. When the similarity exceeds a certain threshold, the key frame is selected as a loop candidate frame. Then, the point cloud registration algorithm is applied to the candidate frame for geometric verification. If the registration residual is below the preset threshold and the point cloud overlap is high, the loop is confirmed to be established, and the corresponding loop factor is added to the factor graph.

[0091] In terms of point cloud map construction, a key frame fusion strategy is adopted, and only when the pose changes exceed the threshold, a new key frame is added. The point cloud of each key frame is converted to the global coordinate system and fused through the occupancy grid mapping to generate a global point cloud map. When the loop triggers global optimization, the historical key frame poses will be updated, and the point cloud map structure will be fine-tuned, ultimately achieving a globally consistent and accurate pipeline point cloud map.

[0092] 6. Sensor calibration and time synchronization

[0093] To ensure the accuracy of the tight coupling optimization, the system needs to achieve:

[0094] Time synchronization: Prefer hardware PPS or PTP synchronization, and software solution is based on timestamp interpolation and delay compensation; External parameter calibration: Use a combination of motion constraints (through joint optimization of external parameters and trajectories) and static calibration (based on calibration board or known geometry) to complete the external parameter estimation between IMU-sonar, DVL-sonar and camera (if present); Internal parameter / geometric distortion correction: Linear / nonlinear distortion correction for sonar polar coordinate mapping and beam geometry.

[0095] 7. Abnormality detection and adaptive weight

[0096] To deal with the situation of DVL lock loss or sonar being contaminated by strong echoes, an online anomaly detection module is designed: by monitoring the signal quality (signal-to-noise ratio, effective echo rate) returned by the DVL and the growth trend of the sonar registration residual, the weight of the corresponding factor is automatically reduced or the observation is temporarily removed to ensure the stable operation of the system.

[0097] In hardware implementation, to ensure the performance of the algorithm, the present application designs a multi-sensor integrated detection platform based on ROV. The platform can enter the internal of the seawater pipeline without draining, perform positioning, mapping and silt thickness measurement tasks, and thus obtain quantitative indicators of the degree of blockage.

[0098] 8. ROV overall structure

[0099] The experiment uses a self-made ROV platform with a frame structure design, equipped with a total of 8 thrusters in horizontal and vertical directions, which can realize full-attitude motion control and has good maneuverability and stability. A single-degree-of-freedom manipulator is installed at the front end of the ROV for auxiliary operation in pipeline detection tasks. The whole machine uses a double-layer polyurethane optical-electric composite cable to communicate with the shore-based main control box, with a communication bandwidth not less than 1 Gbps and a cable tensile strength not less than 500 kg. The main control end winch system uses an electric wire winding mechanism, configured with a 1000 m long double-layer polyurethane optical-electric composite cable. The ROV uses shore power supply, and the main control box is responsible for AC to high-voltage DC conversion, sensor power supply and data transmission, ensuring that the operation time is not less than 1 hour. To adapt to the high turbidity and low visibility environment in the pipeline, the platform is equipped with a multi-mode fusion system combined with multiple sensors.

[0100] BlueView M900 forward-looking sonar: with a maximum field of view angle of 130° and a maximum data update frequency of 25 Hz, it supports multiple working frequencies. The sonar can provide high-resolution real-time sonar images in extremely low optical visibility conditions, which is used for pipeline structure detection and obstacle identification.

[0101] RSM-SNR ring-shaped profile sonar: emits a conical beam with a maximum echo distance of 6 m, which can generate a complete ring-shaped profile of the pipeline in real time and is used for calculating the silt thickness. Waterlinked A125 DVL: used to measure the velocity information of the ROV relative to the pipeline wall or the bottom, outputting high-precision six-degree-of-freedom velocity data, which is an important motion constraint sensor in the SLAM system.

[0102] All sensors are connected through a unified time synchronization module to realize PPS / PTP-based clock synchronization. IMU and DVL data are uploaded to the main control computing unit in real time in a high-frequency sampling manner, and sonar and camera data are transmitted into the factor graph optimization module after time stamp alignment.

[0103] 9. Operation flow

[0104] When the ROV enters the underwater pipeline, the two-dimensional imaging sonar at the front end of the ROV begins to continuously capture the echo signals of the inner wall and obstacles in front of the pipeline. The original echo data is first decoded and polar coordinate projection converted by the ProViewer driver to generate a grayscale intensity image. The driver sends data packets in real time to the ROS system through the Ethernet UDP protocol at a fixed port and IP address.

[0105] The ROS host end runs the sonar receiving node (sonar_node), which subscribes to the data stream sent by the ProViewer driver through the socket interface and parses it into ROS standard messages. Subsequently, the system performs filtering and feature registration processing of the sonar image: first, a beam direction mean filter and a bandpass filter are used to smooth the noise signal and remove near-field interference and multi-path reflections; then, a two-dimensional discrete wavelet transform (db4 base) is used to denoise at multiple scales and extract the main structural information of the echo image; next, the image gradient amplitude is calculated and high-confidence feature points are extracted through local maximum value detection; finally, these feature points are projected into the ROV coordinate system to form a two-dimensional point cloud. Adjacent frame point clouds are preliminarily matched through cylindrical geometric constraints and the RANSAC algorithm, thereby achieving feature registration. The entire processing process runs in multiple threads in parallel in the ROS node, with an average delay of less than 50ms, ensuring real-time processing capability at a sonar frame rate of 25Hz. The Doppler velocity log (DVL) and inertial measurement unit (IMU) installed on the ROV are installed with a common reference, maintaining rigid connection and coordinate consistency. The DVL is used to measure the velocity component of the ROV relative to the pipe wall, and the IMU is used to obtain angular velocity and linear acceleration. Both data are collected by independent drivers and sent to the ROS host through the UDP / IP protocol. The / imu_data and / dvl_data nodes on the ROS end are run for message synchronization, and the system uses a timestamp alignment mechanism to ensure that the synchronization error of sensor data is less than 1ms. The synchronized data enters the dead reckoning module, which calculates the ROV displacement increment based on the IMU attitude estimation and DVL measured velocity in each sampling period, maintaining short-time dead reckoning continuity in the case of temporary loss of sonar data, and providing the pose prior for the SLAM backend. In the ROS system, the preprocessed sonar, IMU, and DVL data are sent to the backend optimization node. The system models the observation relationship in the form of a factor graph, defines the sonar registration factor, IMU pre-integration factor, and DVL velocity factor, and performs incremental optimization within a sliding window. When a loop is detected, the historical pose is adjusted, thereby significantly reducing the cumulative drift. The optimization uses an incremental solver to realize real-time pose updating, outputting the accurate position and attitude of the ROV in the pipeline. The optimization results are transmitted in real time to RVIZ for rendering, generating a three-dimensional point cloud map, and the operator can view the real-time SLAM view in the host computer interface.

[0106] The ROV control subsystem includes three parts of QGroundControl (QGC) operation software, Nano microcomputer and host PC. The Nano microcomputer runs the ROS system and the SLAM algorithm of the application, is responsible for receiving multi-source sensor data, performing optimization calculation and generating a visual video stream. The PC end pulls the RTSP video stream of the Nano end through the VLC player, and displays the point cloud map, pose trajectory and sonar image in real time. The operator can execute the manipulation command in QGroundControl according to the SLAM view, QGroundControl transmits the command to Nano through UDP, and Nano forwards the command to the ROV equipped with Pixhawk, so as to realize the propeller control and attitude adjustment.

[0107] In order to ensure the continuity of the control flow, the system has an abnormality detection and automatic retry mechanism: when any sensor fails to track data, communication is interrupted or time drift occurs, the ROS end will immediately issue an alarm on the interface and execute a reconnection and data buffering mechanism in the background. If it cannot be recovered within the set retry time, the system automatically enables the inertial calculation mode to maintain the ROV attitude stable, and executes the safety shutdown or return instruction if necessary.

[0108] The whole system supports online and offline running modes. In online mode, real-time ROV data is received and SLAM mapping is performed; in offline mode, historical data packets (rosbag) can be loaded for playback, parameter adjustment and algorithm verification. The system running environment is Ubuntu 20.04 + ROS Noetic, which can be deployed on NVIDIA Jetson or x86 industrial computer platform.

[0109] 10. Experimental verification

[0110] The application has carried out verification experiments in real underwater pipelines to evaluate the performance of the proposed algorithm compared with dead reckoning and BRUCE-SLAM. The running effect in real pool and pipeline is verified, such as Figure 4 、 Figure 5 The running effect diagrams of the application in the pool and the pipeline are shown in Figs.

[0111] (1) Trajectory error experiment

[0112] The ROV is equipped with a two-dimensional imaging sonar, a DVL and an IMU, and travels along the center line of the pipeline at a constant speed while maintaining a small attitude change to calculate the real inspection trajectory.

[0113] Three methods were compared: dead reckoning, which relies only on IMU and DVL integration and is prone to drift, as a baseline method; BRUCE-SLAM, a representative sonar-based SLAM method in the prior art; and the method proposed in the present application, which uses tight coupling factor graph optimization to fuse sonar, DVL and IMU measurement data. The ground truth trajectory was obtained by manual ranging benchmarks.

[0114] Table 1 Error analysis of different methods

[0115]

[0116] Table 1 shows the root mean square error and standard deviation of the three methods in three repeated experiments. The dead reckoning error accumulates rapidly, with a root mean square error of 4.70 m and a standard deviation of 0.45 m, far exceeding the actual accuracy requirement. BRUCE-SLAM significantly reduces the error compared to dead reckoning, with a root mean square error of 1.22 m and a standard deviation of 0.15 m, but still has matching errors in long distances or occluded areas. The method proposed in the present application achieves the highest accuracy, with a root mean square error of 0.70 m and a standard deviation of 0.12 m, a reduction of 85% in root mean square error compared to dead reckoning, and a reduction of 42.6% in root mean square error compared to BRUCE-SLAM.

[0117] (2) DVL interference experiment

[0118] To further evaluate the robustness of the system under sensor degradation conditions, a DVL interference experiment was conducted. In this experiment, the present application accelerated the speed of the ROV, increasing the water flow disturbance caused by the ROV thrusters, which would adversely affect the speed estimation. After the experiment, the noise generated by the two running speeds on the DVL was divided into two interference levels: low noise and high noise, with high noise representing more severe speed fluctuations.

[0119] Table 2 Error analysis under DVL interference

[0120]

[0121] The proposed method is compared with BRUCE-SLAM in terms of performance under two noise levels. Table 2 summarizes the root mean square error and standard deviation results. Under low noise condition, the proposed method achieves a root mean square error of 0.74 m and a standard deviation of 0.12 m, which is better than the root mean square error of 1.16 m and the standard deviation of 0.15 m of BRUCE-SLAM. Under high noise condition, the root mean square error of the proposed method increases slightly to 1.05 m and the standard deviation is 0.15 m, but it is still significantly better than the root mean square error of 1.97 m and the standard deviation of 0.20 m of BRUCE-SLAM. The results show that the proposed system still maintains stable positioning performance even in the case of DVL measurement disturbance, effectively reducing the influence of disturbance caused by the thruster.

[0122] The above only describes the preferred embodiments of the present application and is not intended to limit the present application. For those skilled in the art, the present application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

[0123] Although the specific embodiments of the present application are described above, they are not intended to limit the scope of protection of the present application. Those skilled in the art should understand that various modifications or changes made on the basis of the technical solutions of the present application without creative labor are still within the protection scope of the present application.

Claims

1. A method for simultaneous localization and mapping for underwater pipeline inspection, characterized in that, Includes the following steps: S1, based on the two-dimensional imaging sonar, Doppler velocity meter (DVL) and inertial measurement unit (IMU) deployed on the ROV body, the original echo image, velocity information and direction information are obtained respectively; S2, the acquired raw echo image is preprocessed to suppress multipath echoes, and then, based on the obtained velocity and direction information, the sonar polar coordinate projection error is corrected for geometric consistency, and mapped to a Cartesian sonar mapping image. The specific process is as follows: First, the attitude change between adjacent frames is estimated using the angular velocity and linear acceleration of the IMU. Then, the displacement is calculated by combining the velocity information measured by DVL, thus obtaining the predicted pose transformation between sonar frames. By comparing the predicted pose with the actual projected position of the sonar image, the displacement and attitude deviations are obtained, and geometric compensation is performed on the sonar projection points accordingly. The corrected calculation formula is as follows: in, The coordinates of the sonar projection point. The displacement deviation between the predicted coordinates of DVL and IMU and the coordinates of the projected points. Here is the attitude rotation matrix. These are the corrected coordinates of the projected points; S3. Feature points are extracted based on Cartesian sonar mapping images, and a candidate matching set is established using adjacent keyframe templates. False matches are then filtered out based on cylindrical geometric constraints and structural repeatability detection rules. When filtering out false matches, cylindrical geometric constraints are applied to the feature templates of adjacent keyframes for matching. A valid match is considered to be achieved when the radial distance difference and azimuth difference satisfy the following conditions: in, and Represents the radial distance between adjacent keyframe feature points. and Indicates the corresponding azimuth angle. Indicates radius tolerance. This indicates the azimuth tolerance; other matches are invalid and will be deleted. S4 inputs the candidate matching set output from S3 into the registration module, first performs coarse registration to obtain the initial pose estimate; then performs fine registration to achieve self-calibration of the registration residual; at the same time, it calls the velocity direction information of DVL and IMU to construct multi-element fusion constraints and form a factor map. S5, based on the factor map obtained in S4, performs incremental optimization under the sliding window optimization strategy; when the appearance similarity is detected to be higher than the threshold and the geometric residual of the point cloud registration algorithm is lower than the set value within the window, the loop closure factor is automatically injected and the local to global re-optimization is triggered to obtain the key frame pose; Incremental optimization is performed under the sliding window optimization strategy. The specific process is as follows: Based on the factor graph model constructed by S4, incremental optimization is performed within the sliding window. The window size is dynamically adjusted according to the average residual and covariance spectrum radius of the previous period. When the residual is higher than the threshold, the window is expanded, and when it is lower than the threshold, the window is shrunk to balance real-time performance and accuracy requirements. The optimization output includes keyframe pose trajectory and marginalization factor. The system automatically injects loop closure factors and triggers local-to-global re-optimization. The specific process is as follows: using a dual mechanism of appearance and geometry, firstly, the appearance similarity is calculated based on the polar coordinate intensity histogram of keyframes. When the cosine similarity is higher than 0.75, it is marked as a candidate loop closure. Then, iterative registration verification from point to plane is performed. When the root mean square error is less than 0.2m, the loop closure is confirmed. The system then injects loop closure factors into the factor graph and triggers global re-optimization to eliminate accumulated errors. S6, based on the keyframe poses obtained in S5, performs coordinate unification and occupancy probability fusion on the point cloud data; constructs a confidence weight field in the overlapping area, adjusts the contribution of each frame, and generates a globally consistent motion trajectory and point cloud map.

2. The synchronous positioning and mapping method for underwater pipeline inspection as described in claim 1, characterized in that: The preprocessing process in S2 includes: First, near-field culling and beam direction intensity normalization are performed on the original sonar echo image from S1. Then, two-dimensional discrete wavelet transform is used to suppress high-frequency noise, and its wavelet transform coefficients satisfy: in, Represents image signals, For wavelet basis functions, For scale parameters, For spatial indexing.

3. The synchronous positioning and mapping method for underwater pipeline inspection as described in claim 1, characterized in that: The specific steps for extracting feature points based on the Cartesian sonar mapping image in S3 are as follows: First, the gradient magnitude is calculated on the Cartesian sonar mapping image. Then, a threshold filtering method is used to extract feature points, with a threshold range of 0.05–0.

2. The extracted feature points are then reprojected into polar coordinates according to the pipe axis to generate a pipe geometric template, thereby reducing feature drift caused by changes in viewing angle. in, For Cartesian sonar mapping images, , For image coordinates, The gradient magnitude is used; during local maximum detection, only the gradient magnitude is retained. Pixels that exceed a set threshold are designated as feature points.

4. The synchronous positioning and mapping method for underwater pipeline inspection as described in claim 1, characterized in that: In step S4, the registration process employs a two-stage optimization strategy. First, the initial rigid body transformation pose is calculated using the least squares method. Then, a point-to-plane iterative registration algorithm is executed, with the error energy function being: in, As the source point, For the target point, Let be the normal vector of the target point. Let be a rotation matrix. It is a translation vector. The point-to-plane registration error energy is used; after each iteration, the weight matrix is ​​updated according to the residual size to achieve residual-driven self-calibration, and the registration residual and covariance information are output.

5. The synchronous positioning and mapping method for underwater pipeline inspection as described in claim 1, characterized in that: The construction of the factor graph in S4 is specifically as follows: Using a unified coordinate system as a reference, the IMU pre-integration factor, DVL velocity factor, and sonar registration factor are constructed into a joint observation equation. The IMU pre-integration factor is generated by the accumulation of IMU observations in S1, providing short-term attitude and velocity priors. The DVL velocity factor is projected from the DVL observations in S1 onto the same coordinate system, constraining the translational velocity. The sonar registration factor consists of residuals and pose estimations, and the sonar registration factor is a constraint term constructed by linearizing the registration residuals in stage S4. The three factors are fused together and the influence of noise covariance of different sensors is balanced by weight normalization.

6. The synchronous positioning and mapping method for underwater pipeline inspection as described in claim 1, characterized in that: The specific process of S6 is as follows: A confidence-weighted fusion strategy is employed to perform unified coordinate transformation and occupancy probability fusion on keyframe point clouds, with different confidence weights for each frame. From the pose covariance matrix The decision is made using the following formula: in, The confidence weights of the point cloud in the i-th keyframe are: This is the pose covariance matrix for this frame. The normalization coefficient is used; the weighted fusion result performs consistency constraints and confidence smoothing in the overlapping areas, and finally generates a globally consistent motion trajectory and point cloud map.

Images