Path planning method and system for operation of transformer substation oil sample collection robot

Abstract

The invention provides a path planning method and system for operation of a transformer substation oil sample collection robot, and relates to the technical field of robot path planning. The method comprises the following steps: firstly, constructing an elliptical state subset space based on an initial point, a target point and a current optimal path cost in a free space of a configuration space, sampling to obtain a random point, and generating a new node meeting a constraint condition by using nearest neighbor search and a Steer function; a neighborhood point set is constructed through a Near function, a father node is reselected by adopting a ChoseParen function and taking the minimum path cost as a criterion, a bidirectional search tree is constructed by combining a reverse extension strategy starting from an initial point and a target point at the same time, and a work path is output when the node distance in the two search trees is smaller than a set value. The method can improve path planning search efficiency and path generation quality, and is suitable for transformer substation oil sample collection robot operation path planning.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, and in particular to a path planning method and system for a robot operating an oil sample collection system in a substation. Background Technology

[0002] With the rapid development of modern society, electricity has become an indispensable energy source supporting production and daily life. This trend has driven the national power system to evolve towards larger scale and greater complexity. Against this backdrop, substations, as key nodes in the power system, are increasing in number daily. However, current substation inspections still primarily rely on manual sampling, a method that is not only labor-intensive but also has many limitations. Manual oil sampling cannot guarantee that the samples will not be contaminated, easily leading to misjudgments of the health status of oil-filled equipment, making it difficult to identify and resolve some safety hazards in a timely manner. Especially in remote areas or under extreme weather conditions, the difficulty and workload of manual inspections increase significantly, and the probability of misjudgment also rises, posing potential risks to the safety and stability of the power system.

[0003] To address the aforementioned issues, the adoption of oil sampling robots to replace manual oil collection has become an inevitable trend in the industry. These oil sampling robots can sample oil-filled equipment in outdoor environments through autonomous navigation or remote control in substations with no or minimal personnel on duty, thereby effectively reducing equipment hazards and improving the safety and reliability of substation operation.

[0004] However, current oil extraction robots still face numerous technical challenges in practical applications, the most critical of which is path planning. Specifically, intelligent oil extraction robots need to accurately locate the stopping position of the oil filling equipment in the complex substation environment based on the specific needs of the oil extraction task, and possess the ability to automatically identify obstacles, thereby planning a safe and efficient path. Although significant progress has been made in path planning research under deterministic environments, research specifically for the unique scenario of substations is relatively limited, and existing solutions are also quite limited.

[0005] Existing path planning methods for substation oil sampling robots have the following drawbacks: 1. The rail-guided method is relatively simple in principle and not complicated in operation, but it requires the installation of guide rails, which is costly and requires a large area. Robots using this method cannot be flexibly configured or have their work paths adjusted, thus failing to adapt to changes in substation meters, equipment, and facilities. It is not well-suited for the confined space of substations, limiting its development potential.

[0006] 2. The RRT algorithm is used to plan the robot's route. The RRT algorithm does not require specific modeling of the environment and is suitable for complex environments or scenarios with dynamic constraints. However, it also has some problems, such as: random sampling is highly blind; it is difficult to find the optimal path; and the generated path is not smooth.

[0007] 3. Use RRT The algorithm employs multiple iterations and is an asymptotically optimized uniform sampling algorithm. After finding an initial path, the resulting path becomes increasingly optimal with each iteration. However, its drawbacks are also apparent: a large sampling space; numerous sampling nodes; and slow convergence to the optimal path. Summary of the Invention

[0008] In order to overcome the shortcomings of the prior art, the purpose of this invention is to provide a path planning method and system for substation oil sample collection robot operations, which can improve the search efficiency of path planning and the quality of path generation.

[0009] To achieve the above objectives, the present invention provides the following solution: A path planning method for a substation oil sampling robot includes: Within the free space of the configuration space, random points are obtained by the Sample function sampling within the elliptical state subset space constructed based on the initial point, the target point, and the current optimal path cost; The Nearest function finds the node in the random tree that is closest to the random point and selects it as the nearest node. The Steer function generates a new node with the nearest node as the base point, along the direction from the nearest node to the random point, and determines the nearest node as the original parent node of the new node. When a new node satisfies the constraints, the Near function searches for nodes within a specified radius in the random tree to form a point set. The ChooseParen function iterates through the point set, compares the path costs from the initial point through each node in the point set to the new node, and determines the node with the lowest cost as the parent node of the new node, and disconnects the new node from the original parent node. A reverse expansion strategy is adopted, and two search trees are constructed with the initial point and the target point as the starting points respectively. During the process of random sampling and tree expansion on the elliptical state subset space, the current optimal path cost is dynamically updated, and the job path is output when the distance between two nodes in the two search trees is less than a set value.

[0010] Preferably, within the free space of the configuration space, random points are sampled by the Sample function within an elliptical subset of states constructed based on the initial point, the target point, and the current optimal path cost, including: The initial point and the target point are taken as the two foci of the elliptical state subset space; Determine the axial scale of the elliptical state subset space and the center of the elliptical state subset space based on the current optimal path cost; Generate uniform sampling points within the unit circle; The uniformly sampled points are sequentially transformed by the transformation matrix, rotated and translated by the rotation matrix to obtain random points located in the elliptical state subset space.

