A GIS maintenance robot arm path planning method and system
By obtaining the starting and target nodes in the maintenance robot path planning, randomly generating and expanding intermediate nodes, combining obstacles and distance judgments, the low efficiency and optimal path problems of robotic arm path planning in complex environments are solved, and the path is improved The accuracy and efficiency of the planning reduces redundant points and inflection points, and enhances the stability and accuracy of the robotic arm.
Patent Information
- Application Number
- CN202411062174.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-05
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2044-08-05
AI Technical Summary
In the prior art, the problem of low efficiency and inability to obtain the optimal path in complex environments, especially the artificial potential field method and the fast-scaling random tree (RRT) algorithm are insufficient in flexibility and efficiency.
By obtaining the starting node and target node of the maintenance robot, randomly generate sampling nodes, and expanding the intermediate nodes within the preset step range, determining whether there are obstacles and distances between nodes exceeding the preset distance, using greedy ideas and presets Range judgment is used to improve the accuracy and efficiency of path planning.
It improves the accuracy and efficiency of robotic arm path planning, reduces path complexity and planning time, avoids redundant points and inflection points in traditional algorithms, and enhances the stability and accuracy of robotic arm.
Smart Images

Figure CN118578404B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of path planning technology, and in particular to a path planning method and system for a mechanical arm of a GIS maintenance robot. Background Art
[0002] With the development of automatic navigation technology, modern mechanical maintenance based on maintenance robots has gradually become popular. Compared with traditional manual operations, maintenance robots are more efficient in production operations, which is of great significance to the industrial economy and the improvement of people's living standards. The high precision and high flexibility of the maintenance robot arm make it an ideal maintenance tool, which can effectively improve the maintenance efficiency. However, the path planning problem faced by the robot arm during the maintenance process has become an important challenge that limits its application.
[0003] At present, the artificial potential field method in the existing technology causes the robot arm to select paths that are not flexible or efficient enough; the rapidly-exploring random tree (RRT) algorithm is the most commonly used algorithm in robot arm path planning, but the algorithm will have reduced efficiency in complex environments and cannot obtain the optimal path. Summary of the invention
[0004] The embodiments of the present application provide a method and system for path planning of a manipulator arm of a GIS maintenance robot, which are used to improve the accuracy and efficiency of the manipulator arm path planning.
[0005] An embodiment of the present invention provides a path planning method for a mechanical arm of a GIS maintenance robot. The method is applied to the maintenance robot. The maintenance robot includes: a mechanical arm composed of a plurality of mechanical joints and connecting rods, and a processor. The processor is used to execute:
[0006] Step 1, obtaining a starting node and a target node of the maintenance robot; determining the starting node as a first root node, and determining the target node as a second root node;
[0007] Step 2, randomly generating a first sampling node corresponding to the start node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node;
[0008] Step 3, expanding the first root node along the preset step length toward the first sampling node to obtain a first intermediate node; expanding the second root node along the preset step length toward the second sampling node to obtain a second intermediate node;
[0009] Step 4, determining whether there is an obstacle between the first intermediate node and the first root node, and whether there is an obstacle between the second intermediate node and the second root node;
[0010] Step 5: If no obstacles exist, the first intermediate node and the second intermediate node are stored in the search path, and it is determined whether the distance between the first intermediate node and the second intermediate node is greater than a preset distance;
[0011] Step 6: If the distance between the first intermediate node and the second intermediate node is greater than a preset distance, the first intermediate node and the second intermediate node are respectively determined as the first root node and the second root node, and the process jumps to step 2 to continue.
[0012] Step 7: If the distance between the first intermediate node and the second intermediate node is less than or equal to the preset distance and there is no obstacle between the first intermediate node and the second intermediate node, a planned path of the robot arm is generated according to the first intermediate node and the second intermediate node in the search path.
[0013] In an optional embodiment provided by the present invention, before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes:
[0014] Determine whether the first intermediate node is within a first preset range of the second root node, and whether the second intermediate node is within a second preset range of the first root node;
[0015] If the first intermediate node is within a first preset range of the second root node, determining the first intermediate node as the first root node;
[0016] If the second intermediate node is within a second preset range of the first root node, the second intermediate node is determined as the second root node.
[0017] In an optional embodiment provided by the present invention, before determining whether the first intermediate node is within a first preset range of the second root node and whether the second intermediate node is within a second preset range of the first root node, the method further includes:
[0018] Determine a first distance according to the position of the first intermediate node stored in the search path, and determine a second distance according to the position of the second intermediate node stored in the search path;
[0019] The first preset range is determined by taking the second root node as the center and the first distance as the radius; the second preset range is determined by taking the first root node as the center and the second distance as the radius.
[0020] In an optional embodiment provided by the present invention, determining the first distance according to the position of the first intermediate node stored in the search path, and determining the second distance according to the position of the second intermediate node stored in the search path, includes:
[0021] Calculate the distance from the second root node to the first intermediate node, and the distance from the first root node to the second intermediate node;
[0022] The first distance is determined according to the distance from the second root node to the first intermediate node; and the second distance is determined according to the distance from the first root node to the second intermediate node.
[0023] In an optional embodiment provided by the present invention, determining the first distance according to the distance from the second root node to the first intermediate node; determining the second distance according to the distance from the first root node to the second intermediate node includes:
[0024] By formula determining the first distance;
[0025] By formula determining the second distance;
[0026] in, is the first distance, is the second distance, is the preset step size, is the distance from the second root node to the first intermediate node, is the distance from the first root node to the second intermediate node.
[0027] In an optional embodiment provided by the present invention, before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes:
[0028] Calculate a first distance value between a previous node of the first intermediate node and the second root node, and a second distance value between the first intermediate node and the second root node;
[0029] Calculate a third distance value between a previous node of the second intermediate node and the first root node, and a fourth distance value between the second intermediate node and the first root node;
[0030] If the second distance value is less than the first distance value, determining the first intermediate node as the first root node;
[0031] If the fourth distance value is smaller than the third distance value, the second intermediate node is determined as the second root node.
[0032] In an optional embodiment provided by the present invention, before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes:
[0033] Determine whether the distance between the first root node and the target node is less than or equal to a preset distance, and whether the distance between the second root node and the start node is less than or equal to a preset distance;
[0034] If the distance between the first root node and the target node is less than or equal to a preset distance and there is no obstacle between the first root node and the target node, generating a planned path for the robot arm according to the first intermediate node in the search path;
[0035] If the distance between the second root node and the starting node is less than or equal to the preset distance and there is no obstacle between the second root node and the starting node, a planned path of the robot arm is generated according to the second intermediate node in the search path.
[0036] In an optional embodiment provided by the present invention, the method further includes:
[0037] Determine whether there is an obstacle between the starting node and the nth node in the planned path, where the initial value of n is 2;
[0038] If there is no obstacle between the starting node and the nth node, the n-1th node in the planned path is deleted, n=n+1, and the process jumps to determining whether there is an obstacle between the starting node and the nth node in the planned path and continues to execute;
[0039] If there is an obstacle between the starting node and the nth node, the n-1th node is used as the starting node, and the process jumps to determine whether there is an obstacle between the starting node and the nth node in the planned path and continues to execute.
[0040] In an optional embodiment provided by the present invention, the method further includes:
[0041] If multiple planned paths are obtained, the final planned path of the robot arm is determined by comparing the number of nodes and path lengths in the multiple planned paths.
[0042] An embodiment of the present invention provides a GIS maintenance robot mechanical arm path planning system, the system comprising:
[0043] An acquisition module, used to acquire a starting node and a target node of the maintenance robot; determine the starting node as a first root node, and determine the target node as a second root node;
[0044] A generating module, configured to randomly generate a first sampling node corresponding to the starting node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node;
[0045] An expansion module, configured to expand the first root node along a preset step length toward the first sampling node to obtain a first intermediate node; and expand the second root node along a preset step length toward the second sampling node to obtain a second intermediate node;
[0046] a determination module, configured to determine whether there is an obstacle between the first intermediate node and the first root node, and whether there is an obstacle between the second intermediate node and the second root node;
[0047] A storage module, configured to store the first intermediate node and the second intermediate node in a search path if no obstacles exist, and determine whether a distance between the first intermediate node and the second intermediate node is greater than a preset distance;
[0048] The determination module is further configured to, if the distance between the first intermediate node and the second intermediate node is greater than a preset distance, determine the first intermediate node and the second intermediate node as the first root node and the second root node, respectively, and jump to randomly generating a first sampling node corresponding to the start node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node to continue execution;
[0049] The generation module is also used to generate a planned path for the robot arm according to the first intermediate node and the second intermediate node in the search path if the distance between the first intermediate node and the second intermediate node is less than or equal to a preset distance and there is no obstacle between the first intermediate node and the second intermediate node.
[0050] The present invention provides a method and system for path planning of a mechanical arm of a GIS maintenance robot. The method is applied to the maintenance robot. The maintenance robot comprises: a mechanical arm composed of a plurality of mechanical joints and connecting rods, and a processor. The processor is used to execute: step 1, obtaining a starting node and a target node of the maintenance robot; determining the starting node as a first root node, and determining the target node as a second root node; step 2, randomly generating a first sampling node corresponding to the starting node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node; step 3, expanding the first root node along a preset step length toward the first sampling node direction to obtain a first intermediate node; expanding the second root node along a preset step length toward the second sampling node direction to obtain a second intermediate node; step 4, determining the first intermediate node Whether there is an obstacle between the point and the first root node and whether there is an obstacle between the second intermediate node and the second root node; step 5, if there is no obstacle, the first intermediate node and the second intermediate node are stored in the search path, and it is determined whether the distance between the first intermediate node and the second intermediate node is greater than the preset distance; step 6, if the distance between the first intermediate node and the second intermediate node is greater than the preset distance, the first intermediate node and the second intermediate node are respectively determined as the first root node and the second root node, and jump to step 2 to continue execution; step 7, if the distance between the first intermediate node and the second intermediate node is less than or equal to the preset distance and there is no obstacle between the first intermediate node and the second intermediate node, the planning path of the robot arm is generated according to the first intermediate node and the second intermediate node in the search path. Compared with the inflexible or inefficient path selected by the robot arm in the prior art, the present invention can determine the planning path based on the start node and the target node jointly triggered, so that the accuracy and efficiency of the robot arm path planning can be improved through the present invention. BRIEF DESCRIPTION OF THE DRAWINGS
[0051] Figure 1 A flow chart of a path planning method for a GIS maintenance robot mechanical arm provided in this application;
[0052] Figure 2 A first distance calculation example diagram provided in this application;
[0053] Figure 3 An example diagram in which an intermediate node is determined as a root node provided in the present application;
[0054] Figure 4 An example diagram of a planning path of a generated robotic arm provided in this application;
[0055] Figure 5 A structural schematic diagram of a GIS maintenance robot manipulator arm path planning system provided in this application. DETAILED DESCRIPTION
[0056] In order to better understand the above-mentioned technical scheme, the technical scheme of the embodiments of the present application is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present application and the specific features in the embodiments are detailed descriptions of the technical scheme of the embodiments of the present application, rather than limitations on the technical scheme of the present application. In the absence of conflict, the embodiments of the present application and the technical features in the embodiments may be combined with each other.
[0057] See also Figure 1 , a GIS maintenance robot mechanical arm path planning method provided by an embodiment of the present invention, the method is applied to the maintenance robot, the maintenance robot comprises: a mechanical arm composed of a plurality of mechanical joints and connecting rods, a processor, the processor is used to execute the following method:
[0058] Step 1, obtaining the starting node and the target node of the maintenance robot; determining the starting node as the first root node, and determining the target node as the second root node.
[0059] In this embodiment, after obtaining the starting node and the target node of the maintenance robot, random sampling is required in the search space, and new nodes are continuously generated by expanding and connecting nodes, and finally a path from the starting node to the target node is found.
[0060] Step 2: randomly generate a first sampling node corresponding to the start node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node.
[0061] Specifically, this embodiment randomly samples the environment to obtain a first sampling node and a second sampling node, wherein the first sampling node is near the starting node and the second sampling node is near the target node. Therefore, this embodiment realizes the generation of sampling nodes in two relative directions of the starting node and the target node, thereby improving the efficiency of the robot arm path planning.
[0062] Step 3: Expand the first root node along the preset step length toward the first sampling node to obtain a first intermediate node; expand the second root node along the preset step length toward the second sampling node to obtain a second intermediate node.
[0063] Step 4: Determine whether there is an obstacle between the first intermediate node and the first root node, and whether there is an obstacle between the second intermediate node and the second root node.
[0064] Step 5: If no obstacles exist, the first intermediate node and the second intermediate node are stored in the search path, and it is determined whether the distance between the first intermediate node and the second intermediate node is greater than a preset distance.
[0065] Randomly sample the environment to obtain the first sampling node rand1x and the second sampling node rand2x, and find the node near1x closest to rand1x in the constructed tree; then, make near1x grow a preset step in the direction of rand1x, and generate a new node (i.e., the first intermediate node) new1x at the end of the growth. If the path from the first intermediate node near1x to new1x does not pass through an obstacle, the first intermediate node new1x is added to the tree. Similarly, the second sampling node rand2x is processed in the same way as the first sampling node rand1x.
[0066] In this embodiment, when the distance between the first intermediate node and the second intermediate node is close, due to the restriction of the fixed step length, even if there is no obstacle between the first intermediate node and the second intermediate node, the search tree cannot directly connect the first intermediate node and the second intermediate node to reach the target node, and will continue to perform invalid search, making the path complex and increasing the planning time. For these sampling points, this embodiment can use the greedy idea. When the distance between the first intermediate node and the second intermediate node generated by the search tree is within the set threshold, the node stops random sampling, and is not restricted by the fixed step length. The first intermediate node and the second intermediate node are directly connected, and the path between the two points is placed in the final planned path.
[0067] Step 6: If the distance between the first intermediate node and the second intermediate node is greater than a preset distance, the first intermediate node and the second intermediate node are respectively determined as the first root node and the second root node, and the process jumps to step 2 to continue.
[0068] In an optional embodiment provided by the present application, before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes:
[0069] Step 201: Determine whether the first intermediate node is within a first preset range of the second root node, and whether the second intermediate node is within a second preset range of the first root node.
[0070] In this embodiment, the first preset range may be a circular area determined with the second root node as the center, and the second preset range may be a circular area determined with the first root node as the center.
[0071] Specifically, before determining whether the first intermediate node is within the first preset range of the second root node and whether the second intermediate node is within the second preset range of the first root node, the method also includes: determining a first distance based on the position of the first intermediate node stored in the search path, and determining a second distance based on the position of the second intermediate node stored in the search path; determining the first preset range with the second root node as the center and the first distance as the radius; determining the second preset range with the first root node as the center and the second distance as the radius.
[0072] Among them, determining the first distance according to the position of the first intermediate node stored in the search path, and determining the second distance according to the position of the second intermediate node stored in the search path include: calculating the distance from the second root node to the first intermediate node, and the distance from the first root node to the second intermediate node; determining the first distance according to the distance from the second root node to the first intermediate node; determining the second distance according to the distance from the first root node to the second intermediate node.
[0073] In this embodiment, the formula Determine the first distance; by the formula Determine the second distance. is the first distance, is the second distance, is the preset step size, is the distance from the second root node to the first intermediate node, is the distance from the first root node to the second intermediate node.
[0074] like Figure 2 The figure shows an example of the first distance calculation provided by this embodiment, that is, after determining the first intermediate node, according to the formula Determine the first distance , then take the target node as the center of the circle first distance Draw a circular area with a radius. If the first intermediate node is within the circular area, store the first intermediate node in the search path. If the first intermediate node is not within the circular area, jump to step 2 to regenerate the first sampling node to obtain a new first intermediate node until the first intermediate node is within the operation area. The calculation method of the second distance is the same as the calculation method of the first distance, and this embodiment will not be repeated here.
[0075] Compared with the traditional RRT algorithm, which performs sampling in the whole world, the distribution of sampling points is too dispersed, thus prolonging the time of path planning. In order to make the sampling points converge to the vicinity of the target node faster, this embodiment improves the algorithm so that the sampling points are sampled in the target area, that is, the first intermediate node falls within the first distance from the target node as the center. Draw a circular area with a radius to quickly complete path planning.
[0076] Step 202: If the first intermediate node is within a first preset range of the second root node, determine the first intermediate node as the first root node.
[0077] Step 203: If the second intermediate node is within a second preset range of the first root node, determine the second intermediate node as the second root node.
[0078] In another optional embodiment provided by the present application, before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes:
[0079] Step 301: Calculate a first distance value between a previous node of the first intermediate node and the second root node, and a second distance value between the first intermediate node and the second root node.
[0080] Step 302: Calculate a third distance value between a previous node of the second intermediate node and the first root node, and a fourth distance value between the second intermediate node and the first root node.
[0081] Step 303: If the second distance value is smaller than the first distance value, the first intermediate node is determined as the first root node.
[0082] Step 304: If the fourth distance value is less than the third distance value, the second intermediate node is determined as the second root node.
[0083] Figure 3 The diagram shows an example of determining an intermediate node as a root node provided by the present embodiment. After the first intermediate node and the second intermediate node are determined in the first round, the determined first intermediate node and the second intermediate node are stored in the search path, and then the first root node is updated to the determined first intermediate node, and the second root node is updated to the determined second intermediate node, so as to re-determine the first intermediate node and the second intermediate node according to the updated first root node and the second root node. Figure 2 In the example, after re-determining the first intermediate node and the second intermediate node according to the updated first root node and the second root node, it is necessary to calculate the previous node of the first intermediate node ( Figure 3 The first distance value between the first root node in the first intermediate node and the second root node, the second distance value between the first intermediate node and the second root node, from Figure 3It can be seen that the second distance value is greater than the first distance value, indicating that the regenerated first intermediate node is further away from the second root node than the previous node. At this time, it is necessary to jump to step 2 to regenerate the first sampling node in order to obtain a new first intermediate node so that the second distance value is less than the first distance value, thereby realizing rapid path planning. The calculation method of the second distance in this embodiment is the same as the calculation method of the first distance, and this embodiment will not be repeated here.
[0084] The present application provides an optional embodiment, before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method also includes: determining whether the distance between the first root node and the target node is less than or equal to a preset distance, and whether the distance between the second root node and the starting node is less than or equal to a preset distance; if the distance between the first root node and the target node is less than or equal to the preset distance and there are no obstacles between the first root node and the target node, generating a planned path for the robot arm according to the first intermediate node in the search path; if the distance between the second root node and the starting node is less than or equal to the preset distance and there are no obstacles between the second root node and the starting node, generating a planned path for the robot arm according to the second intermediate node in the search path.
[0085] Step 7: If the distance between the first intermediate node and the second intermediate node is less than or equal to the preset distance and there is no obstacle between the first intermediate node and the second intermediate node, a planned path of the robot arm is generated according to the first intermediate node and the second intermediate node in the search path.
[0086] like Figure 4 As shown, after multiple rounds of determining the first intermediate node and the second intermediate node, if the distance between the first intermediate node and the second intermediate node ( Figure 3 If the distance between the first intermediate node and the second intermediate node is smaller than or equal to the preset distance and there is no obstacle between the first intermediate node and the second intermediate node, the planned path of the robot arm is directly generated according to the first intermediate node and the second intermediate node in the search path, that is, the planned path of the robot arm is generated according to nodes ①-node ⑤, wherein nodes ① and ② are the first intermediate nodes, that is, the nodes generated from the starting node; nodes ③, ④ and ⑤ are the second intermediate nodes, that is, the nodes generated from the target node.
[0087] It should be noted that, since the path planned by the RRT algorithm is connected by several nodes, there will be many redundant points and inflection points in the generated path. The redundant points will increase the distance of the path planning, and the time consumed by the robot arm will also increase. The appearance of inflection points may cause the robot arm to shake during the path planning movement, thereby reducing the stability and accuracy of the robot arm. In order to overcome this problem, after generating the planned path of the robot arm, this embodiment may also include: determining whether there is an obstacle between the starting node and the nth node in the planned path, and the initial value of n is 2; if there is no obstacle between the starting node and the nth node, then deleting the n-1th node in the planned path, n=n+1, and jumping to determine whether there is an obstacle between the starting node and the nth node in the planned path to continue execution; if there is an obstacle between the starting node and the nth node, then taking the n-1th node as the starting node, and jumping to determine whether there is an obstacle between the starting node and the nth node in the planned path to continue execution.
[0088] In this embodiment, if multiple planned paths are obtained, the final planned path of the robot arm is determined by comparing the number of nodes and the path lengths in the multiple planned paths.
[0089] Furthermore, in the process of planning the robot's obstacle avoidance path, it is easy to have a sudden acceleration at the turning point of the path, which reduces the stability of the planning process, so the path needs to be smoothed. For the improved RRT algorithm, several turning points in the path are selected, and the path between the turning points is smoothed using the B-spline function. Considering that the higher the B-spline degree, the smoother the path after smoothing, in order to ensure the final planned path effect, this embodiment can be smoothed and optimized by the quintic B-spline function.
[0090] According to the inflection point coordinates that need to be smoothly optimized, a parameterized B-spline curve is constructed, using a value range of The parameter t represents a point on the curve; the parameter t is mapped to the actual range of the independent variable, the range of the independent variable is divided into several intervals according to the number of inflection points, and quintic B-spline interpolation is used for each interval; the interpolation function of each interval can be obtained based on the interpolation result.
[0091] A GIS maintenance robot mechanical arm path planning method provided in an embodiment of the present application is applied to the maintenance robot, and the maintenance robot includes: a mechanical arm composed of multiple mechanical joints and connecting rods, and a processor, wherein the processor is used to execute: step 1, obtaining a starting node and a target node of the maintenance robot; determining the starting node as a first root node, and determining the target node as a second root node; step 2, randomly generating a first sampling node corresponding to the starting node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node; step 3, expanding the first root node along a preset step length toward the first sampling node direction to obtain a first intermediate node; expanding the second root node along a preset step length toward the second sampling node direction to obtain a second intermediate node; step 4, determining the first intermediate node Whether there is an obstacle between the node and the first root node and whether there is an obstacle between the second intermediate node and the second root node; Step 5, if there is no obstacle, the first intermediate node and the second intermediate node are stored in the search path, and it is determined whether the distance between the first intermediate node and the second intermediate node is greater than the preset distance; Step 6, if the distance between the first intermediate node and the second intermediate node is greater than the preset distance, the first intermediate node and the second intermediate node are respectively determined as the first root node and the second root node, and jump to step 2 to continue execution; Step 7, if the distance between the first intermediate node and the second intermediate node is less than or equal to the preset distance and there is no obstacle between the first intermediate node and the second intermediate node, the planning path of the robot arm is generated according to the first intermediate node and the second intermediate node in the search path. Compared with the inflexible or inefficient path selected by the robot arm in the prior art, the present invention can determine the planning path based on the start node and the target node jointly triggered, so that the accuracy and efficiency of the robot arm path planning can be improved through the present invention.
[0092] It should be understood that the order of execution of the steps in the above embodiment does not necessarily mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention.
[0093] In one embodiment, a GIS maintenance robot mechanical arm path planning system is provided, and the GIS maintenance robot mechanical arm path planning system corresponds one-to-one to the GIS maintenance robot mechanical arm path planning method in the above embodiment. Figure 5 As shown in the figure, the functional modules of the GIS maintenance robot arm path planning system are described in detail as follows:
[0094] The acquisition module 51 is used to acquire the starting node and the target node of the maintenance robot; determine the starting node as the first root node, and determine the target node as the second root node;
[0095] A generating module 52, configured to randomly generate a first sampling node corresponding to the starting node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node;
[0096] An expansion module 53 is used to expand the first root node along a preset step length toward the first sampling node to obtain a first intermediate node; and to expand the second root node along a preset step length toward the second sampling node to obtain a second intermediate node;
[0097] A determination module 54, configured to determine whether there is an obstacle between the first intermediate node and the first root node, and whether there is an obstacle between the second intermediate node and the second root node;
[0098] A storage module 55, configured to store the first intermediate node and the second intermediate node in a search path if no obstacles exist, and determine whether the distance between the first intermediate node and the second intermediate node is greater than a preset distance;
[0099] The determining module 54 is further configured to, if the distance between the first intermediate node and the second intermediate node is greater than a preset distance, determine the first intermediate node and the second intermediate node as the first root node and the second root node, respectively, and jump to randomly generating a first sampling node corresponding to the start node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node to continue execution;
[0100] The generation module 52 is also used to generate a planned path for the robot arm according to the first intermediate node and the second intermediate node in the search path if the distance between the first intermediate node and the second intermediate node is less than or equal to a preset distance and there is no obstacle between the first intermediate node and the second intermediate node.
[0101] In an optional embodiment provided by the present invention, the determination module 54 is further configured to:
[0102] Determine whether the first intermediate node is within a first preset range of the second root node, and whether the second intermediate node is within a second preset range of the first root node;
[0103] If the first intermediate node is within a first preset range of the second root node, determining the first intermediate node as the first root node;
[0104] If the second intermediate node is within a second preset range of the first root node, the second intermediate node is determined as the second root node.
[0105] In an optional embodiment provided by the present invention, the determination module 54 is further configured to:
[0106] Determine a first distance according to the position of the first intermediate node stored in the search path, and determine a second distance according to the position of the second intermediate node stored in the search path;
[0107] The first preset range is determined by taking the second root node as the center and the first distance as the radius; the second preset range is determined by taking the first root node as the center and the second distance as the radius.
[0108] In an optional embodiment provided by the present invention, the determination module 54 is specifically configured to:
[0109] Calculate the distance from the second root node to the first intermediate node, and the distance from the first root node to the second intermediate node;
[0110] The first distance is determined according to the distance from the second root node to the first intermediate node; and the second distance is determined according to the distance from the first root node to the second intermediate node.
[0111] In an optional embodiment provided by the present invention, the determination module 54 is specifically configured to:
[0112] By formula determining the first distance;
[0113] By formula determining the second distance;
[0114] in, is the first distance, is the second distance, is the preset step size, is the distance from the second root node to the first intermediate node, is the distance from the first root node to the second intermediate node.
[0115] In an optional embodiment provided by the present invention, the determination module 54 is further configured to:
[0116] Calculate a first distance value between a previous node of the first intermediate node and the second root node, and a second distance value between the first intermediate node and the second root node;
[0117] Calculate a third distance value between a previous node of the second intermediate node and the first root node, and a fourth distance value between the second intermediate node and the first root node;
[0118] If the second distance value is less than the first distance value, determining the first intermediate node as the first root node;
[0119] If the fourth distance value is smaller than the third distance value, the second intermediate node is determined as the second root node.
[0120] In an optional embodiment provided by the present invention, the determination module 54 is further configured to:
[0121] Determine whether the distance between the first root node and the target node is less than or equal to a preset distance, and whether the distance between the second root node and the start node is less than or equal to a preset distance;
[0122] If the distance between the first root node and the target node is less than or equal to a preset distance and there is no obstacle between the first root node and the target node, generating a planned path for the robot arm according to the first intermediate node in the search path;
[0123] If the distance between the second root node and the starting node is less than or equal to a preset distance and there is no obstacle between the second root node and the starting node, a planned path of the robot arm is generated according to the second intermediate node in the search path.
[0124] In an optional embodiment provided by the present invention, the determination module 54 is further configured to:
[0125] Determine whether there is an obstacle between the starting node and the nth node in the planned path, where the initial value of n is 2;
[0126] If there is no obstacle between the starting node and the nth node, the n-1th node in the planned path is deleted, n=n+1, and the process jumps to determining whether there is an obstacle between the starting node and the nth node in the planned path and continues to execute;
[0127] If there is an obstacle between the starting node and the nth node, the n-1th node is used as the starting node, and the process jumps to determine whether there is an obstacle between the starting node and the nth node in the planned path and continues to execute.
[0128] In an optional embodiment provided by the present invention, the determination module 54 is further configured to:
[0129] If multiple planned paths are obtained, the final planned path of the robot arm is determined by comparing the number of nodes and path lengths in the multiple planned paths.
[0130] For the specific definition of the GIS maintenance robot mechanical arm path planning system, please refer to the definition of the GIS maintenance robot mechanical arm path planning method mentioned above, which will not be repeated here. Each module in the above-mentioned device can be implemented in whole or in part by software, hardware and a combination thereof. Each of the above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.
[0131] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.
[0132] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention.
Claims
1. A path planning method for a GIS maintenance robot arm. It is characterized in that The method is applied to a maintenance robot, which includes a mechanical arm composed of a plurality of mechanical joints and connecting rods, and a processor, wherein the processor is used to execute: Step 1, obtaining a starting node and a target node of the maintenance robot; determining the starting node as a first root node, and determining the target node as a second root node; Step 2, randomly generating a first sampling node corresponding to the start node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node; Step 3, expanding the first root node along the preset step length toward the first sampling node to obtain a first intermediate node; expanding the second root node along the preset step length toward the second sampling node to obtain a second intermediate node; Step 4, determining whether there is an obstacle between the first intermediate node and the first root node, and whether there is an obstacle between the second intermediate node and the second root node; Step 5: If no obstacles exist, the first intermediate node and the second intermediate node are stored in the search path, and it is determined whether the distance between the first intermediate node and the second intermediate node is greater than a preset distance; Step 6: If the distance between the first intermediate node and the second intermediate node is greater than a preset distance, the first intermediate node and the second intermediate node are respectively determined as the first root node and the second root node, and the process jumps to step 2 to continue. Step 7: If the distance between the first intermediate node and the second intermediate node is less than or equal to a preset distance and there is no obstacle between the first intermediate node and the second intermediate node, a planned path of the robot arm is generated according to the first intermediate node and the second intermediate node in the search path; Before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes: Determine whether the first intermediate node is within a first preset range of the second root node, and whether the second intermediate node is within a second preset range of the first root node; If the first intermediate node is within a first preset range of the second root node, determining the first intermediate node as the first root node; If the second intermediate node is within a second preset range of the first root node, determining the second intermediate node as the second root node; Before determining whether the first intermediate node is within a first preset range of the second root node and whether the second intermediate node is within a second preset range of the first root node, the method further includes: Determine a first distance according to the position of the first intermediate node stored in the search path, and determine a second distance according to the position of the second intermediate node stored in the search path; The first preset range is determined by taking the second root node as the center and the first distance as the radius; the second preset range is determined by taking the first root node as the center and the second distance as the radius; The determining the first distance according to the position of the first intermediate node stored in the search path, and determining the second distance according to the position of the second intermediate node stored in the search path, comprises: Calculate the distance from the second root node to the first intermediate node, and the distance from the first root node to the second intermediate node; Determine the first distance according to the distance from the second root node to the first intermediate node; determine the second distance according to the distance from the first root node to the second intermediate node; The determining the first distance according to the distance from the second root node to the first intermediate node; and determining the second distance according to the distance from the first root node to the second intermediate node include: By formula determining the first distance; By formula determining the second distance; in, is the first distance, is the second distance, is the preset step size, is the distance from the second root node to the first intermediate node, is the distance from the first root node to the second intermediate node; Before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes: Calculate a first distance value between a previous node of the first intermediate node and the second root node, and a second distance value between the first intermediate node and the second root node; Calculate a third distance value between a previous node of the second intermediate node and the first root node, and a fourth distance value between the second intermediate node and the first root node; If the second distance value is less than the first distance value, determining the first intermediate node as the first root node; If the fourth distance value is less than the third distance value, determining the second intermediate node as the second root node; Before determining the first intermediate node and the second intermediate node as the first root node and the second root node respectively, the method further includes: Determine whether the distance between the first root node and the target node is less than or equal to a preset distance, and whether the distance between the second root node and the start node is less than or equal to a preset distance; If the distance between the first root node and the target node is less than or equal to a preset distance and there is no obstacle between the first root node and the target node, generating a planned path for the robot arm according to the first intermediate node in the search path; If the distance between the second root node and the starting node is less than or equal to a preset distance and there is no obstacle between the second root node and the starting node, generating a planned path for the robot according to the second intermediate node in the search path; The method further comprises: Determine whether there is an obstacle between the starting node and the nth node in the planned path, where the initial value of n is 2; If there is no obstacle between the starting node and the nth node, the n-1th node in the planned path is deleted, n=n+1, and the process jumps to determining whether there is an obstacle between the starting node and the nth node in the planned path and continues to execute; If there is an obstacle between the starting node and the nth node, the n-1th node is used as the starting node, and the process jumps to determine whether there is an obstacle between the starting node and the nth node in the planned path and continues to execute.
2. The method according to claim 1, It is characterized in that The method further comprises: If multiple planned paths are obtained, the final planned path of the robot arm is determined by comparing the number of nodes and path lengths in the multiple planned paths.
3. A GIS maintenance robot arm path planning system, the system implements the method as claimed in claim 1, It is characterized in that The system comprises: An acquisition module, used to acquire a starting node and a target node of the maintenance robot; determine the starting node as a first root node, and determine the target node as a second root node; A generating module, configured to randomly generate a first sampling node corresponding to the starting node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node; An expansion module, configured to expand the first root node along a preset step length toward the first sampling node to obtain a first intermediate node; and expand the second root node along a preset step length toward the second sampling node to obtain a second intermediate node; a determination module, configured to determine whether there is an obstacle between the first intermediate node and the first root node, and whether there is an obstacle between the second intermediate node and the second root node; A storage module, configured to store the first intermediate node and the second intermediate node in a search path if no obstacles exist, and determine whether a distance between the first intermediate node and the second intermediate node is greater than a preset distance; The determination module is further configured to, if the distance between the first intermediate node and the second intermediate node is greater than a preset distance, determine the first intermediate node and the second intermediate node as the first root node and the second root node, respectively, and jump to randomly generating a first sampling node corresponding to the start node and a second sampling node corresponding to the target node within a relative range between the first root node and the second root node to continue execution; The generation module is also used to generate a planned path for the robot arm according to the first intermediate node and the second intermediate node in the search path if the distance between the first intermediate node and the second intermediate node is less than or equal to a preset distance and there is no obstacle between the first intermediate node and the second intermediate node.
