Pipeline robot autonomous cruise and positioning navigation system

Abstract

The application is a kind of pipeline robot autonomous cruise and positioning navigation system, and relates to the technical field of robot navigation and positioning, comprising: a fusion positioning module, which is used for inputting inertial measurement unit pre-integral constraint, odometer displacement constraint, visual observation constraint and point cloud section registration constraint into a factor graph optimizer to perform joint optimization and solve, and outputting the optimal pose estimation of the robot. In the application, multi-source perception fusion integrates the data of laser radar, depth camera, inertial measurement unit, odometer and ultrasonic ranging sensor under a unified time reference, so that each perception source can complement each other under different environmental degradation conditions, and the influence of the failure of a single perception source on the positioning result is reduced.

Description

Technical Field

[0001] This invention relates to the field of robot navigation and positioning technology, and in particular to an autonomous navigation and positioning system for a pipeline robot. Background Technology

[0002] Pipelines are an important component of industrial production and municipal infrastructure. After long-term operation, pipelines may experience problems such as deformation, blockage, or foreign object adhesion, requiring regular internal inspections. Pipeline robots are commonly used equipment for performing these inspection tasks, autonomously navigating inside pipelines and collecting pipeline status information.

[0003] Existing navigation and localization solutions for pipeline robots typically rely on pre-set maps or a single sensing source. Pre-set map-based solutions require prior mapping of the pipeline environment before robot deployment. When the pipeline structure changes, inconsistencies arise between the pre-set map and the actual environment, leading to path planning failure. Single-sensor-based solutions have significant limitations in the unique environment inside pipelines: vision-based solutions suffer from reduced feature extraction capabilities under poor lighting conditions and with sparse pipe wall texture; inertial measurement units and odometry accumulate errors over long distances, causing localization drift to increase with distance traveled.

[0004] The aforementioned problems make it difficult for existing solutions to simultaneously guarantee positioning accuracy and path planning effectiveness in complex pipeline environments with no global navigation satellite system signals, poor lighting conditions, and branches and obstacles, thus limiting the practicality of pipeline robots in long-distance inspection tasks. Summary of the Invention

[0005] The purpose of this invention is to provide an autonomous navigation and positioning system for pipeline robots in order to solve the above-mentioned problems.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: An autonomous navigation and positioning system for pipeline robots includes: The multi-source perception fusion module is used to perform timestamp alignment processing on the perception data synchronously collected by multiple types of sensors on the pipeline robot, and generate a multimodal synchronous dataset under a unified time reference. The topology map generation module is used to perform joint feature extraction on the point cloud data and depth image in the multimodal synchronous dataset, identify the pipe cross-sectional geometric parameters and topology nodes, and generate a pipe topology map in an incremental manner. The fusion positioning module is used to input the pre-integration constraints of the inertial measurement unit, the displacement constraints of the odometry, the visual observation constraints, and the point cloud cross-section registration constraints into the factor graph optimizer to perform joint optimization and solve the problem, and output the optimal pose estimate of the robot. The path planning and obstacle avoidance module is used to perform a global path search based on the pipeline topology map to generate the optimal path node sequence, and to perform local obstacle avoidance trajectory generation based on real-time perception data. The closed-loop detection and relocation module is used to perform matching verification between the current sensing data and historical data. When the closed loop is established, the closed-loop constraint is added to the factor graph optimizer to perform global pose adjustment and correct the cumulative drift error.

[0007] Preferably, the timestamp alignment process includes: Using the sampling time of the lidar as the reference time, the estimated values ​​corresponding to the reference time are obtained by linear interpolation algorithms for the data from the inertial measurement unit, odometer, depth camera and ultrasonic ranging sensor, respectively, and a spatiotemporally synchronized multimodal sensing data frame sequence is generated.