[0011] Preferably, within the free space of the configuration space, random points are sampled by the Sample function within an elliptical subset of states constructed based on the initial point, the target point, and the current optimal path cost, including: The distance between the initial point and the target point is determined as the theoretical cost of the path between the initial point and the target point; During the algorithm iteration process, the elliptical state subset space is updated based on the theoretical cost and the current optimal path cost, so that the elliptical state subset space gradually shrinks to the line segment region between the initial point and the target point as the current optimal path cost converges.

[0012] Preferably, when a new node satisfies the constraints, the Near function searches for a set of nodes within a specified radius in the random tree, including: Determine the domain range corresponding to a specified radius, centered on the new node; Retrieve nodes that fall within the neighborhood of the random tree and form a point set from the retrieved nodes.

[0013] Preferably, the ChooseParen function iterates through the point set, compares the path costs from the initial point through each node in the point set to the new node, determines the node with the lowest cost as the parent node of the new node, and disconnects the new node from the original parent node, including: Calculate the path cost from the initial point to the current node for each node in the point set; Calculate the path cost from the current node to the new node for each node in the point set; The cost of the candidate path from the initial point to the new node is determined based on the cost of the two paths, with the current node as the parent node. The node with the lowest candidate path cost is designated as the parent node of the new node, and the connection between the new node and the original parent node is disconnected.

[0014] Preferably, a reverse expansion strategy is adopted, constructing two search trees starting from the initial point and the target point respectively, including: In each iteration, one of the search trees is selected for expansion, and new nodes for the expanded search tree are generated. The expansion of the new node serves as the direction for the expansion of another search tree, causing the other search tree to expand toward the expansion of the new node; During the expansion of another search tree, if no collision occurs during the expansion, the expansion continues in the same direction until the expansion fails or the tree is connected to one of the search trees.

[0015] Preferably, the current optimal path cost is dynamically updated during the random sampling and tree expansion process on the elliptical state subset space, including: Set the initial value of the current optimal path cost to the theoretical cost of the path between the initial point and the target point, and make the elliptical state subset space the line segment region between the initial point and the target point. During tree expansion, check whether new nodes collide with obstacles in the environment; When a new node does not collide with any obstacles in the environment, the line connecting the initial point and the target point is determined as the operation path. When a new node collides with an obstacle in the environment, the elliptical state subset space is updated by increasing the cost of the current optimal path, and random sampling and tree expansion are repeated until a feasible path that meets the constraints is found in the updated elliptical state subset space.

[0016] Preferably, the current optimal path cost is dynamically updated during the random sampling and tree expansion process on the elliptical state subset space, including: After generating the node, a detection function is used to detect whether there is occlusion within a single distance interval around the node; When the detection result indicates that there is no occlusion, the target point is used as the target point to perform an expansion to obtain a tree node; When the detection result indicates that occlusion exists, a target bias probability threshold is set, and the target bias probability threshold is less than 10%, and a random probability value is generated. When the random probability value is greater than the target bias probability threshold, a random function is used to generate target points for expansion; when the random probability value is less than the target bias probability threshold, the target point is used as the target point for expansion.

[0017] Preferably, when the distance between two nodes in two search trees is less than a set value, the job path is output, including: Import path nodes obtained during the path generation process; Starting from the first path node, check whether the connecting line between the starting point and subsequent path nodes intersects with the obstacle space; When the connecting line does not intersect with the obstacle space, delete the intermediate path node between the starting point and the subsequent path node; When the connecting line intersects with the obstacle space, the inspection continues from the new path node as the starting point until the straightened operation path is obtained.

[0018] A path planning system for a substation oil sampling robot includes: The random sampling unit is used to sample random points in the free space of the configuration space by the Sample function within the elliptical state subset space constructed based on the initial point, the target point, and the current optimal path cost. The nearest node search unit is used by the Nearest function to find the node in the random tree that is closest to the random point and select it as the nearest node. The node expansion unit is used to generate a new node by using the Steer function with the nearest node as the base point and along the direction from the nearest node to the random point, and to determine the nearest node as the original parent node of the new node; The neighborhood node search unit is used to search for a set of nodes within a specified radius in a random tree using the Near function when a new node meets the constraints. The parent node reselection unit is used by the ChooseParen function to traverse the point set, compare the path costs from the initial point through each node in the point set to the new node, determine the node with the lowest cost as the parent node of the new node, and disconnect the new node from the original parent node. The reverse expansion and path generation unit is used to construct two search trees with the initial point and the target point as the starting points, respectively, using a reverse expansion strategy. During the process of random sampling and tree expansion on the elliptical state subset space, the current optimal path cost is dynamically updated, and the job path is output when the distance between two nodes in the two search trees is less than a set value.

