A path planning method for a tracked inspection robot based on an ant colony optimization algorithm

Abstract

This invention relates to a path planning method for inspection robots based on ant colony optimization algorithm, specifically as follows: First, an adaptive adjustment factor is introduced into the ant colony pheromone update rule, which dynamically changes according to the frequency of obstacles encountered by ants; second, redundant stagnation points are removed, key nodes are curve-fitted, and the path between adjacent key nodes is smoothly transitioned, reducing tortuosity and turning angles; simultaneously, information is collected by sensors to identify unknown static and dynamic obstacles in real time, and a dynamic window method with an improved safety distance evaluation sub-function is used for local path planning and obstacle avoidance. This invention is simple to use, highly accurate, and can effectively enhance path adaptability and improve path planning performance.

Description

Technical Field

[0001] This invention relates to a path planning method for a tracked inspection robot based on ant colony optimization algorithm, belonging to the field of special robots. Background Technology

[0002] In today's booming industrial sector, the importance of equipment inspection is increasingly evident. Traditional manual inspection methods, due to their high workload, low efficiency, and questionable safety, are no longer sufficient to meet the demands of modern industry. Tracked inspection robots have emerged to address this need. The tracked design is chosen primarily because of its excellent maneuverability and stability in complex terrains and harsh environments, allowing it to better adapt to various ground conditions in industrial settings, such as uneven surfaces and areas with obstacles. Currently, the path planning methods for inspection robots have several shortcomings. On the one hand, existing path planning methods have poor adaptability in complex industrial environments, struggling to cope with changing terrain, numerous obstacles, and unpredictable environmental factors. On the other hand, the efficiency and accuracy of path planning need improvement, which may lead to robots taking long, circuitous routes, frequently detouring, or even failing to reach the target location during inspections, affecting the timeliness and effectiveness of the inspection work.

[0003] This invention proposes a path planning method for tracked inspection robots based on ant colony optimization algorithm, aiming to provide an efficient and accurate solution to these shortcomings. Through improvement and optimization of the algorithm, it can better adapt to complex industrial environments, improve the efficiency and accuracy of path planning, and enable the inspection robot to accurately and quickly plan the optimal path under various complex conditions. This effectively solves the problems existing in current path planning methods for inspection robots and provides more reliable support for industrial equipment inspection. Summary of the Invention

[0004] Purpose of the invention: In order to overcome the problems of low efficiency and low accuracy in the current inspection process of tracked inspection robots, this invention provides a path planning method for tracked inspection robots based on ant colony optimization algorithm, so as to realize accurate and efficient autonomous path planning of tracked inspection robots in complex industrial environments.

[0005] Technical Solution: To solve the above technical problems, the present invention provides a path planning method for a tracked inspection robot based on ant colony optimization algorithm, which specifically includes the following steps:

[0006] Step 1: Construct an environmental grid map for the tracked inspection robot, dividing the robot's environment into grids of the same size, and setting the starting point and target point of the tracked inspection robot;

[0007] Step 2: Improve the traditional ant colony algorithm and use the improved ant colony optimization algorithm for global path planning to obtain the optimal path;

[0008] Step 3: Identify unknown static and dynamic obstacles in the global map. Collect surrounding environmental information using sensors such as LiDAR;

[0009] Step 4: Process the surrounding environment information data collected in Step 3, update the internal environment of the dynamic window, and determine whether there are unknown static and dynamic obstacles.

[0010] Step 5: If no obstacle is detected, proceed along the shortest obstacle-free path; if an obstacle is detected, the tracked inspection robot performs local path planning to avoid the obstacle, and returns to the optimal path to continue driving after obstacle avoidance.

[0011] Step 6: Use the dynamic window method with improved safety distance evaluation sub-function to perform local path planning to avoid obstacles. The tracked inspection robot moves along the locally planned path. After obstacle avoidance, it returns to the optimal path and determines whether the tracked inspection robot has reached the target point. If it has, the algorithm ends; otherwise, proceed to step 4.