[0008] Preferably, the identification of pipe cross-sectional geometric parameters and topological nodes includes: The lidar point cloud is segmented, and the cross-sectional diameter, ellipticity, and center position of the current pipe cross-section are extracted using cylindrical or elliptical fitting algorithms. When multiple independent pipe cross-sectional profiles appear in the cross-sectional point cloud of consecutive frames, the current position is marked as a branch node; when the rate of change of direction of the cross-sectional center position sequence exceeds a preset angle threshold, the current position is marked as a bend or corner node; when an independent point cloud cluster that is discontinuous with the pipe wall profile and located in the internal space of the pipe appears in the cross-sectional point cloud, the current position is marked as an obstacle node.

[0009] Preferably, the pipeline topology map is represented by a graph structure, with each topology node serving as a vertex of the graph, and the pipe segments between adjacent nodes, along with their lengths and geometric properties, serving as edges of the graph. The method also includes: comparing the current cross-sectional geometric parameters with historical cross-sectional data; when the ellipticity deviates from the normal range or the cross-sectional area decreases to below a preset area threshold, marking the location as a deformation point or blockage point, and attaching the corresponding status attributes to the corresponding node or edge of the pipeline topology map.

[0010] Preferably, the factor graph optimizer uses the iSAM2 incremental optimizer, and the confidence level of the constraint is represented by the information matrix corresponding to each constraint factor; When the number of matched visual feature points is lower than a preset threshold, the weight of the information matrix corresponding to the visual observation constraint is reduced, while the weight of the information matrix corresponding to the point cloud cross-section registration constraint is increased.

[0011] Preferably, the global path search uses the A* algorithm or the Dijkstra algorithm, whose input is the set of nodes and the set of weighted edges of the pipeline topology map, as well as the starting node and the target node, and whose output is the optimal path node sequence; The weight of the weighted edge is obtained by combining the path length and the pipeline throughput attribute. The pipeline throughput attribute is derived from the deformation or blockage status marked in the pipeline topology map.

[0012] Preferably, the local obstacle avoidance trajectory generation includes: During the journey along the optimal path node sequence, real-time sensing data from the depth camera and ultrasonic ranging sensor are continuously received; when a sudden obstacle or a sudden change in pipe diameter is detected ahead, an obstacle avoidance trajectory that satisfies kinematic constraints is generated using the dynamic window method or the time elastic band algorithm. When the obstacle avoidance trajectory causes the robot to deviate from the optimal path node sequence by more than a preset distance threshold, the global path search is re-executed starting from the current position to update the subsequent path node sequence.

[0013] Preferably, the matching verification includes: The cross-sectional geometric features of the current frame are compared with the cross-sectional geometric features of historical key frames to filter candidate closed-loop frames. Only candidate frames that exceed the preset minimum interval are included in the candidate set. The candidate closed-loop frames are verified by point cloud cross-sectional shape similarity, visual bag-of-words model matching, and closure consistency of inertial measurement unit trajectory extrapolation. When all three verifications pass the preset threshold, the closed loop is confirmed to be valid. The point cloud cross-sectional shape similarity is obtained by calculating the cosine similarity after performing Z-score normalization on the cross-sectional geometric parameter vector. The cross-sectional geometric parameter vector includes the cross-sectional diameter, ellipticity, and the lateral and longitudinal coordinates of the cross-sectional center position.

[0014] Preferably, the method further includes: When real-time location is lost, the current consecutive multi-frame cross-sectional geometric feature sequence and visual feature sequence are globally matched with the historical feature sequence stored in the pipeline topology map to obtain a set of candidate locations; Based on the candidate position set as the initial particle distribution, a particle filtering method is used with the current real-time sensing data as the observation input to output the converged optimal pose estimate.

[0015] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are: 1. This invention integrates data from lidar, depth camera, inertial measurement unit, odometer and ultrasonic ranging sensor under a unified time reference through multi-source sensing fusion, so that each sensing source can complement each other under different environmental degradation conditions, reducing the impact of the failure of a single sensing source on the positioning results.

[0016] 2. This invention generates pipeline topology maps online incrementally, allowing robots to obtain a structured representation of the pipeline environment without relying on a preset map. This enables path planning to continue based on the real-time constructed map even when the pipeline structure changes, avoiding path planning failures caused by inconsistencies between the preset map and the actual environment. Attached Figure Description