[0019] The present invention discloses the following technical effects: (1) This invention addresses the problems of non-targeted sampling, severe node redundancy, and slow convergence speed of traditional RRT-type path planning methods in substation operation environments. By constructing an elliptical state subset space within the free space of the configuration space for guided sampling, the sampling points are concentrated within the potential optimal path region, thereby significantly reducing invalid sampling, reducing the search space size, and improving path planning efficiency and path convergence speed.

[0020] (2) In view of the problem that the initial connection strategy of the parent node in the traditional path generation leads to the inability to optimize the path cost and the generation of a lot of redundant segments, the present invention compares the path costs of neighboring nodes after the node is generated and selects the node with the smallest path cost as the parent node of the new node, so that the path has the shortest path tendency in the generation stage, thereby reducing path detours and redundant segments, and improving the quality and feasibility of the generated path.

[0021] (3) In view of the problem that traditional one-way expansion strategy is prone to getting trapped in local search in the case of dense obstacles or limited space, resulting in failure of path generation, the present invention adopts a reverse expansion strategy to construct dual search trees from the initial point and the target point respectively and to determine the connection by using the distance threshold between the trees, thereby improving the obstacle crossing ability and the success rate of path generation, and enhancing the applicability and reliability in complex substation operation scenarios. Attached Figure Description

[0022] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0023] Figure 1 A flowchart of the method provided in an embodiment of the present invention; Figure 2 A flowchart of the design method provided for embodiments of the present invention; Figure 3 A schematic diagram of the initialization state subset provided in an embodiment of the present invention; Figure 4 This is a schematic diagram of the updated state subset provided in an embodiment of the present invention; Figure 5 This is a schematic diagram of finding a feasible path provided in an embodiment of the present invention; Figure 6 Informed-RRT provided for embodiments of the present invention Schematic diagram of the sampling space. Detailed Implementation

[0024] The technical solutions of 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.

[0025] The purpose of this invention is to provide a path planning method and system for substation oil sample collection robot operations, which can improve the convergence speed of path planning and the reliability of path generation.

[0026] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0027] Figure 1 The method flowchart provided in the embodiments of the present invention is as follows: Figure 1 As shown, this invention provides a path planning method for a substation oil sample collection robot operation, including: Step 100: Within the free space of the configuration space, random points are sampled by the Sample function within the elliptical state subset space constructed based on the initial point, the target point, and the current optimal path cost; Step 200: The Nearest function finds the node in the random tree that is closest to the random point and selects it as the nearest node; Step 300: Using the Steer function, generate a new node with the nearest node as the base point, along the direction from the nearest node to the random point, and determine the nearest node as the original parent node of the new node; Step 400: When a new node satisfies the constraints, use the Near function to search for nodes within a specified radius in the random tree to form a point set; Step 500: The ChooseParen function iterates through the point set, compares the path cost from the initial point through each node in the point set to the new node, determines the node with the lowest cost as the parent node of the new node, and disconnects the new node from the original parent node. Step 600: Using a reverse expansion strategy, construct two search trees with the initial point and the target point as the starting points respectively. During the process of random sampling and tree expansion on the elliptical state subset space, dynamically update the current optimal path cost, and output the job path when the distance between two nodes in the two search trees is less than a set value.

[0028] The purpose of this invention is to provide a method based on Informed-RRT. The algorithm proposes a path planning method for substation oil sampling robot operations, using an improved Informed-RRT. The algorithm design achieves a high convergence speed; the specific design method is as follows: Figure 2 As shown, it includes: 1) Sampling: The Sample function samples in the free space of the configuration space to obtain a random point x. rand .

[0029] 2) Finding the nearest point: The Nearest function is used to find the nearest point x in a random tree T. rand The nearest node x nearest .

[0030] 3) Generate new nodes; the Steer function uses x... nearest Using x as the base point, along rand Generate a new node x in the direction new .

[0031] 4) Find X nearIf the generated random points meet the constraints, the point set X is formed by searching for nodes within a specified radius in T using the Near function. near .

[0032] 5) Select the parent node; the ChooseParen function is RRT. The key algorithm is capable of obtaining a shorter feasible path. This algorithm iterates through x... new Point set X within the domain near Compare each node from the initial node to x. i ∈X near With x i The cost of a new node is used to select the node x with the lowest cost. min As x new The parent node, and disconnected from the original parent node.

[0033] 6) Employ a reverse expansion strategy, constructing search trees starting from the initial and target points. In each iteration, expand one tree while attempting to expand using the nearest point in the other tree as a random point. The search is considered complete when the distance between any two nodes in the two trees is less than a set value. This significantly improves the efficiency of expanding the random tree within the configuration space.

[0034] As an optional implementation method, unlike existing path planning methods, this invention can inherit RRT. The algorithm demonstrates completeness and asymptotic optimality; and accelerates the convergence speed of path planning through heuristic direct sampling based on elliptic state subsets; exhibiting reliable state estimation performance.

[0035] The path planning method for substation oil sampling robot operations includes the following steps: Step 1: Sampling. The Sample function samples in the free space of the configuration space to obtain a random point x. rand .

[0036] The algorithm's advantage lies in introducing a method for directly sampling a subset of elliptical states. During the iterative process, the algorithm constructs an elliptical state subset space based on the initial point, the target point, and the cost Cmax of the optimal path found in the current iteration.

