A path planning method and system for an underground pipe gallery inspection robot

Abstract

The application belongs to the technical field of mobile robot path planning, and provides a path planning method and system for an underground pipe gallery inspection robot, which comprises: obtaining an underground pipe gallery environment map; determining the motion state and obstacle information of the inspection robot based on the environment map, and constructing a kinematic model of the inspection robot; performing global path planning of the inspection robot according to the constructed kinematic model to obtain an original global path of the inspection robot; obtaining a global guide path of the inspection robot that satisfies safety constraints according to the original global path; generating a local candidate trajectory in combination with the current state of the inspection robot and the obstacle information based on the obtained global guide path as a guide; performing detection and evaluation of the obstacle information based on the generated local candidate trajectory to obtain an optimal speed control amount of the inspection robot, performing path tracking and dynamic obstacle avoidance of the inspection robot according to the optimal speed control amount, and completing the path planning for the underground pipe gallery inspection robot.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot path planning technology, specifically relating to a path planning method and system for underground utility tunnel inspection robots. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] Underground utility tunnels are long and narrow, with densely packed pipelines, equipment cabinets, supports, and maintenance facilities. They may also contain dynamic targets such as inspection personnel and utility vehicles. Inspection robots need to quickly generate feasible paths from the starting point to the target point while meeting constraints on their own dimensions, kinematics, and safe distances from obstacles, and adjust their speed and direction of movement in real time as the local environment changes. Path planning performance directly impacts the safety, continuity, and efficiency of inspection tasks.

[0004] Currently, RRT The Dynamic Window Approach (DWA) and the Standard Path Avoidance Method (RRT) are widely used in mobile robot path planning due to their respective capabilities of global path search and local real-time obstacle avoidance. However, in environments with dense underground pipelines, narrow passages, and dynamic obstacles, the standard RRT... The algorithm uses random sampling to expand the search tree, lacking directional guidance. This leads to a large number of invalid nodes in narrow passages and areas with dense obstacles, resulting in low search efficiency. The fixed expansion step size struggles to balance rapid searching in open areas with fine-grained expansion near obstacles, and the path cost does not adequately consider environmental risks and safety margins, easily generating paths with many inflection points and close proximity to obstacles, affecting passage safety and motion stability. The standard DWA algorithm selects a local trajectory based on the robot's current state and the instantaneous position of obstacles, lacking prediction of dynamic obstacle movement trends, easily leading to obstacle avoidance lag or unreasonable trajectory selection. Its evaluation function provides insufficient guidance for the global path, potentially causing the robot to deviate from the global path during obstacle avoidance, exhibiting frequent turns, angular velocity fluctuations, or local stagnation, increasing collision risk and reducing path tracking accuracy and motion stability. Summary of the Invention

[0005] To address the aforementioned issues, this invention proposes a path planning method and system for underground utility tunnel inspection robots. This method combines global path planning with local dynamic obstacle avoidance, improving global path search efficiency while enhancing the robot's ability to avoid dynamic obstacles and improving motion stability during path tracking.

[0006] According to some embodiments, the first aspect of the present invention provides a path planning method for underground utility tunnel inspection robots, employing the following technical solution: A path planning method for underground utility tunnel inspection robots includes: Obtain an environmental map of the underground utility tunnel; Based on the acquired environmental map, the motion state and obstacle information of the inspection robot are determined, and a kinematic model of the inspection robot is constructed. Based on the constructed kinematic model, global path planning for the inspection robot is performed to obtain the original global path of the inspection robot; The global guidance path of the inspection robot that satisfies the safety constraints is obtained based on the original global path; Guided by the obtained global guidance path, and combined with the current state of the inspection robot and obstacle information, a local candidate trajectory is generated; Obstacle information is detected and evaluated based on the generated local candidate trajectories, and the optimal speed control value of the inspection robot is obtained. The path tracking and dynamic obstacle avoidance of the inspection robot are performed according to the optimal speed control value, thus completing the path planning for the underground utility tunnel inspection robot.

[0007] As a further technical limitation, an improved RRT is adopted. The algorithm performs global path planning for the inspection robot, and the improved RRT... The algorithm incorporates hybrid sampling, principal component analysis direction guidance, informed sampling, Gaussian process risk prediction, continuous adaptive step size, high-risk node repulsion correction, and edge cost that integrates environmental risk and safety margin to generate the original global path of the inspection robot.

[0008] Furthermore, the principal component analysis direction guidance is applied to the initial feasible path search stage, including: selecting local tree nodes near the sampling points, constructing a covariance matrix based on their location distribution and performing eigenvalue decomposition; taking the eigenvector corresponding to the largest eigenvalue as the local principal direction, and determining the degree of direction concentration based on the proportion of eigenvalues; when a preset threshold is reached, using a direction guidance weight that gradually increases with the iteration process to perform weighted fusion of random sampling points and principal direction projection points; when the threshold is lower than the preset threshold, retaining the original random sampling points.