[0017] Further details, features, and advantages of this application are disclosed in the following description of exemplary embodiments in conjunction with the accompanying drawings, in which: Figure 1 This is a system flowchart of the present invention. Detailed Implementation

[0018] Several embodiments of this application will now be described in more detail with reference to the accompanying drawings to enable those skilled in the art to implement this application. This application may be embodied in many different forms and for various purposes and should not be limited to the embodiments set forth herein. These embodiments are provided to make this application thorough and complete, and to fully convey the scope of this application to those skilled in the art. The embodiments described do not limit this application.

[0019] Unless otherwise defined, all terms used herein (including technical and scientific terms) shall have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. It will be further understood that terms such as those defined in commonly used dictionaries shall be interpreted as having a meaning consistent with their meaning in the relevant field and / or the context of this specification, and shall not be interpreted in an idealized or overly formal sense unless expressly defined herein.

[0020] Example 1 Its specific implementation method is combined with the appendix Figure 1 Please provide a detailed explanation.

[0021] In this embodiment, it includes: Pipeline robots face complex environments when performing inspection tasks inside pipelines, including the absence of global navigation satellite system signals, poor lighting conditions, sparse pipe wall texture, and the presence of branches and obstacles. Existing navigation and positioning schemes often rely on pre-set maps or single sensing sources, which are prone to problems such as accumulated positioning drift and path planning failure during long-distance navigation. This embodiment proposes an autonomous navigation and positioning method for pipeline robots. Through multi-source sensing fusion, online topology map generation, graph-optimized positioning, hierarchical path planning, and loop closure detection, it achieves autonomous navigation and accurate navigation under conditions without prior maps.

[0022] The hardware environment of this embodiment includes: a pipeline robot equipped with a lidar, depth camera, inertial measurement unit, odometer and ultrasonic ranging sensor, and each sensor uses a hardware clock synchronization module to unify the time reference.

[0023] An autonomous navigation and positioning method for a pipeline robot according to an embodiment of the present invention includes the following steps: Step 1: Collect multi-source sensing data and perform spatiotemporal synchronization fusion By synchronously collecting sensing data using multiple types of sensors deployed on the pipeline robot, and performing timestamp alignment processing on the raw data output by each sensor, a multimodal synchronous dataset under a unified time reference is generated.

[0024] Step 1 may specifically include: Step 101: Collect point cloud data of the pipe cross-section profile using lidar, collect depth images of the environment ahead using a depth camera, collect triaxial acceleration and angular velocity data using an inertial measurement unit, collect wheel travel distance and rotation speed data using an odometer, and collect pipe wall distance data using an ultrasonic ranging sensor.

[0025] Step 102: Based on the hardware clock synchronization signal and software timestamp interpolation, align the data from each sensor to a unified time reference to generate a spatiotemporally synchronized multimodal sensing data frame sequence.

[0026] It should be noted that the above timestamp interpolation is for cases where the sampling frequencies of each sensor are different. The sampling time of the lidar is used as the reference time, and the data of the other sensors are obtained by linear interpolation algorithm to obtain the estimated value corresponding to the reference time, so that the same frame of data corresponds to the sensing information of the same physical time.

[0027] Step 2: Extract the geometric features of the pipe cross-section and generate a pipe topology map online. Joint feature extraction is performed on point cloud data and depth images in the multimodal sensing data frame sequence to identify pipeline cross-sectional geometric parameters and key topological nodes, and pipeline topology maps are generated incrementally.

[0028] Step 2 may specifically include: Step 201: Perform segmentation processing on the lidar point cloud, and extract the geometric parameters of the current pipe cross-section using a cylindrical fitting or elliptical fitting algorithm. The geometric parameters include the cross-section diameter, ellipticity, and cross-section center position.

[0029] Step 202: Extract pipe wall texture feature points from the depth image using a visual simultaneous localization and mapping (VLS) algorithm to generate a displacement increment estimate for the visual odometry.

[0030] It should be noted that the above-mentioned visual simultaneous localization and mapping (SSLAM) algorithm is the ORB-SLAM or RTAB-Map algorithm. Its input is the continuous frame depth images acquired by the depth camera, and its output is the relative displacement increment between adjacent frames and the extracted feature point set.