[0037] Among them, C min C represents the distance between the two foci, and also the theoretical cost of the path between the starting and target points. best Let be the major axis and be the minor axis of the ellipse. When the ellipse converges to the line segment between the two foci, C max =C best .

[0038] Compared to random sampling of the entire state space, sampling based on an elliptic state subset x ellipse ~U(x ellipse A uniformly sampled sample can pass through a unit circle n-ball,x ball ~U(x ball The samples are uniformly distributed in the matrix and obtained through matrix transformation, rotation, and translation.

[0039] x ellipse =L x ball + Where, x ball L represents the uniform sampling points in the unit circle, and L represents the transformation matrix. It is the center of the ellipse, and its two foci are x and y. f1 and x f2 .

[0040] The transformation matrix is ​​calculated using the Cholesky decomposition of the hyperelliosoid matrix. LL T ≡S,S ∈R n ×n .

[0041] ( x- ) T S( x- )=1 The hyperelliosoid matrix is ​​obtained by decomposition: S=diag{ ,… } Therefore: L=diag{ ,… } diag{·} represents a diagonal matrix.

[0042] Furthermore, due to the Wahba problem, the rotation matrix from the unit circular coordinate system to the elliptical coordinate system can be obtained: C=U diag{1,…,1,det(U)det(V)}V T Therefore, the unit circle n-ball, x ball ~U( x ball ) After processing with a transformation matrix, a rotation matrix, and a translation, uniformly distributed samples are transformed into uniformly sampled points located in an elliptical subset of states. Xf =CLx ball + This process is represented algorithmically as Algorithm2.

[0043] Step 2: Find the nearest point The Nearest function is used to find the distance x from a random point in a random tree T. rand The nearest node x nearest .

[0044] Step 3: Generate new nodes Using the Steer function with x nearest Using x as the base point, along rand Generate a new node x in the direction new .

[0045] Step 4: Find Xnear If the generated random points meet the constraints, the point set Xnear is formed by searching for nodes within a specified radius in T using the Near function.

[0046] Step 5: Select the parent node The ChooseParen function is an RRT The key algorithm is capable of obtaining a shorter feasible path. This algorithm iterates through x... new Point set X within the domain near Compare each node from the initial node to x. i ∈X near With x i The cost of a new node is used to select the node x with the lowest cost. min As x new The parent node, and disconnected from the original parent node.

[0047] Step 6: Generate the state subset space The reverse expansion strategy will C best =C min As the initial state for path planning, a subset of states is constructed. At this point, the sampling space is a line segment between the starting point and the target point, such as... Figure 3 As shown.

[0048] Step 7: Update the state subset The Algorithm2 algorithm is used to randomly sample the state subset intervals and construct a random tree. During the expansion process, the state of new nodes is checked and evaluated. If the newly generated node never collides with obstacles in the environment, then the space between the starting point and the target point is free space, and the line connecting the two points is the optimal path. Otherwise, the state is adjusted by increasing C. best Update the subset of states, such as Figure 4 As shown, that is: C best = ·r r= ·( ) d in, is the expansion coefficient of the elliptic state subset. is a constant, and n is the number of random tree nodes.

[0049] Step 8: Find feasible paths Repeat step 7 until a feasible path satisfying the constraints is found in the new subset of states, such as... Figure 5 As shown.

[0050] Traditional RRT or RRT The algorithm explores the entire state space by generating a rapidly expanding random tree. The basic principle of the bidirectional expansion algorithm is to construct search trees starting from the initial and target points. In each iteration, one tree is used for expansion, while the nearest node in the other tree is used as a random point for further expansion. The search is considered complete when the distance between any two nodes in the two trees is less than a predetermined value. The advantage of the bidirectional expansion strategy is that it greatly improves the efficiency of expanding the random tree in the configuration space.

[0051] The improved algorithm compared to the traditional RRT Compared to other algorithms, this one has the following advantages: 1) Inherited from RRT Completeness and asymptotic optimality; 2) By using heuristic direct sampling based on elliptic state subsets, the convergence speed of path planning is accelerated; 3) Regardless of the dimensions of the configuration space, Informed-RRT Both algorithms can find a feasible path with an approximate optimal solution within a finite time, and their path planning capabilities are significantly better than RRT. algorithm.

[0052] like Figure 6 As shown, this is the Informed-RRT used in this invention. The sampling space of the algorithm, Informed-RRT The algorithm's advantage lies in introducing a method for directly sampling a subset of elliptical states. During the iterative process, the algorithm constructs an elliptical state subset space based on the initial point, the target point, and the cost Cmax of the optimal path found in the current iteration.

[0053] Among them, C min C represents the distance between the two foci, and also the theoretical cost of the path between the starting and target points. bestLet be the major axis and be the minor axis of the ellipse. When the ellipse converges to the line segment between the two foci, C max =C best .