[0009] Furthermore, the Gaussian process risk prediction includes: using historical extended node positions and their collision states as model samples, constructing a risk estimation model using a fixed-capacity sliding sample set and fixed kernel parameters; deleting the earliest sample when the sample exceeds the preset capacity; and determining the environmental risk based on the safety score of the candidate position and the prediction uncertainty.

[0010] As a further technical constraint, the line-of-sight method is used to detect whether the connection between non-adjacent path nodes meets the safety constraints. If it does, redundant nodes are deleted. Then, the Chaikin corner-cutting method is used for smoothing and collision detection is performed again. When the smoothed path does not meet the safety constraints, the pruned path is retained, which is the global guiding path.

[0011] As a further technical limitation, based on the current position and speed of the dynamic obstacle in the obstacle information, the future position of the dynamic obstacle is predicted, the minimum spatiotemporal distance between the local candidate trajectory and the predicted future position is calculated, the dynamic collision risk is determined based on the minimum spatiotemporal distance, and a dynamic risk function is constructed; based on the dynamic risk function, the probability of collision between the candidate trajectory and the dynamic obstacle in the future domain is judged, and the detection and evaluation of obstacle information is completed.

[0012] According to some embodiments, a second aspect of the present invention provides a path planning system for underground utility tunnel inspection robots, employing the following technical solution: A path planning system for underground utility tunnel inspection robots includes: The acquisition module is configured to acquire an environmental map of the underground utility tunnel. The module is configured to determine the motion state and obstacle information of the inspection robot based on the acquired environmental map, and to build the kinematic model of the inspection robot. The generation module is configured to perform global path planning for the inspection robot based on the constructed kinematic model to obtain the original global path of the inspection robot; obtain a global guidance path for the inspection robot that satisfies safety constraints based on the original global path; and generate local candidate trajectories based on the obtained global guidance path and the current state of the inspection robot and obstacle information. The planning module is configured to detect and evaluate obstacle information based on the generated local candidate trajectories, obtain the optimal speed control value for the inspection robot, and perform path tracking and dynamic obstacle avoidance for the inspection robot based on the optimal speed control value, thus completing the path planning for the underground utility tunnel inspection robot.

[0013] According to some embodiments, a third aspect of the present invention provides a computer-readable storage medium, employing the following technical solution: A computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the steps in a path planning method for an underground utility tunnel inspection robot as described in the first aspect of the present invention.

[0014] According to some embodiments, the fourth aspect of the present invention provides an electronic device, which adopts the following technical solution: An electronic device includes a memory, a processor, and a program stored in the memory and running on the processor. When the processor executes the program, it implements the steps in a path planning method for an underground utility tunnel inspection robot as described in the first aspect of the present invention.

[0015] According to some embodiments, the fifth aspect of the present invention provides a computer program product, which adopts the following technical solution: A computer program product includes software code, wherein the program in the software code performs the steps of a path planning method for an underground utility tunnel inspection robot as described in the first aspect of the present invention.

[0016] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention employs hybrid sampling and PCA directional guidance in the initial path search phase to reduce invalid expansion. After obtaining feasible paths, it switches to Informed sampling to narrow the optimization range. Simultaneously, it utilizes GP risk prediction to adjust the expansion step size and correct high-risk nodes. Combining risk and safety margins, it performs parent node optimization and local rerouting to improve global search efficiency and path safety.

[0017] This invention will improve RRT Combined with improved DWA, it predicts the location of dynamic obstacles, adaptively adjusts the evaluation weights and local target points, and introduces global path fitting, prediction safety, angular velocity stability and forward movement evaluation to achieve path tracking and dynamic obstacle avoidance coordination, reduce path deviation, frequent turning and angular velocity changes, and improve dynamic obstacle avoidance capability, motion stability and operational reliability. Attached Figure Description

[0018] The accompanying drawings, which form part of this embodiment, are used to provide a further understanding of this embodiment. The illustrative embodiments and their descriptions are used to explain this embodiment and do not constitute an improper limitation of this embodiment.