[0031] Step 203: Based on the detection of changes in cross-sectional geometric parameters, identify the locations of branch nodes, bends, and obstacles in the pipeline, and mark these key locations as topological nodes.

[0032] It should be noted that the branch node identification method is as follows: when multiple independent pipe cross-sectional profiles appear in the cross-sectional point cloud across multiple consecutive frames, the current position is determined to be a tee or cross-shaped branch node. The elbow and corner identification method is as follows: when the rate of change of direction of the cross-sectional center position sequence exceeds a preset angle threshold, the current position is determined to be an elbow or corner node. The obstacle location identification method is as follows: when an independent point cloud cluster that is discontinuous with the pipe wall profile appears in the cross-sectional point cloud, and the location of the independent point cloud cluster is located inside the pipe, the current position is determined to have an obstacle, and the location is marked as an obstacle node.

[0033] Step 204: Using a graph structure, each topological node is used as a vertex of the graph, and the pipe segments between adjacent nodes, their lengths, and geometric properties are used as edges of the graph, generating a pipeline topology map incrementally.

[0034] In this embodiment of the application, in order to improve the ability of the pipeline topology map to represent changes in pipeline status, step 201 further includes comparing the current cross-sectional geometric parameters with historical cross-sectional data. When the ellipticity deviates from the normal range or the cross-sectional area is significantly reduced, the location is marked as a deformation point or a blockage point, and the corresponding status attributes are attached to the corresponding node or edge of the pipeline topology map.

[0035] Step 3: Optimize multi-source fusion localization and attitude calculation based on factor graph. The inertial measurement unit pre-integration results, odometry dead reckoning results, visual feature point matching results, and point cloud cross-section registration results are used as multi-source observation constraints. The optimal pose estimation of the robot is solved by factor graph optimization.

[0036] Step 3 may specifically include: Step 301: Perform pre-integration processing on the inertial measurement unit data to generate relative pose pre-integration constraints between adjacent keyframes.

[0037] Step 302: Calculate the displacement constraints between adjacent keyframes based on the odometry data.

[0038] Step 303: Calculate visual observation constraints based on the matching relationship between visual feature points in adjacent frames.

[0039] Step 304: Perform registration processing between the cross-sectional point cloud of the current frame and the cross-sectional point cloud of the previous keyframe to generate point cloud cross-sectional registration constraints.

[0040] Step 305: Input the pre-integration constraints, displacement constraints, visual observation constraints, and point cloud cross-section registration constraints into the factor graph optimizer for joint optimization and solve, and output the optimal pose estimate of the robot at each key frame time. The pose includes three components of position and three components of attitude angle.

[0041] It should be noted that the factor graph optimizer described above uses the iSAM2 incremental optimizer. Its input consists of pose constraint factors provided by each sensor, namely the pre-integration constraints, displacement constraints, visual observation constraints, and point cloud cross-section registration constraints mentioned above. The output is a globally consistent robot pose sequence. The iSAM2 incremental optimizer uses the information matrix corresponding to each constraint factor to represent the confidence level of the constraint. The value of the information matrix reflects the observation reliability of the sensor from which the constraint originates at the current moment.

[0042] In this embodiment, to maintain positioning accuracy in long, straight pipe sections with sparse visual features, step 305 adaptively adjusts the information matrix of each constraint factor. When the number of matched visual feature points is lower than a preset threshold, the weight of the visual observation constraint is reduced, while the weight of the point cloud cross-section registration constraint is increased, making the positioning result more dependent on the geometric constraints provided by the lidar.

[0043] Step 4: Integrate pipeline topology map with real-time perception data to perform hierarchical path planning and dynamic obstacle avoidance. Based on the pipeline topology map and real-time pose estimation, path planning is performed at the global and local layers to generate a navigation path from the current position to the target position, and local obstacle avoidance trajectory generation is performed when a sudden obstacle is detected.

[0044] Step 4 may specifically include: Step 401: Receive the target location information, perform a global path search on the graph structure of the pipeline topology map, and generate the optimal path node sequence from the current node to the target node.