[0012] The technical solution of the present invention is further defined as follows: The method for improving the ant colony optimization algorithm includes the following specific steps:

[0013] S1: An adaptive weighting factor is introduced into the path evaluation function of the ant colony algorithm. This factor is dynamically adjusted based on the probability of ants encountering obstacles during the search process. When the probability of an ant encountering an obstacle in a certain area is high, the weighting factor is decreased to reduce the likelihood of that path being chosen and encourage ants to explore other paths more. Conversely, when the probability of encountering an obstacle is low, the weighting factor is increased to encourage ants to search more actively in that area.

[0014] S2: Eliminate all redundant turning points in the path planned in S1 to obtain a smoother and safer path. By eliminating unnecessary turning points, the path design is further optimized, improving the robot's movement between tracks.

[0015] S3: Round off the turning points in S2 to eliminate redundant turning points. By continuously repeating this process, the entire path is optimized to obtain a smoother and safer path, improving the robot's movement efficiency between tracks and reducing mechanical wear and energy consumption;

[0016] This circular arc treatment not only reduces the path length, as circular arc paths are generally shorter than polygonal paths, but also reduces the mechanical stress on the robot when turning.

[0017] Furthermore, in step 6, the dynamic window method for improving the safety distance evaluation sub-function is as follows: Let the safety distance evaluation sub-function be D(n), and its calculation method considers multiple factors to more comprehensively evaluate the safety of the path.

[0018] D(n) = λ1·d total (n)+λ2·d min (n)+λ3·p(n)+λ4·v(n)

[0019] In the formula, d total (n) represents the total path length from the starting point to the current node n; d min p(n) represents the shortest distance between the current node n and surrounding obstacles; p(n) is the safety penalty function, which is calculated as follows: Where p0 is a large constant used to control the intensity of the penalty, d th It is a safety threshold distance, d min (n) when less than d th At that time, the penalty function begins to take effect, and d min The smaller v(n) is, the greater the penalty, meaning the lower the probability that the path will be chosen. This encourages ants to choose paths further away from obstacles, improving the safety of path planning; v(n) is the speed evaluation function, representing the speed of the inspection robot at the current node n; λ1, λ2, λ3, and λ4 are weighting coefficients used to adjust the influence of factors such as the inspection robot's direction of movement, distance, and speed on the evaluation function results.

[0020] Furthermore, in step 2, the path quality evaluation function is optimized, and the calculation formula is:

[0021]

[0022] F(n) = G(n) + H(n)

[0023] Among them, c i =k·d,

[0024] In the formula, This represents the cumulative movement cost of the ant from its starting point to the current node n. obs (n) represents the minimum distance from the path between the current node n and the next node to the nearest obstacle, r safe Let ω be the safe radius of the inspection robot, and let ω be the safety cost weight. j It is a binary indicator variable; when there is an obstacle at node j that the ant passes through, o j =1, otherwise o j =0, μ is a weighting coefficient used to adjust the importance of the number of obstacles in the actual cost. η is an adaptive weighting factor, d est (n) represents the estimated distance from the current node n to the target node, d total This represents the estimated total distance between the starting node and the target node. The complexity of the path has been taken into account. n is the number of nodes that the ant has already visited, N is the total number of nodes between the starting node and the target node, and ξ is a weighting coefficient.

[0025] Furthermore, in step 2, the path smoothness optimization index is calculated as follows:

[0026]

[0027] Where: m is the number of critical nodes on the path (including the starting point, turning points, and target point, etc.), R i Let θ be the radius of curvature corresponding to the i-th path segment. n is the number of turning points on the path, and θ is... j Let α be the turning angle of the j-th turning point. j It is the weighting coefficient for the turning angle.

[0028] Further, in step 2, the characteristic is that, in S2, the obtained path is smoothed, and the calculation formula is:

[0029]