[0019] Figure 1 This is a flowchart of a path planning method for an underground utility tunnel inspection robot according to Embodiment 1 of the present invention; Figure 2 This is a schematic diagram of the path planning problem for the underground utility tunnel inspection robot in Embodiment 1 of the present invention; Figure 3 This is a detailed schematic diagram illustrating the steps of a collaborative path planning method for an underground utility tunnel inspection robot according to Embodiment 1 of the present invention. Figure 4 The improved RRT in Embodiment 1 of the present invention A flowchart illustrating the algorithm's global path planning process; Figure 5This is a flowchart of local path planning and dynamic obstacle avoidance in the improved DWA of Embodiment 1 of the present invention; Figure 6 The simulation comparison diagram shows the LBT-RRT global path planning algorithm in Embodiment 1 of the present invention. Figure 7 This is a simulation comparison diagram of the BI-RRT global path planning algorithm in Embodiment 1 of the present invention; Figure 8 This is a simulation comparison diagram of the Informed-RRT global path planning algorithm in Embodiment 1 of the present invention; Figure 9 This is a simulation comparison diagram of the improved RRT global path planning algorithm in Embodiment 1 of the present invention; Figure 10 This is a comparison chart of the performance metrics of different global path planning algorithms in Embodiment 1 of the present invention; Figure 11 This is a simulation comparison diagram of the fixed-weight DWA scheme in Embodiment 1 of the present invention; Figure 12 This is a simulation comparison diagram of the DWA scheme without local target bias in Embodiment 1 of the present invention; Figure 13 This is a simulation comparison diagram of the DWA scheme using non-stationarity and progressive evaluation in Embodiment 1 of the present invention; Figure 14 This is a simulation comparison diagram of the improved DWA scheme used in Embodiment 1 of the present invention; Figure 15 This is a comparison chart of the performance indicators of the DWA ablation experiment in Embodiment 1 of the present invention; Figure 16 This is a simulation comparison diagram of the combination of traditional RRT and traditional DWA path planning algorithms in Embodiment 1 of the present invention; Figure 17 This is a simulation comparison diagram of the improved RRT and the traditional DWA path planning algorithm combination in Embodiment 1 of the present invention; Figure 18 This is a simulation comparison diagram of the combination of traditional RRT and improved DWA path planning algorithm in Embodiment 1 of the present invention; Figure 19 This is a simulation comparison diagram of the combination of the improved RRT and the improved DWA path planning algorithms in Embodiment 1 of the present invention; Figure 20 This is a schematic diagram of the minimum dynamic obstacle distance for each algorithm combination in Embodiment 1 of the present invention; Figure 21 This is a schematic diagram of the maximum path offset of each algorithm combination in Embodiment 1 of the present invention; Figure 22This is a structural block diagram of a path planning system for an underground utility tunnel inspection robot according to Embodiment 2 of the present invention. Detailed Implementation

[0020] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0021] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0022] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0023] In this invention, terms such as "upper," "lower," "left," "right," "front," "back," "vertical," "horizontal," "side," and "bottom" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. These terms are used only to facilitate the description of the structural relationships of the various components or elements of this invention and do not specifically refer to any component or element in this invention. They should not be construed as limiting the invention.

[0024] In this invention, terms such as "fixed connection," "connected," and "linked" should be interpreted broadly, indicating a fixed connection, an integral connection, or a detachable connection; a direct connection or an indirect connection through an intermediate medium. Those skilled in the art can determine the specific meaning of these terms in this invention based on the specific circumstances, and they should not be construed as limitations on the invention.

[0025] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0026] Example 1 Embodiment 1 of this invention introduces a path planning method for underground utility tunnel inspection robots.

[0027] like Figure 1 The method shown is a path planning method for underground utility tunnel inspection robots, including: Obtain an environmental map of the underground utility tunnel; Based on the acquired environmental map, the motion state and obstacle information of the inspection robot are determined, and a kinematic model of the inspection robot is constructed. Based on the constructed kinematic model, global path planning for the inspection robot is performed to obtain the original global path of the inspection robot; The global guidance path of the inspection robot that satisfies the safety constraints is obtained based on the original global path; Guided by the obtained global guidance path, and combined with the current state of the inspection robot and obstacle information, a local candidate trajectory is generated; Obstacle information is detected and evaluated based on the generated local candidate trajectories, and the optimal speed control value of the inspection robot is obtained. The path tracking and dynamic obstacle avoidance of the inspection robot are performed according to the optimal speed control value, thus completing the path planning for the underground utility tunnel inspection robot.

[0028] like Figure 3 As shown, the path planning method for underground utility tunnel inspection robots in this embodiment constructs an environmental map of the underground utility tunnel, determines the starting point, target point, and static obstacle information of the inspection robot, and obtains the position and motion state of dynamic obstacles; an improved RRT is adopted. The algorithm performs global path planning, generating an original global path from the starting point to the target point. Redundant nodes in the original global path are removed using the line-of-sight method, and the path is smoothed using the Chaikin corner-cutting method to obtain a global guidance path that meets safety constraints. A dynamic window is established based on the robot's kinematic constraints, and local candidate trajectories are generated. Safety detection and comprehensive evaluation are performed by combining the global guidance path and static and dynamic obstacle information. The optimal speed control quantity is selected to implement path tracking and dynamic obstacle avoidance. The algorithm is executed cyclically until the target point is reached.

[0029] This embodiment improves the efficiency of global path search in complex underground utility tunnel environments, increases the safe distance between the planned path and obstacles, and enhances the path-following ability and motion stability of the inspection robot during dynamic obstacle avoidance.

[0030] To facilitate understanding, the specific implementation method of this embodiment will be further explained below.

[0031] As one or more implementation methods, this embodiment abstracts the underground utility tunnel environment into a two-dimensional grid map. M Set the robot's starting point S Target point G static obstacle set O s and dynamic obstacle set O d Static obstacles include inaccessible areas such as equipment boxes, brackets, fixed pipelines, and walls, while dynamic obstacles include moving objects such as people and vehicles. Specific modeling methods are as follows: Figure 2 As shown.

[0032] The inspection robot is simplified into a two-dimensional planar kinematic model, with its state variables and control variables as follows: ; ;in, , and These are the robot's position and heading angle, respectively. and These are linear velocity and angular velocity, respectively. During the sampling period... Inside, the robot's status is updated to: .