[0045] It should be noted that the global path search described above uses either the A* algorithm or Dijkstra's algorithm. Its input consists of the graph structure of the pipeline topology map (a set of nodes and a set of weighted edges), the starting node, and the target node. The output is the optimal path node sequence. The edge weights are calculated by combining the path length and the pipeline throughput attribute, which is derived from the deformation or blockage status marked in step 2.

[0046] Step 402: During the journey along the optimal path node sequence, continuously receive real-time sensing data from the depth camera and ultrasonic ranging sensor to detect the position of obstacles and changes in pipe diameter in the local environment ahead.

[0047] Step 403: When a sudden obstacle or a sudden change in pipe diameter is detected ahead, local path replanning is triggered to generate an obstacle avoidance trajectory, bypass the obstacle, and then return to the optimal path node sequence.

[0048] It should be noted that the above local path replanning adopts the dynamic window method or the time elastic band algorithm. Its input is the robot's current pose, current velocity, local obstacle position, and the next sub-target point on the optimal path node sequence. The output is the obstacle avoidance trajectory that satisfies the kinematic constraints.

[0049] In this embodiment, to ensure the coordination between global navigation target orientation and local obstacle avoidance agility, steps 401 and 403 are linked in real time through tree structure search. When the local obstacle avoidance trajectory causes the robot to deviate from the optimal path node sequence by more than a preset distance threshold, the global path search in step 401 is re-executed starting from the current position to update the subsequent path node sequence.

[0050] Step 5: Perform closed-loop detection and optimize pipeline topology map and pose estimation. During the cruise, current sensing data is matched and verified with historical data. When the loop closure is confirmed, the loop closure constraints are added to the factor graph optimizer to perform global pose adjustment and correct the accumulated drift error. When real-time positioning is lost, repositioning is performed based on the pipeline topology map.

[0051] Step 5 may specifically include: Step 501: Perform similarity calculation between the cross-sectional geometric features of the current frame and the cross-sectional geometric features of historical keyframes to filter candidate closed-loop frames.

[0052] Step 502: Perform multi-feature joint verification on the candidate closed-loop frames, including point cloud cross-sectional shape similarity verification, visual bag-of-words model matching verification, and inertial measurement unit trajectory inference closure consistency verification. When all three verifications pass the preset threshold, the closed loop is confirmed to be established.

[0053] It should be noted that in the above multi-feature joint verification, the point cloud cross-sectional shape similarity is obtained by calculating the cosine similarity between the current frame cross-sectional geometric parameter vector and the candidate frame cross-sectional geometric parameter vector. The cross-sectional geometric parameter vector contains four components: cross-sectional diameter, ellipticity, and the horizontal and vertical coordinates of the cross-sectional center position. Each component has different dimensions (the diameter and coordinates are in units of length, while the ellipticity is dimensionless). Before calculating the cosine similarity, Z-score normalization is performed on each component of the cross-sectional geometric parameter vector to eliminate the influence of dimensional differences on the similarity calculation.

[0054] The formula for calculating cosine similarity is: ; in, Represents cosine similarity. The first element represents the normalized cross-sectional geometric parameter vector of the current frame. One portion, The first element represents the cross-sectional geometric parameter vector of the candidate frame after standardization. One portion, This represents the total number of components in the cross-sectional geometric parameter vector. The component index is used. The cross-sectional geometric parameter vectors of the current frame and candidate frames are both four-dimensional after standardization, and the dimension used for cosine similarity calculation is consistent. Visual bag-of-words model matching is obtained by calculating the similarity score between the bag-of-words vector of the visual features of the current frame and the bag-of-words vector of the candidate frame. Inertial measurement unit (IMU) trajectory extrapolation closure verification is obtained by calculating the deviation between the IMU integral displacement from the candidate frame to the current frame and the corresponding path length in the pipeline topology map.

[0055] Step 503: After the loop closure is confirmed, the loop closure constraint is added as a new factor to the factor graph optimizer, the global pose graph is adjusted, the corrected global consistent pose sequence is output, and the position coordinates of each node in the pipeline topology map are updated synchronously.

