Pipeline robot multi-sensor data fusion path planning method
By integrating multi-sensor data and optimizing path planning, the problems of path planning accuracy and obstacle avoidance response of pipeline robots in complex environments were solved, achieving efficient and smooth path execution.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- ZHICHENG MANUFACTURING (BEIJING) TECHNOLOGY CO LTD
- Filing Date
- 2025-12-09
- Publication Date
- 2026-05-12
AI Technical Summary
Existing pipeline robots suffer from low path planning accuracy, slow obstacle avoidance response, and unsmooth motion control in complex geometries and dynamic environments.
A multi-sensor data fusion path planning method is adopted. By combining the spatiotemporal calibration and semantic feature extraction of lidar and inertial measurement unit with global path planning and local dynamic adjustment mechanism, a three-dimensional semantic map is constructed, and path smoothing optimization is performed by using Bézier curves and polynomial fitting.
It improves the positioning accuracy and obstacle avoidance response speed of pipeline robots in complex environments, ensures the smoothness of the path and the stability of execution, and enhances planning efficiency and safety.
Smart Images
Figure CN121521150B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of pipeline robot technology, specifically to a multi-sensor data fusion path planning method for pipeline robots. Background Technology
[0002] As a core infrastructure for energy transportation, municipal drainage, and industrial production, the safe operation and maintenance of pipelines are of great significance to the stable operation of the social economy. With the widespread application of pipeline robots in inspection and maintenance scenarios, their path planning capabilities have become crucial in determining operational efficiency and safety. However, the pipeline environment is characterized by its enclosed nature, topological complexity (such as multiple branches, bends, and diameter changes), and dynamic disturbances (such as temporary obstacles and water flow impacts), which impose significant limitations on existing technologies.
[0003] Traditional path planning algorithms, such as Dijkstra and other algorithms are mature in structured environments, but they do not consider the mechanical constraints of pipe bends and often plan sharp bends that are not feasible. While algorithms can quickly search for paths, they are prone to path oscillations within narrow pipes, leading to decreased planning efficiency. Intelligent optimization algorithms, such as genetic algorithms, improve path optimality through group optimization, but they lag in response to dynamic obstacles and require excessively long replanning times. Reinforcement learning methods, while showing adaptability in unknown environments, suffer from high training costs, weak generalization ability, and significant performance degradation after changing pipe parameters. Therefore, there is an urgent need for a pipe robot path planning method that balances environmental adaptability, planning efficiency, and robustness. Summary of the Invention
[0004] To address the problems of low path planning accuracy, slow obstacle avoidance response, and unsmooth motion control in existing pipeline robots in complex geometries and dynamic environments, this invention provides a multi-sensor data fusion path planning method and system for pipeline robots.
[0005] The first aspect of this invention provides a multi-sensor data fusion path planning method for a pipeline robot, comprising: constructing a pipeline robot path planning system, the system including an environment perception module, a semantic map construction module, a global path planning module, a local dynamic adjustment module, and a path execution conversion module; configuring a pipeline environment semantic modeling method based on multi-source data fusion using the semantic map construction module, and performing spatiotemporal calibration and semantic feature extraction of multi-sensor data; constructing a global path planning algorithm that fuses pipeline characteristics and a local adjustment mechanism for dynamic obstacle avoidance based on the global path planning module and the local dynamic adjustment module, respectively, and calculating a travel route that conforms to the pipeline geometric constraints; and establishing a path smoothing optimization model based on the path execution conversion module, mapping the planned path coordinates to the robot's underlying control commands.
[0006] Specifically, in the spatiotemporal calibration and semantic feature extraction stage of multi-sensor data, in the time dimension, the nearest neighbor matching method is used to synchronize the lidar data and inertial measurement data collected by the environmental perception module, and time synchronization error constraints are set to ensure the alignment accuracy of multi-source data on the time axis. In the spatial dimension, lidar data and inertial measurement data are mapped to a unified coordinate system, and the position data is fused using the extended Kalman filter algorithm. Consistency constraints for spatial registration are set to eliminate spatial errors caused by differences in sensor installation positions. Based on the calibrated data, semantic features are extracted and a map is constructed according to a three-level structure of pipe segments, obstacles, and constraints. Among them, the pipe segment type is identified, including straight pipes, curved pipes, and branch pipes. For curved pipes, the turning angle is recorded. Obstacle attributes are extracted, labeled as static or dynamic types, and their size and position coordinates are recorded. Traffic constraints are generated, and the maximum turning angle and minimum radius of curvature are calculated and recorded. At the same time, changes in obstacle features are monitored. When a new obstacle size is detected that exceeds a preset threshold, a local map update is triggered.
[0007] During the path planning phase, the global path planning algorithm is configured as follows: The algorithm dynamically adjusts the heuristic function for different pipeline segment types: for straight pipeline segments, the Euclidean distance is calculated as the heuristic function value; for curved pipeline segments, a curvature penalty factor is introduced based on the Euclidean distance, which is determined by the curve weight coefficient and the curve curvature; for branch pipeline segments, a risk aversion factor is introduced based on the Euclidean distance, which is determined by the branch priority coefficient and the branch risk degree. Simultaneously, a comprehensive cost function is constructed, which is a weighted sum of travel distance cost, friction cost, and turning cost, to comprehensively consider the physical characteristics of the pipeline environment at the planning level.
[0008] During the dynamic obstacle avoidance phase, the local adjustment mechanism is configured as follows: when the visual sensor in the environmental perception module detects a dynamic obstacle, a scrolling window is established centered on the robot's current position. The side length of the scrolling window is calculated based on the robot's current travel speed, preset braking time, and bilateral safety redundancy distance. Within the scrolling window, a fast expanding random tree algorithm is used to perform local path search. Furthermore, a third-order Bézier curve is used to smooth the search results to achieve the connection between the global path and the local path, constraining the continuity of the first and second derivatives at the connection point, and limiting the maximum turning angular velocity.
[0009] During the path execution conversion phase, the path smoothing optimization model employs a fifth-order polynomial fitting to eliminate sharp angles in the path and sets a smoothness constraint on the rate of curvature change, defined as the derivative of the path curvature with respect to the path arc length, to prevent excessive angular acceleration. The smoothed path coordinates are then converted into travel speed and steering angle commands. Based on the conversion accuracy verification formula, the relative error between the theoretical command value calculated by the model and the actual command value issued to the actuator is calculated to verify whether the relative error meets the preset accuracy requirements.
[0010] A second aspect of the present invention provides a path planning system for a pipeline robot, the system comprising an environment perception module, a semantic map construction module, a global path planning module, a local dynamic adjustment module, and a path execution conversion module.
[0011] The environmental perception module is configured to collect pipeline environmental data and transmit it to the semantic map construction module. The data includes LiDAR data, inertial measurement data, and visual image data. The semantic map construction module is configured to perform spatiotemporal calibration and semantic feature extraction based on the received data, construct a 3D semantic map containing pipeline segment types, obstacle attributes, and passage constraints, and transmit the 3D semantic map to the global path planning module. The global path planning module is configured to generate an initial global path based on the 3D semantic map and apply differentiated heuristic functions for straight pipelines, curved pipelines, and branch pipelines during the calculation process. The local dynamic adjustment module maintains bidirectional communication with the global path planning module and is configured to calculate rolling window parameters based on the robot's motion state when a dynamic obstacle is detected, and perform local path replanning and smooth connection processing within the window. The path execution conversion module is configured to receive the planned path, apply polynomial fitting and curvature change rate constraints for smooth optimization, and map the path coordinates to the robot's low-level control commands.
[0012] This invention solves the positioning drift problem of a single sensor in a pipeline environment with weak texture by combining multi-source data fusion technology with the high-precision ranging of lidar and the attitude perception of an inertial measurement unit; The algorithm dynamically adjusts the heuristic function based on the geometric features of the pipeline, preventing the robot from getting trapped in local optima in complex pipe networks. Through a rolling window based on dynamic speed adjustment and a fast-expanding random tree algorithm, it achieves rapid response and avoidance of dynamic obstacles. Finally, through strict smoothness constraints, it ensures that the robot moves smoothly when performing path tracking, thereby protecting the actuator and improving travel efficiency.
[0013] This invention provides a multi-sensor data fusion path planning method for pipeline robots. It has the following beneficial effects:
[0014] 1. This invention effectively solves the problem of single-sensor positioning drift caused by the weak texture environment inside pipelines through multi-source data fusion and a rigorous spatiotemporal calibration mechanism. By precisely aligning the spatial data of the LiDAR with the attitude data of the inertial measurement unit in both time and space dimensions, and constructing a three-dimensional semantic map that includes the geometric properties of the pipeline and passage constraints, the robot's environmental perception capability and positioning accuracy in complex pipeline environments are improved.
[0015] 2. This invention employs a strategy combining global path planning optimized for pipeline characteristics with local dynamic obstacle avoidance, thereby improving the rationality and safety of path planning. In global planning... The algorithm dynamically adjusts the heuristic function based on the differences between straight pipes, curved pipes, and branch pipes to avoid getting trapped in local optima. The local adjustment mechanism utilizes a rolling window based on dynamic speed calculation and a fast expanding random tree algorithm to quickly generate obstacle avoidance paths when dynamic obstacles are detected, balancing the efficiency of long-distance navigation with the response speed in sudden situations.
[0016] 3. This invention establishes a path execution transformation model that includes smoothness constraints, ensuring the executability of the planned path at the underlying control level. Through fifth-order polynomial fitting and third-order Bézier curve smoothing, acute angles in the path are eliminated, and a curvature change rate constraint is introduced to limit the maximum angular acceleration, preventing the risk of jitter during robot turning, thereby extending the service life of the actuator and ensuring the smoothness of the travel process. Attached Figure Description
[0017] Figure 1 This is a flowchart of a pipeline robot path planning method according to the present invention;
[0018] Figure 2 This is a classification diagram of the solution algorithm described in this invention;
[0019] Figure 3 This is a schematic diagram of the path planning algorithm described in this invention. Detailed Implementation
[0020] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0021] Please see the appendix Figure 1 - Appendix Figure 3 This invention provides a multi-sensor data fusion path planning method for a pipeline robot, comprising the following steps:
[0022] S1. Build a path planning system for the pipeline robot, which includes an environmental perception module, a semantic map construction module, a global path planning module, a local dynamic adjustment module, and a path execution conversion module.
[0023] The environment perception module and the semantic map construction module are connected for data transmission. The environment perception module collects lidar data, inertial measurement data and visual image data, and transmits the data to the semantic map construction module.
[0024] The semantic map construction module is connected to the global path planning module. The semantic map construction module constructs a three-dimensional semantic map containing obstacle attributes based on the received data, and transmits the three-dimensional semantic map to the global path planning module.
[0025] The global path planning module and the local dynamic adjustment module maintain bidirectional communication. The global path planning module generates an initial global path based on the map and transmits it to the local dynamic adjustment module. At the same time, it receives replanning status information fed back by the local dynamic adjustment module.
[0026] The local dynamic adjustment module is connected to the path execution conversion module, and the local dynamic adjustment module transmits the target path after dynamic obstacle avoidance adjustment to the path execution conversion module.
[0027] S2. Based on the semantic map construction module in the system described in step S1, configure a pipeline environment semantic modeling method for multi-source data fusion to perform spatiotemporal calibration and semantic feature extraction of multi-sensor data;
[0028] S3. Based on the global path planning module and local dynamic adjustment module in the system described in step S1, construct a global path planning algorithm that integrates pipeline characteristics and a local adjustment mechanism for dynamic obstacle avoidance, respectively, to calculate the travel route that conforms to the pipeline geometric constraints.
[0029] S4. Based on the path execution conversion module in the system described in step S1, establish a path smoothing optimization model to map the planned path coordinates into the robot's underlying control commands.
[0030] S5. During system operation, the environment perception module collects pipeline environment data and transmits it to the semantic map construction module to generate a three-dimensional semantic map. The global path planning module generates an initial path based on the semantic map. The local dynamic adjustment module performs path replanning when dynamic obstacles are detected. The path execution conversion module converts the optimized path into robot control commands.
[0031] The specific hardware configuration of the environment perception module mentioned in step S1 includes:
[0032] The lidar is configured with a sampling frequency of ≥10Hz and a ranging accuracy of ≤±2mm.
[0033] Inertial measurement unit (IMU) equipped with gyroscope zero-bias stability ≤0.1° / h;
[0034] Visual sensor with a resolution of ≥1280×720 pixels and a frame rate of ≥25fps;
[0035] To facilitate subsequent multi-source data fusion calculations, the set of environmental perception modules is defined as follows: ,in For the first A sensing device. This represents the total number of devices;
[0036] The global path planning module and the local dynamic adjustment module are deployed on the same computing platform. The computing platform uses an industrial-grade embedded processor with a computing power of ≥2TOPS to ensure that the calculation response time of the local dynamic adjustment module when dealing with dynamic obstacles is ≤0.1s.
[0037] The method for configuring multi-source data fusion for pipeline environment semantic modeling described in step S2 specifically includes the following steps:
[0038] S2.1 Multi-source data acquisition: The control environment perception module performs data acquisition, obtains point cloud data of pipe inner diameter and obstacle distance from LiDAR, robot attitude angle data from IMU, and loads pre-stored pipe CAD drawings in the semantic map construction module to obtain the initial topology.
[0039] S2.2, Data Spatiotemporal Calibration: Perform spatiotemporal alignment processing on multi-sensor data.
[0040] In the time dimension, the nearest neighbor matching method is used to synchronize the data frames of the lidar and the IMU, and the time synchronization error constraint is set as follows:
[0041]
[0042] in, This represents the time synchronization error; Represents the timestamp of lidar sampling; This is the IMU sampling timestamp; 5ms represents the maximum allowed time deviation threshold of the system.
[0043] In the spatial dimension, the lidar data and IMU data are respectively mapped to a unified coordinate system defined by the preset pipeline CAD drawing, and the position data are fused using the extended Kalman filter algorithm. The consistency constraint condition for spatial registration is set as follows:
[0044]
[0045] in, Represents spatial registration error; This refers to the lidar positioning data mapped to a unified coordinate system. This refers to IMU positioning data mapped to a unified coordinate system. This represents the maximum permissible spatial positioning deviation threshold of the system;
[0046] S2.3 Semantic Feature Extraction and Map Construction: Based on the calibrated data, semantic features are extracted and a map is constructed according to a three-level structure of pipeline segments, obstacles, and constraints.
[0047] Identify pipe segment types, including straight pipes and curved pipes (record the turning angle). ,satisfy T-type and Y-type branch pipes;
[0048] Extract obstacle attributes, label them as static or dynamic, and record their dimensions (diameter). ,length and location coordinates;
[0049] Generate traffic constraints, calculate and record the maximum turning angle. Minimum radius of curvature ;
[0050] S2.4. Map Update Mechanism: Monitors changes in obstacle features; when a new obstacle size is detected... When the temperature reaches mm, a local map update is triggered, and the update frequency is consistent with the sampling frequency of the environment perception module.
[0051] Furthermore, the construction of the global path planning algorithm that integrates pipeline characteristics and the local adjustment mechanism for dynamic obstacle avoidance described in step S3 specifically includes the following steps:
[0052] Global path planning algorithm design: configuration The algorithm dynamically adjusts the heuristic function for different pipe segment types.
[0053] For a straight pipe section, calculate the Euclidean distance:
[0054]
[0055] in, This represents the estimated cost (heuristic value) from the current node to the target node. The coordinates of the target point, The coordinates of the current node;
[0056] For curved pipe sections, a curvature penalty factor is introduced:
[0057]
[0058] in, These are the weighting coefficients. The curvature of the curve (r is the turning radius);
[0059] For branch pipeline sections, a risk aversion factor is introduced:
[0060]
[0061] in, This is the branch priority coefficient. Branch risk level ( A higher value indicates a higher risk level in the current area, which should be avoided as a priority.
[0062] Constructing a comprehensive cost function
[0063]
[0064] in, This represents the actual total cost of the current node. For the cost of travel distance, Friction cost (value is positively correlated with the roughness of the pipe material). For steering costs (the value is positively correlated with the turning angle);
[0065] S3.2. Local Dynamic Adjustment Mechanism: When the vision sensor detects a dynamic obstacle, a scrolling window is established with the robot's current position as the center, and the window side length is calculated.
[0066]
[0067] in, This represents the side length of the scrolling window; The robot's current speed (unit: m / s) To preset the braking time, The grid size is set to 0.1m. Represents the safety redundancy distance on both sides;
[0068] S3.3. Local Replanning and Smooth Connection: Within the rolling window, the RRT algorithm is used for local path search, with a sampling frequency of 100Hz. The search results are smoothed using a third-order Bézier curve to achieve connection between the global and local paths. The first derivative (tangent direction) and second derivative (rate of curvature change) at the connection point are constrained to be continuous, and the maximum turning angular velocity is limited. satisfy .
[0069] Furthermore, the establishment of the path smoothing optimization model in step S4 specifically includes the following steps:
[0070] S4.1. Path Smoothing Optimization: A fifth-order polynomial fitting is used to eliminate sharp angles in the path, and the smoothness constraint of the rate of change of curvature is set as follows:
[0071]
[0072] in, is the derivative of curvature with respect to arc length (rate of change of curvature), k is the path curvature, and s is the path arc length (used to prevent excessive angular acceleration when the robot turns).
[0073] S4.2. Control Command Conversion: Convert smooth path coordinates into travel speed and steering angle commands. Set the conversion accuracy verification formula as follows:
[0074]
[0075] in, This represents the actual relative error of the conversion. The theoretical command value calculated by the model. The actual command values (including speed commands and steering commands) issued to the actuators. The maximum allowable relative error threshold;
[0076] S4.3. Performance Constraint Settings: Set the path evaluation index: the maximum deviation between the planned path and the pipeline centerline. Must meet:
[0077]
[0078] in, This represents the actual path deviation. The diameter of the pipe; This is the deviation tolerance ratio coefficient;
[0079] Deviation between total path length and the theoretical shortest path Must meet:
[0080]
[0081] in, This represents the deviation from the actual path length. This is the theoretical shortest path length. This is the tolerance coefficient for length deviation.
[0082] Furthermore, the specific implementation method of step S5 includes the following steps:
[0083] S5.1. After starting the pipeline robot path planning system, first complete the parameter initialization of each module: the environmental perception module sets the LiDAR sampling frequency to 10Hz, the IMU sampling frequency to 100Hz, and the vision sensor frame rate to 25fps; the semantic map construction module loads the preset pipeline CAD drawings (including basic parameters such as pipeline topology, initial pipe diameter, and material roughness); the global path planning module imports the target point coordinates (such as the detection endpoint and maintenance point) and robot mechanical constraint parameters (maximum turning angle). Minimum radius of curvature The local dynamic adjustment module sets the dynamic obstacle detection threshold (e.g., a moving speed ≥ 0.1 m / s is considered a dynamic obstacle); the path execution conversion module configures the command output cycle to 10 ms and initializes the robot's initial speed. Initial steering angle .
[0084] S5.2. The environmental sensing module synchronously collects data at a preset frequency: the lidar outputs one set of pipe inner diameters every 100ms (e.g., ...). The IMU outputs obstacle distance data (e.g., 1.2m from a static obstacle) every 10ms; it also outputs a set of robot attitude angles (pitch angle) every 10ms. Roll angle Heading angle The data is processed by the visual sensor, which outputs one frame of image of the pipe's inner wall and obstacles every 40ms. The acquired data is then transmitted to the preprocessing unit: lidar data undergoes median filtering to remove impulse noise, IMU data undergoes Kalman filtering to suppress drift error, and visual images undergo grayscale conversion and edge enhancement to highlight obstacle outlines. The preprocessed data is then transmitted to the semantic map construction module in a standardized format (e.g., JSON), with transmission latency controlled to ≤10ms.
[0085] S5.3. The semantic map construction module receives preprocessed multi-source data and updates it in real time according to a three-level structure of pipeline segments, obstacles, and constraints.
[0086] Pipeline segment update: Combining the inner diameter changes measured by lidar (e.g., the inner diameter of a variable-diameter section decreases from 300mm to 200mm) with IMU attitude data, the topology coordinates of the pipeline segment are corrected to ensure that the linearity deviation of straight pipeline segments is ≤1mm / m and the curvature deviation of curved pipeline segments is ≤1mm / m. ;
[0087] Obstacle Update: By fusing visual sensor image recognition (using a lightweight YOLOv8 model, single-frame recognition time ≤20ms) with LiDAR distance data, the obstacle type (static / dynamic) and size (e.g., dynamic obstacles are...) are determined. The new obstacle (sphere) and its real-time coordinates are updated. If the size of the new obstacle is greater than or equal to 30 mm, a local map update is triggered (the update range is within 5 m around the obstacle). The update frequency is synchronized with the sensor sampling frequency.
[0088] Constraint Update: Adjust the friction cost coefficient based on the measured roughness value of the pipe material (e.g., roughness 0.4 for metal pipes, roughness 0.8 for concrete pipes), and correct the maximum turning angle constraint (e.g., within an inclined pipe) based on the robot's current posture. (The temperature drops to 25°). The updated 3D semantic map is pushed to the global path planning module in real time via memory sharing, with a map data refresh cycle of ≤50ms.
[0089] S5.4. The global path planning module receives the updated semantic map and triggers [a specific action] when the system starts or the target point changes. Algorithm generates initial path:
[0090] Path search: Starting from the robot's current coordinates and ending at the preset target point, expand nodes according to the differences in heuristic functions for straight pipe segments, curved pipe segments, and branch pipe segments (expand only in the three directions of "forward, left turn, and right turn, excluding the backward direction), calculate the cost function g(n) of each node, and select the node with the lowest cost as the next hop until the target point is found;
[0091] Path verification: Perform a mechanical feasibility check on the generated initial path, checking whether the curvature of all turning segments in the path meets the requirements. (e.g., the curvature of a bend in the pipe section) ), whether the deviation between the path and the pipeline centerline is (e.g., deviation under a pipe diameter of 300mm) If the verification passes, the initial path is transmitted to the local dynamic adjustment module; if the verification fails (e.g., due to the existence of a sharp bend in the path), the heuristic function weight coefficients are readjusted. (If the value increases from 0.4 to 0.5), the path search is repeated until the verification passes, and the total global planning time is controlled within [a certain limit]. (Scene of a 100-meter pipeline).
[0092] S5.5. The local dynamic adjustment module receives the global initial path and obstacle detection results from the visual sensor in real time:
[0093] Dynamic obstacle detection: When the vision sensor detects an obstacle with a moving speed ≥ 0.1 m / s or a position change ≥ 5 mm / s, it is identified as a dynamic obstacle, and the real-time distance between the obstacle and the robot is calculated. ;
[0094] Replanning Triggered: If (L is the side length of the scroll window; for example, when L=2.15m, the trigger threshold is ≤1.075m). Immediately initiate local replanning: centered on the robot's current position, according to the formula... Calculate the window side length (e.g., when the current speed v=0.3m / s, L=0.3×0.5+2=2.15m, corresponding to 22 0.1m grids), and use the RRT algorithm to perform local path search within the window (sampling frequency 100Hz, generating 1 candidate path every 10ms).
[0095] Path Connection and Verification: Candidate local paths are connected to the global path using a third-order Bézier curve. The first derivative (tangent direction) and second derivative (rate of curvature change) at the connection point are calculated to ensure... rate of change of curvature After successful verification, the replanned complete path (global path + local connection path) is transmitted to the path execution and transformation module. The replanning time is controlled within [timeframe missing]. If the verification fails, adjust the sampling range of the RRT algorithm (reduce it by 10%) and search again. If the search fails 3 times in a row, trigger the emergency mechanism (decelerate the robot to 0.1m / s and expand the scrolling window range by 20%).
[0096] S5.6. The path execution conversion module receives the complete path after replanning and performs instruction conversion and execution according to the following steps:
[0097] Path discretization: Discretize the continuous path into a sequence of path points at 10ms time intervals (e.g., one path point per 10ms). ), calculate the distance difference between adjacent path points With angle difference ;
[0098] Control command generation: based on Calculate travel speed (Ensure 0 ≤ v ≤ 0.5 m / s), according to Calculate the steering angle (to ensure) The PID control algorithm generates low-level commands such as motor speed and servo angle, and the command conversion error is minimized. ;
[0099] Command issuance and feedback: The generated control commands are sent to the robot actuators (motors, servos) via the CAN bus, with a issuance cycle of 10ms; at the same time, the real-time status of the actuators (such as actual speed and steering angle) is sent back to the path execution conversion module through the feedback interface. If the deviation between the actual value and the command value is >5%, command compensation is triggered (such as adjusting the motor PWM duty cycle) to ensure that the path tracking accuracy is ≤2mm.
[0100] S5.7. During system operation, the monitoring module collects key indicators of each module in real time: the validity of sensor data of the environmental perception module (e.g., the validity rate of LiDAR data ≥99%), the update success rate of the semantic map construction module (≥98%), the planning success rate of the global path planning module (≥99%), the obstacle avoidance success rate of the local dynamic adjustment module (≥95%), and the instruction execution accuracy of the path execution conversion module.
[0101] All operational data (such as data collection timestamps, path coordinates, command values, and abnormal events) are stored in a log format (such as CSV) to a local storage unit. The log storage period is ≥30 days, which facilitates subsequent troubleshooting and algorithm optimization.
[0102] Example 1: This implementation method is for ordinary pipelines with constant diameter. Based on the above method, it specifically includes:
[0103] S1. Establish a path planning system for the pipeline robot, including an environmental perception module (containing a 10Hz LiDAR, a 100Hz IMU, and a 25fps visual sensor), a semantic map construction module, a global path planning module (computing power of 2TOPS), a local dynamic adjustment module (response time of 0.1s), and a path execution conversion module (output cycle of 10ms); the environmental perception module set includes a LiDAR, an IMU, and a visual sensor, with m=3;
[0104] S2. Establish a semantic modeling method for pipeline environments based on multi-source data fusion;
[0105] Furthermore, the specific implementation method of step S2 includes the following steps:
[0106] S2.1. Data Acquisition: LiDAR acquires pipe inner diameter of 300mm and obstacle distance data; IMU acquires attitude angle (error ≤ 0.1°); Import CAD drawings containing two 90° bends (turning radius r = 500mm) and one T-shaped branch;
[0107] S2.2. Spatiotemporal calibration: control After Kalman filtering ;
[0108] S2.3. Semantic annotation: Straight pipe sections are labeled "Roughness 0.6", and curved pipe sections are labeled "Curvature k = Static obstacles are labeled "d=50mm, l=100mm, coordinates (x1, y1, z1)", and traffic constraints are labeled " ”;
[0109] S2.4. Update Trigger: When a new obstacle d≥30mm is detected, a local map update is triggered;
[0110] S3. Construct a global path planning algorithm and local adjustment mechanism that integrates pipeline characteristics:
[0111] Furthermore, the specific implementation method of step S3 includes the following steps:
[0112] S3.1. algorithm:
[0113] Heuristic function for curved pipe segments Branch pipe h(n) = Euclidean distance × (1 + 0.2 × 0.6) (P = 0.6);
[0114] Cost function ;
[0115] S3.2. Scrolling window: When the robot speed v = 0.3 m / s, the window side length... (22 grids);
[0116] S3.3. Local Replanning: The RRT algorithm has a sampling frequency of 100Hz, and the Bezier curve connection is ensured. ;
[0117] S4. Establish a path smoothing optimization model;
[0118] Furthermore, the specific implementation method of step S4 includes the following steps:
[0119] S4.1. Fifth-order polynomial fitting, control ;
[0120] S4.2. Conversion speed ≤ 0.3m / s, turning angle ≤ 30°, conversion error ;
[0121] S4.3. Constraints (5%×300mm) ;
[0122] S5. During system operation, the environmental perception module collects data in real time, and the semantic map construction module dynamically updates the 3D semantic map; the global path planning module generates an initial path within 1 second in a 100-meter pipeline scene; when the visual sensor detects a dynamic obstacle (a sphere with d=60mm), the local dynamic adjustment module completes replanning within 0.1 seconds; the path execution conversion module outputs control commands to enable the robot to smoothly avoid obstacles and move efficiently.
[0123] Furthermore, the specific implementation method of step S5 includes the following steps:
[0124] S5.1. System Initialization: Set the LiDAR to 10Hz, IMU to 100Hz, and the vision sensor to 25fps; load the CAD drawing (300mm pipe diameter, two 90° bends); import the target point coordinates (detection point at 100m); set the dynamic obstacle detection threshold to 0.1m / s; command cycle to 10ms, initial speed to 0m / s, and turning angle to 0°.
[0125] S5.2. Data Acquisition and Preprocessing: The LiDAR outputs data with an inner diameter of 300mm and a distance of 1.2m from a static obstacle every 100ms, and performs median filtering for noise reduction; the IMU outputs attitude angles (γ=0.3°, φ=0.2°, ψ=90°) every 10ms, and performs Kalman filtering for drift suppression; the vision system outputs images every 40ms, and performs grayscale processing; the preprocessed data is transmitted in JSON format with an 8ms delay.
[0126] S5.3 Map Update: Corrected pipe curvature to Visually recognize dynamic obstacles (φ60mm sphere, speed 0.2m / s) and trigger local updates (range 5m); adjust the friction coefficient according to the roughness of the concrete pipe (0.8); refresh the map every 50ms and push it to the global module.
[0127] S5.4. Global Path Generation: Starting from the current coordinates (0,0,0) and ending at (100,0,0), Algorithm search path; Verification of pipe curvature Deviation ≤ 12mm ≤ 15mm, planning time 0.8s, output initial path;
[0128] S5.5. Local Replanning: When a dynamic obstacle is detected within 1.0m to 1.075m of the robot, replanning is triggered; the window side length is 2.15m, the RRT algorithm samples at 100Hz, and a local path is generated; the Bezier curve is connected, the turning angular velocity is 4° / s to 5°, the replanning time is 0.08s, and the complete path is output;
[0129] S5.6. Command Conversion and Execution: The discrete path has a 10ms interval point, the calculation speed is 0.3m / s, and the turning angle is 20°; the CAN bus sends commands and the feedback actual speed is 0.297m / s (error 1%) and the turning angle is 19.8° (error 1%), and the command accuracy meets the standard;
[0130] S5.7. Monitoring and Logs: Record sensor effectiveness of 99.5% and obstacle avoidance success rate of 96%. Logs are stored in CSV format and retained for 30 days.
[0131] Example 2: This implementation method is for pipeline diameter variation scenarios (common in municipal drainage and industrial fluid transport pipelines), verifying the adaptability of the method to different pipe diameters. The specific steps are as follows:
[0132] S1. System Setup: The environmental perception module uses a 10Hz LiDAR (range accuracy ±1.5mm), a 100Hz IMU (positioning accuracy 0.08° / h), and a 25fps visual sensor; the global path planning module has a computing power of 2.5 TOPS; the local dynamic adjustment module has a response time ≤0.09s; and the environmental perception module is integrated.
[0133] S2. Semantic modeling adapts to variable path scenarios:
[0134] Furthermore, the specific implementation method of step S2 includes the following steps:
[0135] S2.1. Data Acquisition: The lidar acquires data on the pipe diameter before the change of diameter (400mm) and after the change of diameter (200mm after the change of diameter, with a change of diameter section length of 2m), as well as static obstacles (φ80mm×150mm metal blocks, 3m from the entrance of the change of diameter section); the IMU acquires the robot's attitude angle (tilt angle γ=1.2°) when passing through the change of diameter section; and imports CAD drawings containing the change of diameter section (change of diameter section taper 1:5).
[0136] S2.2. Spatiotemporal calibration: Controlled within 4ms, after Kalman filtering (Higher spatial accuracy is required for the variable diameter section);
[0137] S2.3. Semantic annotation: The front section of the diameter change is labeled "Inner diameter 400mm, roughness 0.5", the diameter change section is labeled "Taper 1:5, minimum radius of curvature r=400mm", the rear section of the diameter change is labeled "Inner diameter 200mm, roughness 0.7", the static obstacle is labeled "d=80mm, coordinates (45,0,0)", and the passage constraint is labeled "diameter change section". ”;
[0138] S2.4. Update mechanism: An update is triggered when a new obstacle d≥20mm appears in the diameter change section (smaller diameter pipes are more sensitive to obstacles).
[0139] S3. Performance Constraint Adaptation:
[0140] Furthermore, the specific implementation method of step S3 includes the following steps:
[0141] S3.1. Deviation of the front section of the variable diameter section (5%×400mm), diameter reduction section ≤10mm (5%×200mm);
[0142] S3.2. Command conversion: The speed in the variable diameter section is controlled at 0.15-0.2 m / s, the steering angle is ≤25°, and the conversion error is ≤0.8%;
[0143] S4. System Operation:
[0144] Furthermore, the specific implementation method of step S4 includes the following steps:
[0145] S4.1. Initialization: Load the variable diameter CAD drawing, target point coordinates (80,0,0) (50m from the end of the variable diameter section), variable diameter section. ;
[0146] S4.2. Data Acquisition: The lidar outputs the change in the inner diameter of the variable diameter section every 100ms, the IMU monitors the attitude angle γ≤±1.5°, and the visual system identifies the position of static obstacles;
[0147] S4.3. Map Update: Real-time correction of topological coordinates of variable diameter sections to ensure taper deviation ≤ 0.1° and obstacle coordinate error ≤ 2mm;
[0148] S4.4. Global Planning: The algorithm generates a path through the variable diameter segment and verifies the curvature at the variable diameter point. The deviation was 18mm (front section of the diameter change) and 9mm (rear section of the diameter change), with a planning time of 0.9s.
[0149] S4.5. Local adjustment: There are no dynamic obstacles in the variable diameter section. Only the smoothness of the path is checked to ensure that the steering angular velocity is 3.5° / s≤5°.
[0150] S4.6. Command execution: Variable diameter section speed 0.18m / s, steering angle 22°, actual feedback speed 0.178m / s (error 1.1%), path tracking accuracy 1.5mm;
[0151] S4.7. Monitoring: Sensor efficiency is 99.2%, path pass rate of variable diameter section is 100%, and log records key parameters of variable diameter section (such as curvature and speed).
[0152] Example 3: This implementation method verifies the branch selection and multi-interference handling capabilities of the method for Y-shaped branches (common in energy pipeline distribution scenarios) and multiple dynamic obstacles (such as floating foreign objects in the pipeline). The specific steps are as follows:
[0153] S1. System Setup: The environmental perception module adds one side-looking LiDAR (S4, 10Hz, ranging accuracy ±2mm) to achieve full coverage of branch pipelines; the global path planning module supports dynamic adjustment of branch priority; the local dynamic adjustment module supports priority sorting of multiple obstacles.
[0154] S2. Semantic modeling adapts to Y-shaped branches:
[0155] Furthermore, the specific implementation method of step S2 includes the following steps:
[0156] S2.1. Data Acquisition: Main pipe inner diameter 350mm, left branch inner diameter 300mm (high-risk area, P=0.3), right branch inner diameter 280mm (low-risk area, P=0.7); 2 dynamic obstacles: O1 (φ70mm sphere, velocity in main pipe 0.15m / s), O2 (φ65mm cylinder, velocity at left branch inlet 0.12m / s); Import Y-shaped branch CAD drawing (branch angle 60°);
[0157] S2.2. Spatiotemporal calibration: Time synchronization error of 4 sensors ≤ 5ms, spatial deviation ;
[0158] S2.3. Semantic annotation: The left branch is labeled "P=0.3, roughness 0.6", the right branch is labeled "P=0.7, roughness 0.7", O1 is labeled "dynamic, velocity 0.15m / s, coordinate (30,0,0)", and O2 is labeled "dynamic, velocity 0.12m / s, coordinate (45,25,0)".
[0159] S2.4. Update Mechanism: The update frequency of obstacles within 5m of the branch entrance is increased to 50Hz;
[0160] S3. Algorithm adaptability to multiple dynamics and branches:
[0161] Furthermore, the specific implementation method of step S3 includes the following steps:
[0162] S3.1. Algorithm: The branch turning cost weight in the cost function is increased to 0.25, i.e. ;
[0163] S3.2. Scrolling window: Main pipeline speed 0.3m / s (window L=2.15m), branch speed 0.25m / s (window L=2.05m);
[0164] S3.3. Multi-obstacle replanning: sort by distance from nearest to farthest (O1 is 2.5m away from the robot, O2 is 5m away), process O1 first and then O2;
[0165] S4. Performance Constraints:
[0166] Furthermore, the specific implementation method of step S4 includes the following steps:
[0167] S4.1. Branch selection accuracy is 100%, and multi-obstacle avoidance success rate is ≥95%;
[0168] S4.2. The turning angle at the branch entrance is ≤30°, and the rate of change of curvature at the junction is ≤0.5 rad / m²;
[0169] S5. System Operation:
[0170] Furthermore, the specific implementation method of step S5 includes the following steps:
[0171] S5.1. Initialization: The target point is set to 40m on the left branch (high-risk detection point), the dynamic obstacle threshold is 0.1m / s, and the multi-obstacle sorting rule is set to "distance priority";
[0172] S5.2. Data Acquisition: Side-looking lidar monitors the inner diameter of the branch, main lidar tracks the positions of O1 and O2, and IMU records the attitude angle ψ=60° when the branch turns;
[0173] S5.3. Map Update: Real-time push of O1 and O2 coordinates, with the left branch P value maintained at 0.3 and the right branch at 0.7;
[0174] S5.4. Global Planning: The algorithm selects the left branch path, verifies the branch turning angle to be 28°≤30°, the deviation to be 17mm (5%×350mm), and the planning time is 1.0s;
[0175] S5.5. Local Replanning: When the distance from O1 to the robot is detected to be 2.5m ≤ 1.075m (trigger threshold), replanning is initiated with a window L = 2.15m. RRT generates a path around O1, taking 0.07s. Before the robot moves to the left branch entrance, when the distance from O2 is detected to be 3m ≤ 1.025m (trigger threshold within the branch), replanning is initiated with a window L = 2.05m. A path around O2 is generated, taking 0.09s.
[0176] S5.6. Command execution: The speed of the main pipeline around O1 is 0.28 m / s and the turning angle is 22°; the speed of the left branch around O2 is 0.24 m / s and the turning angle is 26°; the feedback error is ≤1%.
[0177] S5.7. Monitoring: Branch selection accuracy 100%, O1 obstacle avoidance success rate 100%, O2 obstacle avoidance success rate 95%, and log records the timing of multi-obstacle processing.
[0178] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.
Claims
1. A path planning method for a pipeline robot using multi-sensor data fusion, characterized in that, Includes the following steps: S1. Build a path planning system for a pipeline robot, the system including an environment perception module, a semantic map construction module, a global path planning module, a local dynamic adjustment module, and a path execution conversion module; S2. Based on the semantic map construction module, configure a pipeline environment semantic modeling method for multi-source data fusion, and perform spatiotemporal calibration and semantic feature extraction of multi-sensor data; S3. Based on the global path planning module and the local dynamic adjustment module, a global path planning algorithm that integrates pipeline characteristics and a local adjustment mechanism for dynamic obstacle avoidance are constructed respectively, and a travel route that conforms to the pipeline geometric constraints is calculated. S4. Based on the path execution conversion module, establish a path smoothing optimization model and map the planned path coordinates into the robot's underlying control commands; S5. During system operation, the environmental perception module collects pipeline environmental data and transmits it to the semantic map construction module to generate a three-dimensional semantic map. The global path planning module generates an initial path based on the semantic map. The local dynamic adjustment module performs path replanning when dynamic obstacles are detected. The path execution conversion module converts the optimized path into robot control commands. In step S3, the global path planning algorithm is configured as the Pipe-A* algorithm, and the heuristic function is dynamically adjusted for different pipeline segment types: For straight pipe segments, calculate the Euclidean distance as the heuristic function value; For curved pipe sections, a curvature penalty factor is introduced based on the Euclidean distance. The curvature penalty factor is determined by the curve weight coefficient and the curve curvature. For branch pipeline sections, a risk aversion factor is introduced based on the Euclidean distance. The risk aversion factor is determined by the branch priority coefficient and the branch risk degree. The Pipe-A* algorithm constructs a comprehensive cost function, which is a weighted sum of travel distance cost, friction cost, and turning cost.
2. The multi-sensor data fusion path planning method for a pipeline robot according to claim 1, characterized in that, In step S2, the spatiotemporal calibration specifically includes: In the time dimension, the nearest neighbor matching method is used to synchronize the lidar data and inertial measurement data collected by the environmental perception module, and time synchronization error constraints are set. In the spatial dimension, the lidar data and inertial measurement data are mapped to a unified coordinate system, the position data are fused using the extended Kalman filter algorithm, and consistency constraints for spatial registration are set.
3. The multi-sensor data fusion path planning method for a pipeline robot according to claim 1, characterized in that, In step S2, the semantic feature extraction specifically includes constructing a map based on a three-level structure of pipeline segments, obstacles, and constraints: Identify pipe segment types, including straight pipes, curved pipes, and branch pipes; for curved pipes, record the turning angle. Extract obstacle attributes, label them as static or dynamic, and record their dimensions and position coordinates; generate passage constraints, calculate and record the maximum turning angle and minimum radius of curvature; Simultaneously monitor changes in obstacle features, and trigger a local map update when a new obstacle's size exceeds a preset threshold.
4. The multi-sensor data fusion path planning method for a pipeline robot according to claim 1, characterized in that, The local adjustment mechanism for dynamic obstacle avoidance specifically includes: When the visual sensor in the environment perception module detects a dynamic obstacle, a scrolling window is established with the robot's current position as the center, and the side length of the scrolling window is calculated based on the robot's current travel speed, preset braking time, and double-sided safety redundancy distance. Within the scrolling window, a fast expanding random tree algorithm is used for local path search.
5. The multi-sensor data fusion path planning method for a pipeline robot according to claim 4, characterized in that, The local adjustment mechanism also includes: using a third-order Bézier curve to smooth the search results in order to connect the global path and the local path, constraining the continuity of the first and second derivatives at the connection point, and limiting the maximum steering angular velocity.
6. The multi-sensor data fusion path planning method for a pipeline robot according to claim 1, characterized in that, In step S4, the path smoothing optimization model uses a fifth-order polynomial fitting to eliminate sharp angles in the path and sets a smoothness constraint on the rate of curvature change, which is defined as the derivative of the path curvature with respect to the path arc length.
7. The multi-sensor data fusion path planning method for a pipeline robot according to claim 1, characterized in that, In step S4, the path execution conversion module is set with a conversion accuracy verification formula. The conversion accuracy verification formula is used to calculate the relative error between the theoretical instruction value calculated by the model and the instruction value actually issued to the execution mechanism, and to verify whether the relative error is less than a preset threshold.
8. A path planning system for a pipeline robot, characterized in that, The system includes an environment perception module, a semantic map construction module, a global path planning module, a local dynamic adjustment module, and a path execution conversion module. The system is configured to execute the multi-sensor data fusion path planning method for pipeline robots as described in any one of claims 1 to 7.