[0033] During initialization, set RRT Maximum number of iterations, expansion step size, neighborhood radius, collision criterion and GP parameters, as well as DWA velocity range, acceleration constraints, sampling resolution and prediction time, etc.

[0034] As one or more implementation methods, this embodiment employs an improved RRT. The algorithm performs global path planning, and its planning process is as follows: Figure 4 As shown.

[0035] The improved RRT in this embodiment The algorithm includes hybrid sampling, PCA direction guidance, Informed sampling, GP risk prediction, continuous adaptive step size, high-risk node repulsion correction, joint edge cost of risk and safety margin, parent node selection, and neighborhood node rerouting.

[0036] This embodiment uses hybrid sampling and principal component analysis to generate guiding sampling points. Before obtaining the initial feasible path, a hybrid sampling strategy is adopted, including target bias sampling, Gaussian sampling around the starting and ending point line, and global uniform sampling, to take into account target guidance, directional search, and global exploration.

[0037] During the random tree expansion process, the target bias probability is dynamically adjusted based on the cumulative effective expansion rate. When the effective expansion rate is high, the target bias probability is increased to accelerate the expansion of the random tree towards the target point; when the effective expansion rate is low, the target bias probability is decreased to enhance the exploration of other free regions.

[0038] To extract the directional features of local nodes in a random tree, a covariance matrix is ​​constructed based on the position coordinates of neighboring tree nodes at the sampling point, and eigenvalue decomposition is performed on the covariance matrix. Let the largest eigenvalue of the covariance matrix be... The sum of all eigenvalues ​​is Then the local directionality coefficient is ;when When this occurs, it indicates that the local node distribution has a relatively obvious directionality, and the eigenvector corresponding to the largest eigenvalue is taken as the local principal direction; when When this occurs, it indicates that the local node distribution is relatively discrete, and the original random sampling results are retained. In this embodiment, the directional coefficient threshold... Take 0.75.

[0039] Let random sampling points be set The projection point on the local principal direction is Then the sampling points after direction guidance are represented as ;in, α k The weights are assigned to guide the direction and gradually increase during iteration. This weighting method utilizes local directional information while retaining random sampling components, thus preventing the search direction of the random tree from becoming overly concentrated.

[0040] This embodiment first determines whether an initial feasible path has been obtained; if not, it continues to use hybrid sampling and PCA directional guidance to expand the random tree; if it has been obtained, it constructs an elliptical region with the origin and destination as the focus and the current optimal path length as the constraint, and switches to Informed sampling.

[0041] As the length of the current optimal path decreases, the elliptical sampling region shrinks accordingly, causing subsequent sampling to focus on regions that are more likely to improve the current solution, thereby reducing ineffective expansion and improving path optimization efficiency.

[0042] This embodiment uses a Gaussian process model to predict the risk of candidate locations and adjust the expansion step size; it constructs a GP risk estimation model using the positions and collision states of historical expansion nodes; when no collision occurs on the expansion edge, the corresponding node is used as a safe sample; when a collision occurs on the expansion edge, the corresponding node is used as a collision sample.

[0043] To control the computational scale during the online update of the GP model, a fixed-size sliding sample set is used, retaining only the 55 most recently expanded nodes and their collision labels. When the number of samples exceeds 55, the oldest added sample is deleted. Meanwhile, the model kernel function parameters remain fixed, and no online hyperparameter optimization is performed.

[0044] Spatial correlation between nodes is represented by radial basis kernel function. ;in, =15、 =1、 =0.10. By limiting the sample size and fixing the kernel function parameters, the computational scale of the GP model during online updates remains stable.

[0045] GP model outputs candidate positions Safety rating and forecast uncertainty The comprehensive risk of candidate positions is defined as follows: ;in, For uncertain weights, This indicates that the calculation results are restricted to the interval [0,1]. When the safety score decreases or the prediction uncertainty increases, the risk value of the candidate location increases accordingly.

[0046] Design a continuous adaptive step size function based on the risk value. ;in, and These are the minimum expansion step size and the maximum expansion step size, respectively. β This is the risk attenuation coefficient.

[0047] When the candidate location risk value is low, a larger expansion step size is used to improve the expansion efficiency in free space; when the candidate location risk value is high, the expansion step size is reduced to improve the search resolution in the obstacle neighborhood and narrow passage.

[0048] The tree node closest to the current sampling point in the random tree. Generate new nodes along the sampling direction When PCA directional guidance is used, the following are... Replace with After obtaining the initial feasible path, the sampling points within it are the sampling points generated within the Informed elliptical region.

[0049] When the risk of a new node exceeds the threshold, repulsion correction is performed based on the location of nearby obstacles, and collision detection is performed on the extended edge. If a collision occurs, the new node is discarded and recorded as a collision sample; otherwise, the new node is retained and recorded as a safe sample to update the GP risk model.

[0050] This embodiment constructs a joint edge cost of risk and safety margin; to avoid the planned path getting too close to static obstacles, in RRT... The edge cost incorporates GP environmental risk and obstacle safety margin penalties.