[0056] Step 504: When real-time positioning is lost, perform global matching between the current consecutive multi-frame cross-sectional geometric feature sequence and visual feature sequence and the historical feature sequence stored in the pipeline topology map to obtain a candidate position set. Perform relocalization processing based on the candidate position set and output the robot's restored pose estimate.

[0057] It should be noted that the above relocation process uses the Monte Carlo localization method or the particle filter method. Its input is the candidate position set as the initial particle distribution and the current real-time sensing data as the observation, and the output is the optimal pose estimate after convergence.

[0058] Example 2

[0059] During a city's water supply network inspection, a pipeline robot was deployed inside the underground main water supply pipeline to perform routine inspections. This network consists of 500mm diameter cast iron circular pipes, approximately 280 meters long, including two tee junctions, one bend, one area of ​​suspected local deformation, and a temporary obstacle caused by construction work. The pipeline interior has no natural light source, no GPS signal, and the pipe wall surface has sparse texture and localized water film coverage. The robot starts from the entry point, targeting the valve inspection point at the end of the network. The robot is equipped with a lidar (10Hz sampling frequency), a depth camera (30Hz sampling frequency), an inertial measurement unit (200Hz sampling frequency), an odometry (50Hz sampling frequency), and an ultrasonic ranging sensor (20Hz sampling frequency).

[0060] After the robot enters the pipe, all sensors synchronously start collecting data. The lidar uses a sampling frequency of 10 Hz as the reference, and its sampling time serves as a unified time reference. The raw data from the depth camera, inertial measurement unit, odometry, and ultrasonic ranging sensors are all aligned to the lidar's sampling times using a linear interpolation algorithm to generate a multimodal synchronous data frame sequence. Table 1 below shows a snapshot of the synchronous data in frame 12 (when the robot has traveled approximately 18 meters).

[0061] Table 1. Snapshot of multimodal synchronization data in frame 12.

[0062] Cylindrical fitting is performed frame-by-frame on the point cloud data in the synchronized data frame sequence to extract the cross-sectional diameter, ellipticity, and center position. Simultaneously, the ORB-SLAM algorithm is run on the depth image to extract pipe wall texture feature points and generate visual odometry displacement increments. When significant changes occur in the cross-sectional geometry parameters, topological nodes are identified and the pipeline topology map is incrementally updated.

[0063] Table 2 below shows the results of extracting the cross-sectional geometric parameters of the robot as it travels from the entrance to about 90 meters. Frame 38 corresponds to the T-junction branch recognition event, and frame 61 corresponds to the deformation point marking event.

[0064] Table 2 Results of Geometric Parameter Extraction for Typical Frame Sections

[0065] The topology map is represented by a graph structure: each identified node is a vertex, and the length and passability attributes of the pipe segments between adjacent nodes are used as edge weights. The state attributes of the deformed point in frame 61 are attached to the corresponding edge for reference in subsequent path planning.

[0066] During the journey, inertial measurement unit pre-integration constraints, odometer displacement constraints, visual observation constraints, and point cloud cross-section registration constraints are continuously input into the iSAM2 incremental optimizer to jointly solve for the optimal pose estimation of each keyframe.

[0067] When the robot passes through the pipe section near the deformation area in frame 61, the water film on the pipe wall causes the number of visual feature points to drop to 8, which is lower than the preset threshold (30). The system automatically reduces the visual observation constraint weight while increasing the point cloud cross-section registration constraint weight, so that the positioning continues to rely on the geometric constraints of the lidar to maintain accuracy. Table 3 below shows the adaptive adjustment of constraint weights and pose output results for three typical keyframes.

[0068] Table 3 Typical Keyframe Constraint Weight Adaptive Adjustment and Pose Output

[0069] The pose estimation output includes three position components (along the pipe axis, laterally, and longitudinally) and three attitude angle components (pitch, roll, and yaw). The pose results of each keyframe are synchronously written to the corresponding node in the pipe topology map.