[0054] Compared to random sampling of the entire state space, sampling based on an elliptic state subset x ellipse ~U(x ellipse A uniformly sampled sample can pass through a unit circle n-ball,x ball ~U(x ball The samples are uniformly distributed in the matrix and obtained through matrix transformation, rotation, and translation.

[0055] x ellipse =Lx ball + Where, x ball L represents the uniform sampling points in the unit circle, and L represents the transformation matrix. It is the center of the ellipse, and its two foci are x and y. f1 and x f2 .

[0056] The transformation matrix is ​​calculated using the Cholesky decomposition of the hyperelliosoid matrix, where LLT≡S,S∈Rn×n.

[0057] (x- )TS(x- )=1 The hyperelliosoid matrix is ​​obtained by decomposition: S=diag{ ,… } Therefore L=diag{ ,… } diag{·} represents a diagonal matrix.

[0058] Furthermore, due to the Wahba problem, the rotation matrix from the unit circular coordinate system to the elliptical coordinate system can be obtained: C=U diag{1,…,1,det(U)det(V)}VT Therefore, the unit circle n-ball,x ball ~U(x ball The uniformly distributed samples in the elliptical state subset are processed by transformation matrix, rotation matrix, and translation to obtain uniformly sampled points located in the elliptical state subset: Xf=CLx ball + This process is represented algorithmically as Algorithm2.

[0059] To verify the advantages of the algorithm, this invention conducted a sampling comparison experiment on a two-dimensional map with a unit length of 4010×4010 using MATLAB. The hardware and software configurations are shown in Table 1.

[0060] Table 1 Hardware and Software Configuration Table

[0061] By analyzing the mean and standard deviation of the number of iterations for the four algorithms in Table 2, it can be seen that the IBI-RRT algorithm... The average number of iterations compared to Informed-RRT The algorithm reduces the computational cost by half and has the smallest standard deviation among all algorithms. This demonstrates that the new algorithm not only converges faster but also exhibits higher stability.

[0062] Table 2 Algorithm Comparison Data Table

[0063] Theoretical research shows that the optimal path in the state space is distributed in the boundary region of the obstacle. Combining the advantages of direct sampling of elliptical state subsets, a reverse expansion strategy is proposed. The algorithm path is as follows: 1) Generate a subset space of states The reverse expansion strategy will C best =C min As the initial state for path planning, a subset of states is constructed. At this point, the sampling space is a line segment between the starting point and the target point, such as... Figure 3 As shown.

[0064] 2) Update the state subset The Algorithm2 algorithm is used to randomly sample the state subset intervals and construct a random tree. During the expansion process, the state of new nodes is checked and evaluated. If the newly generated node never collides with obstacles in the environment, then the space between the starting point and the target point is free space, and the line connecting the two points is the optimal path. Otherwise, the state is adjusted by increasing C. best Update the subset of states, such as Figure 4 As shown, that is: C best = ·r r= ·( )d in, is the expansion coefficient of the elliptic state subset. is a constant, and n is the number of random tree nodes.

[0065] 3) Finding feasible paths Repeat step (2) until a feasible path satisfying the constraints is found in the new subset of states, such as... Figure 5 As shown.

[0066] Traditional RRT or RRT The algorithm explores the entire state space by generating a rapidly expanding random tree. The basic principle of the bidirectional expansion algorithm is to construct search trees starting from the initial and target points. In each iteration, one tree is used for expansion, while the nearest node in the other tree is used as a random point for further expansion. The search is considered complete when the distance between any two nodes in the two trees is less than a predetermined value. The advantage of the bidirectional expansion strategy is that it greatly improves the efficiency of expanding the random tree in the configuration space.

[0067] The basic idea of ​​the bidirectional RRT algorithm is to construct two search trees, one starting from the target and the other from the target. During each iteration, one tree is expanded by connecting the nearest nodes in the other tree to create new nodes. The search is considered complete when the distance between any two nodes in the two trees is less than a predetermined value. Here, a directed graph G=(V, E) is used to represent the path; a feasible path is a sequence of vertices. , , , ..., ),in = Vn= At the same time (V) i V i+1 Let ∈ E, where 1 ≤ i ≤ n-1 represents an edge; E is the set of available points within the graph. The basic bidirectional RRT algorithm is as follows: Let the starting configuration be The target configuration is To contain a set of nodes Sum of edges The picture This represents a search tree that expands from the starting point to contain a set of nodes. Sum of edges The picture This represents the search tree expanded from the target side, where the initial time is set to... and count the current iteration. Set to zero if the number of iterations does not exceed the preset limit. The bidirectional expanding loop is started under the condition of [condition].

[0068] In the main loop, random sample points are first generated using the Sample function. And increment the iteration count by one; then search the tree on the starting side. The function `Nearest` is called to find the distance from the random sample point. The nearest node Then, through the Steer function from Orientation Direction generates new nodes If the starting side is from arrive If the line segment satisfies the ObstacleFree criterion, meaning it does not collide with obstacles in the environment, then it will be... Add to node set and the edge Add to edge set Based on this, a target-side search tree is used. For an object, call the Nearest function in Finding distance The nearest node And generate from the Steer function Orientation new node If from arrive If the connection also satisfies the ObstacleFree condition, then... Add to target side node set and the edge Add to target side set .