[0051] Let the candidate edges be ; Select along candidate edges The nth sampling point. Let the nth sampling point be... i sampling points The distance to the nearest obstacle expansion boundary is The safe distance threshold is The safety margin penalty is When the sampling point meets the safety distance constraint, the safety margin term does not incur additional costs; when the sampling point enters the safety distance range, the penalty increases as the net distance between obstacles decreases.

[0052] By integrating GP environmental risk with safety margin penalty, the local comprehensive risk of sampling points on candidate edges is obtained. ;in, The penalty weight is set for safety margin.

[0053] The average risk of candidate edges is .

[0054] To enhance the constraint on path safety in the later stages of the search, the risk weight is gradually increased with each iteration. ;in, This represents the current iteration number. This represents the maximum number of iterations.

[0055] No. The cost of candidate edges in the next iteration is ;in, The geometric length of the candidate edge is given. This edge cost incorporates the candidate edge length, environmental risk, and obstacle safety margin into the path evaluation.

[0056] In the neighborhood of the new node, a candidate parent node is selected. The node with the minimum cumulative cost and no collisions between the connecting edges is selected as the parent node, and the new node is added to the random tree. Then, using the new node as the candidate parent node, the neighborhood nodes that satisfy the conditions of no collisions and reduced cumulative cost are rewired.

[0057] When a new node can be connected to the target point without collision, the current optimal path is generated or updated. After obtaining a feasible path for the first time, Informed sampling and local rewiring continue until the termination condition is met, and the original global path is obtained by backtracking along the parent node relationships.

[0058] As one or more implementation methods, this embodiment improves RRT. The generated original global path is pruned and smoothed.

[0059] The path is pruned using a line-of-sight method. It sequentially checks whether the current node can be directly connected to subsequent non-adjacent nodes without collision while maintaining a safe distance. The furthest directly connected node is retained, and intermediate nodes are deleted. This process is repeated until the target point is reached to reduce redundant nodes and unnecessary turns.

[0060] Although the number of path nodes is reduced after pruning, it still consists of polyline segments. To improve path continuity, the Chaikin angle-cutting method is used for smoothing. Let adjacent path points be... and Then generate ; Next, adjacent path segments are truncated, with transition nodes replacing the original inflection points. Collision detection is then performed again after smoothing. If the smoothed path does not meet the collision-free constraint, the path pruned using the line-of-sight method is retained, and the path that finally meets the safety constraints is used as the global reference path for improving DWA. .

[0061] As one or more implementation methods, this embodiment uses a global reference path To guide the process, local candidate trajectories are generated by combining the robot's current state with information on static and dynamic obstacles. The improved DWA local path planning process is as follows: Figure 5 As shown, specifically: This embodiment selects a local target point and establishes a dynamic window; based on the robot's current position, it follows the global reference path. The system determines the nearest path point to the robot and updates the global path progress index. Based on the preset forward look-ahead distance, it proceeds along... Select local target points in the direction of the target point This serves as a phased tracking target for the current planning cycle of DWA.

[0062] Let candidate velocity combinations be defined. The generated predicted trajectory is The predicted trajectory contains a total of M The trajectory point, the th _ ... i The trajectory points are Global reference path The path point on is The average deviation between the candidate trajectory and the global path is then... The corresponding global path fit is ;in, To prevent constants with a denominator of zero. The smaller the value, the closer the candidate local trajectory is to the global reference path.

[0063] By introducing a global path fit evaluation, the problem of robots deviating excessively from the global path after dynamic obstacle avoidance can be reduced.

[0064] A dynamic window is determined based on the robot's current linear velocity, angular velocity, maximum linear acceleration, maximum angular acceleration, and velocity limit. Within the dynamic window, discrete sampling is performed according to linear velocity resolution and angular velocity resolution to obtain candidate velocity combinations.

[0065] For each candidate velocity, a corresponding local candidate trajectory is generated in the prediction time domain using the robot's kinematics model. After the candidate trajectory is generated, static and dynamic obstacle safety checks are performed first, and candidate trajectories that do not meet the safety distance constraints are directly discarded.

[0066] This embodiment predicts the location of dynamic obstacles and performs safety detection on candidate trajectories. To avoid candidate trajectories getting too close to obstacles, the robot, obstacles, and safety margins are treated equivalently, and a safety radius is planned. ;in, Where is the radius of the obstacle. For the robot's equivalent radius, For safety margin, This refers to the safe obstacle avoidance radius used during the planning process.

[0067] For a candidate predicted trajectory, the minimum safe distance between it and a static obstacle is: ;in, For prediction points To static obstacles The shortest distance to the boundary. When If the value is less than the set threshold, the candidate trajectory is discarded.

[0068] For dynamic obstacles, short-term position is predicted based on their current position, speed, movement boundaries, and movement pattern. ;in, The current position of the dynamic obstacle. The speed of the dynamic obstacle. For motion boundary parameters, For sports mode, For predicting time.

[0069] This embodiment uses sinusoidal motion, diagonal reciprocating motion, and figure-eight motion models to predict the position of dynamic obstacles, and uses the prediction results for candidate trajectory risk assessment.