[0070] After receiving the target location (end valve detection point, corresponding to the target node in the topology map), an A* global path search is performed on the graph structure of the pipeline topology map to generate the optimal path node sequence. The edge weights are calculated by combining the pipe segment length and the throughput attribute. The throughput coefficient of the deformed pipe segment corresponding to frame 61 is increased due to the deformation state, and the path search results bypass the branch of the deformed segment and choose the main road for passage.

[0071] When the robot had traveled approximately 145 meters along the optimal path, both the ultrasonic ranging sensor and the depth camera simultaneously detected a construction-related pipe component (a sudden obstacle) 3.2 meters ahead. The system triggered local path replanning, generating an obstacle avoidance trajectory using a dynamic window method. The robot bypassed the obstacle along the wider side of the pipe's inner wall and returned to the main path. Table 4 below shows the key parameters of this obstacle avoidance event.

[0072] Table 4 Parameters of Sudden Obstacle Avoidance Events

[0073] When the robot travels to about 230 meters, the cross-sectional geometric feature similarity calculation results show that there is a high similarity candidate closed loop relationship between the current frame and the historical 38th frame (three-way branch node). The time interval between the candidate frame and the current frame is 193 seconds, which meets the minimum time interval constraint, and enters the multi-feature joint verification process.

[0074] After performing Z-score normalization on the cross-sectional geometric parameter vectors of the current frame and the candidate frame (frame 38), the results are calculated according to the formula. Calculate cosine similarity, where The normalized cross-sectional geometric parameter vector of the current frame One portion, The standardized cross-sectional geometric parameter vector of the candidate frame One portion, (Cross-section diameter, ellipticity, transverse coordinates of cross-section center, longitudinal coordinates of cross-section center). Table 5 below shows the results of the three joint verifications.

[0075] Table 5. Results of joint validation of closed-loop multi-features

[0076] All three verifications passed the threshold, confirming the loop closure. The loop closure constraint was added as a new factor to the iSAM2 optimizer, performing global pose graph adjustment and outputting a corrected globally consistent pose sequence, with the coordinates of each node in the pipeline topology map updated synchronously. Before correction, the global cumulative drift was 0.38 meters, which was reduced to 0.06 meters after correction.

[0077] The foregoing has only described certain exemplary embodiments of the present invention by way of illustration. Undoubtedly, those skilled in the art can modify the described embodiments in various ways without departing from the spirit and scope of the present invention. Therefore, the foregoing drawings and descriptions are illustrative in nature and should not be construed as limiting the scope of protection of the claims of the present invention.

[0078] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0079] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. An autonomous navigation and positioning system for a pipeline robot, characterized in that, include: The multi-source perception fusion module is used to perform timestamp alignment processing on the perception data synchronously collected by multiple types of sensors on the pipeline robot, and generate a multimodal synchronous dataset under a unified time reference. The topology map generation module is used to perform joint feature extraction on the point cloud data and depth image in the multimodal synchronous dataset, identify the pipe cross-sectional geometric parameters and topology nodes, and generate a pipe topology map in an incremental manner. The fusion positioning module is used to input the pre-integration constraints of the inertial measurement unit, the displacement constraints of the odometry, the visual observation constraints, and the point cloud cross-section registration constraints into the factor graph optimizer to perform joint optimization and solve the problem, and output the optimal pose estimate of the robot. The path planning and obstacle avoidance module is used to perform a global path search based on the pipeline topology map to generate the optimal path node sequence, and to perform local obstacle avoidance trajectory generation based on real-time perception data. The closed-loop detection and relocation module is used to perform matching verification between the current sensing data and historical data. When the closed loop is established, the closed-loop constraint is added to the factor graph optimizer to perform global pose adjustment and correct the cumulative drift error.

2. The autonomous navigation and positioning system for a pipeline robot according to claim 1, characterized in that, Timestamp alignment processing includes: Using the sampling time of the lidar as the reference time, the estimated values ​​corresponding to the reference time are obtained by linear interpolation algorithms for the data from the inertial measurement unit, odometer, depth camera and ultrasonic ranging sensor, respectively, and a spatiotemporally synchronized multimodal sensing data frame sequence is generated.