[0030] Where C(t) represents the Bézier curve function, used to describe the coordinates of points on the curve. n is the degree of the Bézier curve, affecting the shape complexity of the curve. P i This represents the i-th control point, used to determine the approximate shape of the curve. B i,n C'(t) is an nth-degree Bernstein polynomial, a function of the parameter t, used to determine the weighted contribution of control points to the curve. C'(t) is the first derivative of the Bézier curve, used to represent the direction of the tangent at a point on the curve. C'(t) is the second derivative of the Bézier curve, used in calculations related to the curvature of the curve.

[0031] Beneficial effects

[0032] In summary, the path planning method for tracked inspection robots of this invention offers significant advantages. Tracked robots possess excellent maneuverability and stability, enabling them to adapt to complex terrain. The improved ant colony optimization algorithm plays a crucial role, enhancing robot performance in multiple ways. By optimizing the evaluation function and introducing adaptive weighting factors, the path is smoothed, allowing the robot to flexibly respond to environmental changes and quickly and accurately plan the optimal inspection path during path search. This achieves efficient, unmanned inspection in factory workshops and brings a series of practical benefits through path optimization, such as reduced energy consumption and inspection time, significantly improving inspection efficiency and quality. Detailed Implementation

[0033] The present invention will now be described in further detail.

[0034] This invention discloses a path planning method for a tracked inspection robot based on an ant colony optimization algorithm. This algorithm can be used for global path planning and dynamic obstacle avoidance during movement. The specific steps are as follows:

[0035] Step 1: Construct an environmental grid map for the tracked inspection robot, dividing the robot's environment into grids of the same size, and setting the starting point and target point of the tracked inspection robot;

[0036] Step 2: Improve the traditional ant colony algorithm by using the improved ant colony optimization algorithm for global path planning to obtain the optimal path;

[0037] Step 3: Identify unknown static and dynamic obstacles on the global map. Collect surrounding environmental information using sensors such as LiDAR;

[0038] Step 4: Process the surrounding environment information data collected in Step 3, update the internal environment of the dynamic window, and determine whether there are unknown static and dynamic obstacles.

[0039] Step 5: If there are no obstacles, continue along the shortest barrier-free path; if there are obstacles, perform collision prediction and implement obstacle avoidance measures.

[0040] Step 6: Use the dynamic window method with an improved safety distance evaluation subfunction for local path planning to avoid obstacles. The tracked inspection robot moves along the locally planned path, and after obstacle avoidance, returns to the optimal path and determines whether the tracked inspection robot has reached the target point. If it has reached the target point, the algorithm ends; otherwise, proceed to step 4.

[0041] Furthermore, the improved ant colony optimization algorithm in step 2 is described in detail below:

[0042] S1: Given the information on obstacles, starting point, target point, and map environment, the ant colony algorithm introduces an adaptive weighting factor to calculate a new path evaluation function. This factor is dynamically adjusted based on the probability of ants encountering obstacles during their search. When an ant is exploring a certain area, if the probability of encountering an obstacle is high, the adaptive weighting factor decreases accordingly. This makes the ant more inclined to avoid that area when choosing the next node, and instead explore other path directions with a lower probability of encountering obstacles. Conversely, when the probability of an ant encountering an obstacle in a certain area is low, the adaptive weighting factor increases, encouraging the ant to actively search for a forward path in that area. This dynamic adjustment mechanism aims to enable the algorithm to quickly adapt to environmental changes and rapidly plan the optimal path that best meets the movement requirements of the tracked inspection robot.

[0043] S2: After path search based on the new path quality evaluation function, when the coordinates of the target point are calculated, determine whether this coordinate point is the final target point. If the final target point has been reached, further optimize the planned path, i.e., eliminate all redundant turning points in the planned path from S1. By carefully analyzing the node relationships on the path, determine the path situation between three consecutive nodes. If the robot can safely reach the third node directly from the first node without colliding with any obstacles and without adversely affecting subsequent path planning, then the second node in the middle is identified as a redundant turning point and eliminated.