[0070] Let the candidate trajectory of the robot be at the t. The position of each predicted time point is , No. The predicted positions of the dynamic obstacles at the same time are: Then the minimum spatiotemporal distance between the robot and the dynamic obstacle is In this embodiment, the safety margin Take 0.18 m. When When this candidate trajectory is discarded, its equivalent actual boundary net spacing threshold is: When the actual net distance between the boundaries is not greater than 0, it is determined that the robot has come into contact with or collided with the dynamic obstacle.

[0071] Construct a dynamic risk function based on the predicted distance. ;in, To determine the safe distance between the robot's predicted trajectory and the predicted location of dynamic obstacles. This represents the risk diffusion coefficient. To predict the number of samples.

[0072] In this embodiment, The larger the value, the higher the probability that the candidate trajectory will collide with dynamic obstacles in the future. When the distance to the dynamic obstacle decreases or the predicted risk exceeds the threshold, the weights of the dynamic obstacle prediction safety assessment and angular velocity stability assessment are increased, while the weight of the velocity assessment is decreased.

[0073] When a dynamic obstacle may cut into the global reference path, the local target point is shifted to the side away from the obstacle based on the position of the dynamic obstacle relative to the path, so as to guide the robot to make a local detour in advance.

[0074] This embodiment comprehensively evaluates candidate trajectories and outputs the optimal control quantity. When the inspection robot operates in a narrow pipe gallery, frequent sharp turns or circling in place during local planning will reduce the stability of the robot's motion trajectory. Therefore, an angular velocity stability evaluation is added to the DWA evaluation function. ;in, For candidate angular velocities, The angular velocity used at the previous moment. This is the adjustment coefficient. The smaller the change in angular velocity, the higher the stability evaluation.

[0075] For each candidate trajectory that passes the safety check, a heading evaluation is calculated. Static obstacle distance evaluation Speed ​​evaluation Global path fit evaluation Safety assessment of dynamic obstacle prediction Angular velocity stability evaluation and progressive evaluation .

[0076] The improved DWA comprehensive evaluation function is as follows: ;in, to The weights for each evaluation item.

[0077] Among all candidate speed combinations that meet safety constraints, the control command with the highest comprehensive evaluation value is selected. Based on the optimal linear velocity and optimal angular velocity Update the robot's status, and also update the positions of dynamic obstacles, the global path progress index, and the angular velocity at the previous time step.

[0078] Determine if the robot has reached the target point; if the robot has not yet reached the target point, reselect a local target point, establish a dynamic window, and generate candidate trajectories; if the robot has already reached the target point, end the path planning.

[0079] To verify the effectiveness of the method, this embodiment conducted comparative experiments on global planning, local planning ablation, and fusion algorithms.

[0080] 1) Improve RRT Global path planning comparison In the simulation platform, a 100 m × 100 m simulation map was used, with the starting point at [0,0] and the target point at [90,40]. Black areas represented static obstacles. All algorithms were run under the same map, start / end point, and obstacle conditions to improve RRT. With LBT-RRT, BI-RRT and Informed-RRT The planning results are as follows: Figure 6 , Figure 7 , Figure 8 and Figure 9 As shown, each algorithm can obtain a feasible path, but the comparative algorithms have problems such as detours, close proximity to obstacles, or many turns to varying degrees; the path obtained by the present invention is shorter and smoother, and maintains a larger safe distance from obstacles.

[0081] Each algorithm was run independently 30 times, and the path length, planning time, minimum obstacle distance, path smoothness, and planning success rate were recorded. The results are as follows: Figure 10 As shown.

[0082] The corresponding metrics in this embodiment are 117.54 m, 0.66 s, 0.49 m, 2.18, and 100%; LBT-RRT is 125.76 m, 1.10 s, 0.30 m, 6.82, and 100%; BI-RRT is 119.70 m, 0.75 s, 0.12 m, 5.74, and 100%; and Informed-RRT... The results are 125.23 m, 0.89 s, 0.28 m, 4.36, and 100%, respectively. The results indicate that the present invention has good overall performance in terms of path length, planning efficiency, safety distance, and path smoothness.

[0083] 2) Improved DWA ablation comparison The local planning experiment used a narrow underground utility tunnel environment of 36 m × 6 m. The robot's starting point was S[0.5, 3.0], and the target point was G[35.5, 3.0]. Rectangular static obstacles were set on both sides of the tunnel and in the middle of the passage, and three types of dynamic obstacles were set: sinusoidal motion, diagonal reciprocating motion, and figure-eight motion.

[0084] Four groups of ablation experiments were set up: Group A had risk trigger weight adjustment disabled, but retained evaluations of local target point lateral bias, angular velocity stability, and progressiveness; Group B had local target point lateral bias disabled; Group C removed angular velocity stability and progressiveness evaluations; and Group D retained all improvement modules. The planning results and performance indicators for each group are as follows: Figure 11 , Figure 12 , Figure 13 , Figure 14 and Figure 15 As shown.