[0069] To gradually narrow the distance between the two search trees, after obtaining the node... Then, a multi-step expansion is performed along the direction from the target side to the starting point using an inner do-while loop: in this loop, the current node... Starting from, with To achieve the goal, the Steer function is called to generate a new extended node. If from arrive If the connection satisfies the ObstacleFree criterion, then... Add to node set and the edge Add to edge set At the same time, Continue expanding; if a collision occurs during any expansion, exit the inner loop. When the inner loop ends and... arrive When the two search trees are considered successfully connected, the algorithm returns the search tree from the starting side. The corresponding complete path; if not yet connected, compare the node sizes of the two search trees, and perform a Swap operation to exchange nodes when the number of nodes on the target side is less than the number of nodes on the starting side. and To maintain a relative balance in the size of the two trees, the above iterations continue until a feasible path is found or the number of iterations reaches the upper limit.

[0070] Furthermore, the first two steps of the above algorithm are based on the starting point. and the end point As the starting points for two spanning trees, both points are checked to ensure they are within the graph. Then, in each iteration, the initial steps are the same as the original RRT algorithm: sampling random points and then expanding. The new nodes of the expanded first tree... Then, this new target point is used as the direction for expanding the second tree. The second tree expands slightly differently; it first expands the tree obtained in the first step. If there is no collision, continue expanding in the same direction for the second step until the expansion fails or = If the connection is to the first tree, the entire algorithm ends.

[0071] In practical applications, due to the need to consider the robot's size, the (x, y) coordinates between the state space edge and the robot's center must be maintained. Therefore, when importing the state space graph, the state space edge needs to be extended and protected according to the robot's size settings to prevent nodes from colliding due to excessive proximity to the state space edge.

[0072] In the implementation of the algorithm, the expansion sampling involves uniformly and randomly sampling the entire state space. While this method can effectively search all unknown spaces, it performs a large number of samples at too many unnecessary locations and expands into multiple branches, greatly wasting search time and significantly reducing the algorithm's convergence speed.

[0073] To overcome this deficiency, this invention employs a variable-probability target selection strategy based on node environment. After generating a node, the function `detect(q)` is used to detect whether there are occlusions (i.e., points not included in the available state space) within a single distance interval. If no occlusion is found, the endpoint is selected as the target point for expansion, resulting in a tree node; otherwise, a predetermined target bias probability value `p` is used. set (Generally set to less than 10%), when the probability of this random generation p > p set When p < p, a random function is used to generate the result. set In this case, the endpoint is used as the target point.

[0074] This approach ensures both the randomness of the algorithm and accelerates convergence to the target state, while also achieving significant improvements in relatively open state spaces. Compared to the original algorithm, environmental constraints are added during path generation, which can purposefully alter the selection probability of target nodes. In most scenarios, the algorithm's path will encounter nodes with unobstructed surroundings; the rapid convergence of these nodes greatly reduces the number of invalid path searches in practical applications, thus reducing the overall search time. It should be noted that the algorithm also has a certain probability of leading to local minima; therefore, a relatively large probability of random target points needs to be retained to ensure that path results are obtained during local searches. The optimized bidirectional RRT algorithm is as follows: In this embodiment, a bidirectional fast expanding random tree algorithm with a target bias mechanism is used to improve path search efficiency and target convergence. Specifically, let the starting configuration be... The target configuration is Construct a starting-side search tree and a target-side search tree respectively, where the set of nodes in the starting-side search tree is denoted as . The edge set is denoted as The set of nodes in the target-side search tree is denoted as The edge set is denoted as And set the current iteration count to zero. Simultaneously set the target bias probability threshold. As a probability parameter that controls the bias toward the target configuration during random sampling, it is used to increase the probability that the sampling point falls in the vicinity of the potential feasible path under the condition of satisfying the iterative constraints, thereby shortening the connection distance between bidirectional search trees.

[0075] During the specific path search process, a bidirectional expansion loop is executed as long as the current iteration count does not exceed a preset upper limit. In each iteration, a visibility or reachability check is first performed on the starting-side configuration. If the check result indicates that the starting-side configuration is directly reachable from the target configuration, the target configuration is directly used as a sampling point for subsequent expansion. Otherwise, a random determination is made based on a preset probability threshold. When the random probability value is less than the target bias probability threshold, the target configuration is used as a sampling point to induce the starting-side search tree to expand towards the target configuration. When the random probability value is greater than the target bias probability threshold, random sampling points are generated in the configuration space through a random sampling function to maintain spatial exploration capability. After the sampling points are determined, the nearest neighbor search function and the expansion function are called to perform node selection and node generation in sequence. Then, the bidirectional expansion of the starting-side and target-side search trees is completed according to the expansion method of the basic bidirectional fast expansion random tree algorithm, and a connection is attempted. When the connection between the search trees is successful, a feasible path is output.