[0044] S3: During the inspection process, when the robot's turning angle is large, drift may occur, which adversely affects the robot's actual control. Therefore, the turning points in the optimized path in S2 are processed with arcs. For each turning point on the path, the relevant geometric parameters are first calculated based on the position information of its preceding and following nodes. Then, combined with the inspection robot's motion characteristics and actual environmental requirements, a suitable arc radius is determined. An arc is drawn with the turning point as the center and the determined arc radius as the radius, allowing the robot to smoothly transition from the current path segment to the next along this arc.

[0045] Furthermore, in step 6, the improved dynamic window method for the safety distance evaluation sub-function is as follows: Let the safety distance evaluation sub-function be D(n),

[0046] D(n) = λ1·d total (n)+λ2·d min (n)+λ3·p(n)+λ4·v(n)

[0047] In the formula, d total (n) represents the total path length from the starting point to the current node n; d min p(n) represents the shortest distance between the current node n and surrounding obstacles; p(n) is the safety penalty function, which is calculated as follows: Where p0 is a large constant used to control the intensity of the penalty, d th It is a safety threshold distance, d min (n) when less than d th At that time, the penalty function begins to take effect, and d min The smaller v(n) is, the greater the penalty, meaning the lower the probability that the path will be chosen. This encourages ants to choose paths further away from obstacles, improving the safety of path planning; v(n) is the speed evaluation function, representing the speed of the inspection robot at the current node n; λ1, λ2, λ3, and λ4 are weighting coefficients used to adjust the influence of factors such as the inspection robot's direction of movement, distance, and speed on the evaluation function results.

[0048] Furthermore, in step 2, the path quality evaluation function is optimized, and the calculation formula is:

[0049]

[0050] F(n) = G(n) + H(n)

[0051] Among them, c i =k·d,

[0052] In the formula, This represents the cumulative movement cost of the ant from its starting point to the current node n. obs (n) represents the minimum distance from the path between the current node n and the next node to the nearest obstacle, r safe Let ω be the safe radius of the inspection robot, and let ω be the safety cost weight. j It is a binary indicator variable; when there is an obstacle at node j that the ant passes through, o j =1, otherwise o j =0, μ is a weighting coefficient used to adjust the importance of the number of obstacles in the actual cost. η is an adaptive weighting factor, d est (n) represents the estimated distance from the current node n to the target node, d total This represents the estimated total distance between the starting node and the target node. The complexity of the path has been taken into account. n is the number of nodes that the ant has already visited, N is the total number of nodes between the starting node and the target node, and ξ is a weighting coefficient.

[0053] Furthermore, in step 2, the path smoothness optimization index is calculated as follows:

[0054]

[0055] Where: m is the number of critical nodes on the path (including the starting point, turning points, and target point, etc.), R i Let θ be the radius of curvature corresponding to the i-th path segment. n is the number of turning points on the path, and θ is... j Let α be the turning angle of the j-th turning point. j It is the weighting coefficient for the turning angle.

[0056] Furthermore, after generating a candidate path, we evaluate it according to the smoothness evaluation index J mentioned above. If the J value is large, it indicates that the path is not smooth enough and needs to be smoothed. The calculation formula is:

[0057]

[0058] Where C(t) represents the Bézier curve function, used to describe the coordinates of points on the curve. n is the degree of the Bézier curve, affecting the shape complexity of the curve. P i This represents the i-th control point, used to determine the approximate shape of the curve. B i,n C'(t) is an nth-degree Bernstein polynomial, a function of the parameter t, used to determine the weighted contribution of control points to the curve. C'(t) is the first derivative of the Bézier curve, used to represent the direction of the tangent at a point on the curve. C'(t) is the second derivative of the Bézier curve, used in calculations related to the curvature of the curve.

[0059] The path planning method for tracked inspection robots based on ant colony optimization algorithm proposed in this invention enables inspection robots to plan simple and smooth motion paths in complex industrial environments and to avoid unknown obstacles.