3. The autonomous navigation and positioning system for a pipeline robot according to claim 1, characterized in that, Identifying pipe cross-sectional geometric parameters and topological nodes includes: The lidar point cloud is segmented, and the cross-sectional diameter, ellipticity, and center position of the current pipe cross-section are extracted using cylindrical or elliptical fitting algorithms. When multiple independent pipe cross-sectional profiles appear in the cross-sectional point cloud of consecutive frames, the current position is marked as a branch node; when the rate of change of direction of the cross-sectional center position sequence exceeds a preset angle threshold, the current position is marked as a bend or corner node; when an independent point cloud cluster that is discontinuous with the pipe wall profile and located in the internal space of the pipe appears in the cross-sectional point cloud, the current position is marked as an obstacle node.

4. The autonomous navigation and positioning system for a pipeline robot according to claim 3, characterized in that, The pipeline topology map is represented by a graph structure, with each topology node as the vertex of the graph, and the pipe segments between adjacent nodes, their lengths, and geometric properties as the edges of the graph. The method also includes: comparing the current cross-sectional geometric parameters with historical cross-sectional data; when the ellipticity deviates from the normal range or the cross-sectional area decreases to below a preset area threshold, marking the location as a deformation point or blockage point, and attaching the corresponding status attributes to the corresponding node or edge of the pipeline topology map.

5. The autonomous navigation and positioning system for a pipeline robot according to claim 1, characterized in that, The factor graph optimizer uses the iSAM2 incremental optimizer, and the confidence of the constraint is represented by the information matrix corresponding to each constraint factor. When the number of matched visual feature points is lower than a preset threshold, the weight of the information matrix corresponding to the visual observation constraint is reduced, while the weight of the information matrix corresponding to the point cloud cross-section registration constraint is increased.

6. The autonomous navigation and positioning system for a pipeline robot according to claim 1, characterized in that, The global path search uses either the A* algorithm or the Dijkstra algorithm. Its input is the set of nodes and the set of weighted edges of the pipeline topology map, as well as the starting node and the target node. The output is the optimal path node sequence. The weight of the weighted edge is obtained by combining the path length and the pipeline throughput attribute. The pipeline throughput attribute is derived from the deformation or blockage status marked in the pipeline topology map.

7. The autonomous navigation and positioning system for a pipeline robot according to claim 1, characterized in that, Local obstacle avoidance trajectory generation includes: During the journey along the optimal path node sequence, real-time sensing data from the depth camera and ultrasonic ranging sensor are continuously received; when a sudden obstacle or a sudden change in pipe diameter is detected ahead, an obstacle avoidance trajectory that satisfies kinematic constraints is generated using the dynamic window method or the time elastic band algorithm. When the obstacle avoidance trajectory causes the robot to deviate from the optimal path node sequence by more than a preset distance threshold, the global path search is re-executed starting from the current position to update the subsequent path node sequence.

8. The autonomous navigation and positioning system for a pipeline robot according to claim 1, characterized in that, Match verification includes: The cross-sectional geometric features of the current frame are compared with the cross-sectional geometric features of historical key frames to filter candidate closed-loop frames. Only candidate frames that exceed the preset minimum interval are included in the candidate set. The candidate closed-loop frames are verified by point cloud cross-sectional shape similarity, visual bag-of-words model matching, and closure consistency of inertial measurement unit trajectory extrapolation. When all three verifications pass the preset threshold, the closed loop is confirmed to be valid. The point cloud cross-sectional shape similarity is obtained by calculating the cosine similarity after performing Z-score normalization on the cross-sectional geometric parameter vector. The cross-sectional geometric parameter vector includes the cross-sectional diameter, ellipticity, and the lateral and longitudinal coordinates of the cross-sectional center position.

9. The autonomous navigation and positioning system for a pipeline robot according to claim 1, characterized in that, Also includes: When real-time location is lost, the current consecutive multi-frame cross-sectional geometric feature sequence and visual feature sequence are globally matched with the historical feature sequence stored in the pipeline topology map to obtain a set of candidate locations; Based on the candidate position set as the initial particle distribution, a particle filtering method is used with the current real-time sensing data as the observation input to output the converged optimal pose estimate.

Images