[0076] Given the randomness of sampling in the RRT algorithm, despite the adoption of a target-oriented selection strategy, the generated paths are often unnatural and tortuous. In practical applications, vertices cause repetitive turning maneuvers for the intelligent robot, leading it to travel unnecessary paths and accelerating equipment wear. Therefore, based on practical applications, a path post-processing workflow based on the shortest path is proposed to straighten and remove unnecessary nodes from the generated path to shorten its length. The optimized algorithm flow is as follows: In this embodiment, after the bidirectional fast expanding random tree algorithm completes the path search and obtains the connection path from the initial configuration to the target configuration, path node detection processing is performed on the obtained path. Specifically, the complete path output by the bidirectional expanding algorithm is represented as a sequence of nodes. ,in As the initial configuration node, The target configuration node is defined, and the path is arranged in chronological order of the configuration nodes. A temporary pointer node is then set to point to the starting node of the path, used to advance segment by segment along the path during subsequent detection, thereby verifying the validity of subsequent nodes.

[0077] During path detection, if the temporary pointer node has not reached the target configuration node, a feasibility check is performed on each node in the path sequentially, and the connectivity between the starting node and the current node is verified using a path feasibility detection function. When the detection result shows that the connection from the starting node to the current node does not collide, the current node is recorded and the detection continues; when the detection result shows that there is a collision or impassable condition, the temporary pointer node is updated to the current node and the detection loop is exited, thus obtaining an optimized path node sequence that meets the obstacle avoidance requirements, achieving the elimination of redundant nodes and the improvement of path quality.

[0078] In the post-processing workflow, the path nodes obtained in the previous path generation are first imported. Then, starting from the first node, it is checked whether the connecting lines to subsequent nodes intersect with the obstacle space. If not, the node is deleted; otherwise, the connection of subsequent nodes is checked again starting from the new node. Compared with the original algorithm, the subsequent processing allows the intelligent robot's algorithm application to be more reasonable, reducing invalid turning nodes and paths, and further improving the rationality of the final path.

[0079] Corresponding to the above method, this embodiment also provides a path planning system for substation oil sample collection robot operations, including: The random sampling unit is used to sample random points in the free space of the configuration space by the Sample function within the elliptical state subset space constructed based on the initial point, the target point, and the current optimal path cost. The nearest node search unit is used by the Nearest function to find the node in the random tree that is closest to the random point and select it as the nearest node. The node expansion unit is used to generate a new node by using the Steer function with the nearest node as the base point and along the direction from the nearest node to the random point, and to determine the nearest node as the original parent node of the new node; The neighborhood node search unit is used to search for a set of nodes within a specified radius in a random tree using the Near function when a new node meets the constraints. The parent node reselection unit is used by the ChooseParen function to traverse the point set, compare the path costs from the initial point through each node in the point set to the new node, determine the node with the lowest cost as the parent node of the new node, and disconnect the new node from the original parent node. The reverse expansion and path generation unit is used to construct two search trees with the initial point and the target point as the starting points, respectively, using a reverse expansion strategy. During the process of random sampling and tree expansion on the elliptical state subset space, the current optimal path cost is dynamically updated, and the job path is output when the distance between two nodes in the two search trees is less than a set value.

[0080] The beneficial effects of this invention are as follows: This invention overcomes the problems of slow convergence speed and poor path quality of traditional RRT and its algorithms in complex constraint scenarios. It inherits the completeness and asymptotic optimality of the RRT algorithm and introduces heuristic sampling of a subset of elliptic states, concentrating the search space on potentially optimal path regions, thus significantly accelerating the convergence process of path planning. By employing a reverse expansion strategy to construct a bidirectional search tree between the initial and target configurations, it improves the connection success rate and path generation capability in complex obstacle environments. Furthermore, by optimizing node connection relationships through a parent node reselection mechanism based on path cost, the generated path possesses shorter path characteristics while maintaining feasibility, effectively reducing path costs. Through the above collaborative optimization, this invention can obtain a feasible path with near-optimal solution within a finite time, and its path planning efficiency and path quality are significantly better than traditional RRT. algorithm.

[0081] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0082] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A path planning method for a substation oil sample collection robot, characterized in that, include: Within the free space of the configuration space, random points are obtained by the Sample function sampling within the elliptical state subset space constructed based on the initial point, the target point, and the current optimal path cost; The Nearest function finds the node in the random tree that is closest to the random point and selects it as the nearest node. Using the Steer function, a new node is generated along the direction from the nearest node to the random point, with the nearest node as the base point, and the nearest node is determined as the original parent node of the new node; When the new node satisfies the constraints, the Near function searches the random tree for nodes within a specified radius to form a point set. The ChooseParen function iterates through the set of points, compares the path costs from the initial point through each node in the set to the new node, determines the node with the lowest cost as the parent node of the new node, and disconnects the new node from the original parent node. A reverse expansion strategy is adopted, and two search trees are constructed with the initial point and the target point as the starting points respectively. During the random sampling and tree expansion of the elliptical state subset space, the current optimal path cost is dynamically updated, and the job path is output when the distance between two nodes in the two search trees is less than a set value.

2. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, Within the free space of the configuration space, random points are sampled by the Sample function within an elliptical subset of states constructed based on the initial point, the target point, and the current optimal path cost. These points include: The initial point and the target point are taken as the two foci of the elliptical state subset space; The axis scale of the elliptical state subset space is determined based on the current optimal path cost, and the center of the elliptical state subset space is determined. Generate uniform sampling points within the unit circle; The uniform sampling points are sequentially subjected to transformation matrix transformation, rotation matrix rotation and translation processing to obtain the random points located in the elliptical state subset space.

3. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, Within the free space of the configuration space, random points are sampled by the Sample function within an elliptical subset of states constructed based on the initial point, the target point, and the current optimal path cost. These points include: The distance between the initial point and the target point is determined as the theoretical cost of the path between the starting point and the target point; During the algorithm iteration process, the elliptical state subset space is updated based on the theoretical cost and the current optimal path cost, so that the elliptical state subset space gradually shrinks to the line segment region between the initial point and the target point as the current optimal path cost converges.

4. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, When the new node satisfies the constraints, the Near function searches the random tree for a set of nodes within a specified radius, including: The domain range corresponding to the specified radius is determined with the new node as the center; Nodes falling within the specified domain are retrieved from the random tree, and the retrieved nodes are combined into the point set.

5. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, The `ChooseParen` function iterates through the set of points, compares the path costs from the initial point through each node in the set to the new node, determines the node with the lowest cost as the parent node of the new node, and disconnects the new node from the original parent node, including: Calculate the path cost from the initial point to the current node for each node in the point set; Calculate the path cost from the current node to the new node for each node in the point set; Based on the path costs of the two segments, determine the candidate path cost from the initial point to the new node when the current node is the parent node; The node with the lowest candidate path cost is determined as the parent node of the new node, and the connection between the new node and the original parent node is disconnected.

6. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, A reverse expansion strategy is adopted, and two search trees are constructed with the initial point and the target point as starting points respectively, including: In each iteration, one of the search trees is selected for expansion, and new nodes for the expanded search tree are generated. The newly expanded node is used as the direction of expansion for another search tree, causing the other search tree to expand toward the newly expanded node; During the expansion of the other search tree, expansion continues in the same direction if no collision occurs, until the expansion fails or the tree connects to one of the search trees.

7. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, The current optimal path cost is dynamically updated during the random sampling and tree expansion process of the elliptical state subset space, including: The initial value of the current optimal path cost is set as the theoretical cost of the path between the initial point and the target point, so that the elliptical state subset space is the line segment region between the initial point and the target point; During tree expansion, check whether the new node collides with obstacles in the environment; When the new node does not collide with any obstacles in the environment, the line connecting the initial point and the target point is determined as the operation path; When the new node collides with an obstacle in the environment, the elliptical state subset space is updated by increasing the cost of the current optimal path, and the random sampling and tree expansion are repeated until a feasible path that meets the constraints is found in the updated elliptical state subset space.

8. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, The current optimal path cost is dynamically updated during the random sampling and tree expansion process of the elliptical state subset space, including: After generating a node, a detection function is used to detect whether there is occlusion within a single distance interval around the node; When the detection result indicates that there is no occlusion, the target point is used as the target point for expansion to obtain a tree node; When the detection result indicates that there is occlusion, a target bias probability threshold is set, and the target bias probability threshold is less than 10%, and a random probability value is generated. When the random probability value is greater than the target bias probability threshold, a target point is generated using a random function for expansion; when the random probability value is less than the target bias probability threshold, the target point is used as the target point for expansion.

9. The path planning method for substation oil sample collection robot operation according to claim 1, characterized in that, When the distance between two nodes in two search trees is less than a set value, output the job path, including: Import path nodes obtained during the path generation process; Starting from the first path node, check whether the connecting line between the starting point and subsequent path nodes intersects with the obstacle space; When the connecting line does not intersect with the obstacle space, delete the intermediate path node between the starting point and the subsequent path node; When the connecting line intersects with the obstacle space, the check continues with the new path node as the starting point until the straightened operation path is obtained.

10. A path planning system for a robot operating an oil sample collection system in a substation, characterized in that, include: The random sampling unit is used to sample random points in the free space of the configuration space by the Sample function within the elliptical state subset space constructed based on the initial point, the target point, and the current optimal path cost. The nearest node search unit is used by the Nearest function to find the node in the random tree that is closest to the random point, and to select it as the nearest node. The node expansion unit is used to generate a new node using the Steer function with the nearest node as the base point, along the direction from the nearest node to the random point, and to determine the nearest node as the original parent node of the new node; The neighborhood node search unit is used to search for a set of nodes within a specified radius in the random tree using the Near function when the new node satisfies the constraints. The parent node reselection unit is used to traverse the point set by the ChooseParen function, compare the path costs from the initial point through each node in the point set to the new node, determine the node with the lowest cost as the parent node of the new node, and disconnect the new node from the original parent node. The reverse expansion and path generation unit is used to construct two search trees with the initial point and the target point as the starting points, respectively, using a reverse expansion strategy. During the random sampling and tree expansion process of the elliptical state subset space, the current optimal path cost is dynamically updated, and the job path is output when the distance between two nodes in the two search trees is less than a set value.

Images