[0060] For global planning, an adaptive weighting factor and safety radius are introduced into the path evaluation function of the ant colony optimization algorithm. Redundant nodes are optimized and circular arcs are applied to the global path, significantly improving the algorithm's safety, turning angles, and smoothness, while also resulting in shorter paths. For local planning, a kinematic model of the inspection robot is first established. A dynamic window method with an improved safety distance evaluation sub-function is adopted, enhancing the robot's ability to avoid unknown obstacles. By combining the improved ant colony optimization algorithm with the dynamic window method, the inspection robot gains the ability to find the shortest obstacle-free path in complex industrial environments, enabling real-time obstacle avoidance and collision-free arrival at the target location.

[0061] In summary, the path planning method for tracked inspection robots based on ant colony optimization algorithm of this invention is innovative and practical. By improving the ant colony optimization algorithm and using the ant colony optimization dynamic window method, it achieves efficient and accurate autonomous path planning for tracked inspection robots in complex industrial environments, providing strong support for the development of industrial automation and intelligence. In the future, we will continue to conduct in-depth research and optimization of this algorithm to further improve the performance and application scope of inspection robots, making greater contributions to promoting the development of the industrial field.

Claims

1. A path planning method for a tracked inspection robot based on ant colony optimization algorithm, characterized in that, Includes the following steps: Step 1: Construct an environmental grid map for the tracked inspection robot, dividing the robot's environment into grids of the same size, and setting the starting point and target point of the tracked inspection robot; Step 2: Improve the traditional ant colony algorithm by using an improved ant colony optimization algorithm for global path planning to obtain the optimal path; the improvements include: S1 introduces an adaptive weighting factor into the path quality evaluation function of the ant colony algorithm. The factor is dynamically adjusted according to the probability of the ants encountering obstacles during the search process. S2 removes redundant turning points from the path; S3 smooths the critical nodes of the processed path using Bézier curves; the path quality evaluation function is: F ( n )= G ( n )+ H ( n ) in, c i = k⋅d , For the ant to travel from the starting point to the current node n The cumulative moving costs, d obs ( n ) is the current node n The minimum distance from the path to the next node to the nearest obstacle. r safe To determine the safe radius of the inspection robot, Weighted by safety cost, o j For binary indicator variables, nodes j When there are obstacles o j =1， Otherwise 0 , These are the weighting coefficients. As an adaptive weighting factor, d est (n) For the current node n The estimated distance to the target node. d total This is the estimated total distance between the starting point and the target point. These are the weighting coefficients. n This represents the number of nodes the ant has visited. N This represents the total number of nodes between the starting point and the target point. Step 3: Use a lidar sensor to collect information about the surrounding environment and identify unknown static and dynamic obstacles on the global map; Step 4: Process the environmental information data collected in Step 3, update the internal environment of the dynamic window, and determine whether there are any unknown static and dynamic obstacles; Step 5: If no obstacle is detected, proceed along the shortest obstacle-free path; if an obstacle is detected, the tracked inspection robot performs local path planning to avoid the obstacle, and returns to the optimal path to continue driving after obstacle avoidance is completed. Step 6: Use the dynamic window method with an improved safety distance evaluation sub-function to perform local path planning to avoid obstacles. The improved safety distance evaluation sub-function is: in, d total (n) From the starting point to the current node n The total path length, d min ( n ) is the current node n The shortest distance to surrounding obstacles. , Let be the penalty intensity constant. d th For the safety threshold distance, v (n) For the inspection robot at the current node n speed, , , , The weight coefficient is used; the tracked inspection robot moves along the locally planned path, avoids obstacles, returns to the optimal path, and determines whether it has reached the target point. If it has, the algorithm ends; otherwise, it proceeds to step 4.