[0085] Depend on Figure 11 , Figure 12 , Figure 13 , Figure 14 and Figure 15 It can be seen that the minimum dynamic obstacle distance for groups A and C is 0, resulting in a collision; groups B and D can both complete obstacle avoidance and reach the target point. Compared with group B, group D, with a similar path length, has a shorter movement time, a larger minimum dynamic obstacle distance, and a higher average linear velocity. This indicates that the improved modules can work together to reduce stagnation, ineffective detours, and frequent turns, improving the safety and efficiency of dynamic obstacle avoidance.

[0086] 3) Improve RRT -DWA Fusion Comparison In a 100 m × 100 m high-density pipeline environment, traditional RRT-DWA and improved RRT are installed. -Traditional DWA, Traditional RRT - Improved DWA and Improved RRT -DWA four-group algorithm. Each group uses the same start / end point, obstacle conditions, robot kinematic parameters, and safety judgment criteria. The planning results are as follows: Figure 15 As shown.

[0087] Depend on Figure 16 , Figure 17 , Figure 18 and Figure 19 It can be seen that the combination of traditional RRT and traditional DWA has many global path inflection points and obvious fluctuations in the actual trajectory; although the combination of improved RRT and traditional DWA improves the path continuity, collisions still occur; the combination of traditional RRT and improved DWA can reach the target point, but local adjustments are frequent; the complete improved RRT-DWA combines global path guidance, dynamic risk prediction and local detours, resulting in a smoother trajectory.

[0088] The minimum dynamic obstacle distance and maximum path offset of the four algorithms are as follows: Figure 20 and Figure 21 As shown, the m and y values ​​are 0.239 m, 0, 0.743 m, 1.400 m and 9.92 m, 4.18 m, 3.99 m, 3.44 m, respectively; Complete Improved RRT -DWA completed the task in 396.0 seconds.

[0089] The results show that the improved RRT It can generate shorter, continuous global paths with a large safety margin; the improved DWA achieves safe and stable local obstacle avoidance through dynamic risk prediction, path fitting, weight adjustment, target point offset and motion stability evaluation, thereby improving the robot's path safety, tracking ability and motion stability.

[0090] In the initial path search phase, this embodiment employs hybrid sampling and PCA directional guidance to reduce invalid expansion. After obtaining feasible paths, it switches to Informed sampling to narrow down the optimization range. Simultaneously, it utilizes GP risk prediction to adjust the expansion step size and correct high-risk nodes. Combining risk and safety margins, it performs parent node optimization and local rerouting to improve global search efficiency and path safety.

[0091] This embodiment will improve RRT Combined with improved DWA, it predicts the location of dynamic obstacles, adaptively adjusts the evaluation weights and local target points, and introduces global path fitting, prediction safety, angular velocity stability and forward movement evaluation to achieve path tracking and dynamic obstacle avoidance coordination, reduce path deviation, frequent turning and angular velocity changes, and improve dynamic obstacle avoidance capability, motion stability and operational reliability.

[0092] Example 2 Embodiment 2 of the present invention introduces a path planning system for underground utility tunnel inspection robots.

[0093] like Figure 22 The path planning system shown is for an underground utility tunnel inspection robot, comprising: The acquisition module is configured to acquire an environmental map of the underground utility tunnel. The module is configured to determine the motion state and obstacle information of the inspection robot based on the acquired environmental map, and to build the kinematic model of the inspection robot. The generation module is configured to perform global path planning for the inspection robot based on the constructed kinematic model to obtain the original global path of the inspection robot; obtain a global guidance path for the inspection robot that satisfies safety constraints based on the original global path; and generate local candidate trajectories based on the obtained global guidance path and the current state of the inspection robot and obstacle information. The planning module is configured to detect and evaluate obstacle information based on the generated local candidate trajectories, obtain the optimal speed control value for the inspection robot, and perform path tracking and dynamic obstacle avoidance for the inspection robot based on the optimal speed control value, thus completing the path planning for the underground utility tunnel inspection robot.

[0094] The detailed steps are the same as those of the path planning method for an underground utility tunnel inspection robot provided in Embodiment 1, and will not be repeated here.

[0095] Example 3 Embodiment 3 of the present invention provides a computer-readable storage medium.

[0096] A computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements the steps of a path planning method for an underground utility tunnel inspection robot as described in Embodiment 1 of the present invention.

[0097] The detailed steps are the same as those of the path planning method for an underground utility tunnel inspection robot provided in Embodiment 1, and will not be repeated here.

[0098] Example 4 Embodiment 4 of the present invention provides an electronic device.

[0099] An electronic device includes a memory, a processor, and a program stored in the memory and running on the processor. When the processor executes the program, it implements the steps in a path planning method for an underground utility tunnel inspection robot as described in Embodiment 1 of the present invention.

[0100] The detailed steps are the same as those of the path planning method for an underground utility tunnel inspection robot provided in Embodiment 1, and will not be repeated here.

[0101] Example 5 Embodiment 5 of the present invention provides a computer program product.

[0102] A computer program product includes software code, wherein the program in the software code performs the steps of a path planning method for an underground utility tunnel inspection robot as described in Embodiment 1 of the present invention.