2. The path planning method for a tracked inspection robot based on ant colony optimization algorithm according to claim 1, characterized in that, Step 2 improves the traditional ant colony algorithm, as follows: S1: An adaptive adjustment factor is introduced into the ant colony pheromone update rule. This factor changes dynamically based on the frequency of obstacles encountered by ants during the search path. When ants frequently encounter obstacles in a certain area, the evaporation rate of pheromones in that area is adjusted to change the amount of pheromone accumulation, so as to guide subsequent ants to explore more other feasible paths. S2: Remove all redundant stalls in the planned path of S1. By backtracking the path, identify and eliminate these redundant stalls to obtain a simpler and more efficient path. S3: Perform curve fitting on the key nodes on the path processed in S2 to reduce the tortuosity and turning angle of the path, making it more in line with the kinematic characteristics of the inspection robot, reducing energy loss and mechanical wear during the robot's movement, and improving the stability and smoothness of path planning.

3. The path planning method for a tracked inspection robot based on ant colony optimization algorithm according to claim 1, characterized in that, In step 6, the safe distance evaluation sub-function in the ant colony optimization algorithm is improved, and the specific formula is as follows: in, The azimuth deflection angle evaluation sub-function represents the angle difference between the current forward direction of the inspection robot and the direction of the target point; d For the safety distance evaluation sub-function, d=min { d 1 ,d 2 ,d 3 } ，d 1 Simulate the shortest distance between globally known obstacles and the ant's current position for its next path. d 2 Simulate the shortest distance between an unknown dynamic obstacle and the ant's current location for its next path. d 3 Simulate the shortest distance between a static obstacle and the ant's current location for its next path. p The safety penalty function is used when the path chosen by the ant is too close to the obstacle. p Increase the probability of that path being chosen; v Let be the speed evaluation function, representing the ant's movement speed in the current state; , , , These are weighting coefficients used to adjust the impact of the inspection robot's movement direction, distance, and speed on the evaluation function results.

4. The path planning method for a tracked inspection robot based on ant colony optimization algorithm according to claim 2, characterized in that, In S2, the path quality evaluation function is optimized, and the calculation formula is as follows: f(n) = g(n) + h(n) g ( n )= h ( n )= in, f ( n () is from the starting point through the node n Cost estimation to reach the target point; g ( n ) represents the actual cost from the starting point to the node in the state space, including the cost of the ant moving from the starting point to the current node and the additional cost considering obstacles. h ( n ) is a slave node n The estimated cost of the optimal path to the target state; The safety cost weight is used to adjust the degree of influence of obstacle factors on path cost. This is an adaptive weighting factor, whose value is dynamically adjusted based on the obstacles encountered by the ant during the search process. When the probability of the ant encountering an obstacle in a certain area is high, the weighting factor is adjusted accordingly. Decrease the number of paths to increase caution in exploring the area, or increase it to encourage a wider search. c i For ants from nodes i -1 to node i The cost of moving between two points is directly proportional to the distance between them. d obs ( n () represents the minimum distance from the current node to the nearest obstacle along the path between the current node and the next node; r safe The safe radius for the inspection robot; d est ( n ) is from the current node n The estimated distance to the target node is estimated using Euclidean distance. d total This represents the estimated total distance between the starting node and the target node.

5. The path planning method for a tracked inspection robot based on ant colony optimization algorithm according to claim 2, characterized in that, In step 2, the path smoothness optimization index is calculated as follows: in, m This represents the number of critical nodes on the path, including the starting point, turning points, and the target point. R i For the first i The radius of curvature corresponding to the path segment; n The number of turning points on the path. For the first j The turning point's angle. It is the weighting coefficient for the turning angle.

6. The path planning method for a tracked inspection robot based on ant colony optimization algorithm according to claim 2, characterized in that, In S2, the obtained path is smoothed using the following formula: in, C ( t () represents a Bézier curve function, used to describe the coordinates of points on the curve; n The degree of the Bézier curve affects the shape complexity of the curve; P i Indicates the first i Several control points are used to determine the approximate shape of the curve; B i,n ( t )for n The second Bernstein polynomial is a polynomial with respect to parameters. t The function is used to determine the weighted contribution of control points to the curve; The first derivative of the Bézier curve is used to indicate the direction of the tangent to the curve at a certain point; This is the second derivative of the Bézier curve, used for calculations related to the curve's curvature.

Images