[0103] The detailed steps are the same as those of the path planning method for an underground utility tunnel inspection robot provided in Embodiment 1, and will not be repeated here.

[0104] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of the present invention can be implemented using various computer languages, such as the object-oriented programming language Java and the interpreted scripting language JavaScript.

[0105] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0106] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0107] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0108] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0109] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

[0110] The above description is merely a preferred embodiment of this practice and is not intended to limit the scope of this practice. Various modifications and variations can be made to this practice by those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of this practice should be included within the protection scope of this practice.

Claims

1. A path planning method for underground utility tunnel inspection robots, characterized in that, include: Obtain an environmental map of the underground utility tunnel; Based on the acquired environmental map, the motion state and obstacle information of the inspection robot are determined, and a kinematic model of the inspection robot is constructed. Based on the constructed kinematic model, global path planning for the inspection robot is performed to obtain the original global path of the inspection robot; The global guidance path of the inspection robot that satisfies the safety constraints is obtained based on the original global path; Guided by the obtained global guidance path, and combined with the current state of the inspection robot and obstacle information, a local candidate trajectory is generated; Based on the generated local candidate trajectories, obstacle information is detected and evaluated to obtain the optimal speed control value of the inspection robot. The path tracking and dynamic obstacle avoidance of the inspection robot are performed according to the optimal speed control value, thus completing the path planning for the underground utility tunnel inspection robot. Adopting an improved RRT The algorithm performs global path planning for the inspection robot, and the improved RRT... The algorithm incorporates hybrid sampling, principal component analysis direction guidance, informed sampling, Gaussian process risk prediction, continuous adaptive step size, high-risk node repulsion correction, and edge cost that integrates environmental risk and safety margin to generate the original global path of the inspection robot. The principal component analysis direction guidance is applied to the initial feasible path search stage, including: selecting local tree nodes near the sampling points, constructing a covariance matrix based on their location distribution, and performing eigenvalue decomposition; The eigenvector corresponding to the largest eigenvalue is taken as the local principal direction, and the degree of directional concentration is determined according to the proportion of eigenvalues. When the local directionality coefficient reaches a preset threshold, a direction-guided weight that gradually increases with the iteration process is used to weight and fuse the random sampling points and the projection points of the principal direction. When the local directionality coefficient is lower than the preset threshold, the original random sampling points are retained. for , The largest eigenvalue of the covariance matrix. It is the sum of all eigenvalues.

2. The path planning method for an underground utility tunnel inspection robot as described in claim 1, characterized in that, The Gaussian process risk prediction includes: using historical extended node positions and their collision states as model samples, constructing a risk estimation model using a fixed-capacity sliding sample set and fixed kernel parameters; deleting the earliest sample when the sample exceeds the preset capacity; and determining the environmental risk based on the safety score of the candidate position and the prediction uncertainty.

3. The path planning method for an underground utility tunnel inspection robot as described in claim 1, characterized in that, The line-of-sight method is used to check whether the connection between non-adjacent path nodes meets the safety constraints. If it does, redundant nodes are deleted. Then, the Chaikin corner cutting method is used to smooth the path and re-detect collisions. When the smoothed path does not meet the safety constraints, the pruned path is retained, which is the global guiding path.

4. The path planning method for an underground utility tunnel inspection robot as described in claim 1, characterized in that, Based on the current position and speed of the dynamic obstacle in the obstacle information, predict the future position of the dynamic obstacle, calculate the minimum spatiotemporal distance between the local candidate trajectory and the predicted future position, determine the dynamic collision risk based on the minimum spatiotemporal distance, and construct a dynamic risk function; The probability of collision with dynamic obstacles in the future domain of candidate trajectories is determined based on the dynamic risk function, thus completing the detection and evaluation of obstacle information.

5. A path planning system for an underground utility tunnel inspection robot, employing a path planning method for an underground utility tunnel inspection robot as described in any one of claims 1-4, characterized in that, include: The acquisition module is configured to acquire an environmental map of the underground utility tunnel. The module is configured to determine the motion state and obstacle information of the inspection robot based on the acquired environmental map, and to build the kinematic model of the inspection robot. The generation module is configured to perform global path planning for the inspection robot based on the constructed kinematic model, thereby obtaining the original global path of the inspection robot; and to obtain the global guidance path of the inspection robot that satisfies the safety constraints based on the original global path. Guided by the obtained global guidance path, and combined with the current state of the inspection robot and obstacle information, a local candidate trajectory is generated; The planning module is configured to detect and evaluate obstacle information based on the generated local candidate trajectories, obtain the optimal speed control value for the inspection robot, and perform path tracking and dynamic obstacle avoidance for the inspection robot based on the optimal speed control value, thus completing the path planning for the underground utility tunnel inspection robot.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of a path planning method for an underground utility tunnel inspection robot as described in any one of claims 1-4.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that, When the processor executes the program, it implements the steps of a path planning method for an underground utility tunnel inspection robot as described in any one of claims 1-4.

8. A computer program product, comprising software code, characterized in that, The program in the software code executes the steps of a path planning method for an underground utility tunnel inspection robot as described in any one of claims 1-4.

Images