Multi-constraint robot joint space path planning method

Abstract

The invention provides a multi-constraint robot joint space path planning method. The multi-constraint robot joint space path planning method comprises the following steps: randomly sampling in a robot joint configuration space by adopting heuristic node search to generate new nodes, and adding the new nodes into a path tree; dynamically adjusting a neighbor radius threshold according to the number of nodes; the connection cost between the generated new nodes and existing nodes in the path tree is calculated according to the node path cost, the connection with the minimum cost is selected, and the node path cost comprises the distance cost, the collision risk cost and the joint mutation cost; executing path rewriting and reconnecting operations in the path tree; and when the generated nodes reach a target point or meet a specific stop condition, backtracking from the target point to the starting point, and forming an initial path PA according to a connection relationship among the nodes. According to the multi-constraint robot joint space path planning method, the initial path which is free of collision risk, low in energy consumption, small in joint mutation and high in operation speed can be rapidly generated.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a method for planning a multi-constraint robot joint space path. Background Art

[0002] As modern manufacturing continues to pursue automation and intelligence, the combination of industrial vision hardware and computer vision algorithms has given industrial robots more powerful perception and decision-making capabilities. As the eyes and brain of the robot, the vision controller analyzes the image data collected from the camera and performs tasks such as object recognition, position positioning, dimension measurement, and environmental map construction in real time, providing accurate navigation and control basis for the robot in complex industrial environments. This improvement in capability enables robots to undertake higher-level tasks on automated production lines and achieve more efficient and intelligent operation processes.

[0003] However, the industrial production environment is often full of uncertainty, and robots must face obstacles on the path when performing tasks. This places high demands on the robot's obstacle avoidance path planning, which not only requires the path to be able to safely bypass obstacles, but also emphasizes the stability of the path, the simplification of control points, and the optimization of energy consumption to ensure the efficient operation of the robot and the smooth change of posture.

[0004] At present, the obstacle avoidance path planning of industrial robots mainly relies on teaching programming and single path planning algorithms. Although teaching programming is intuitive and easy to use, its flexibility and adaptability are limited, and it is difficult to cope with the changing production environment. Single path planning algorithms, such as A*, ant colony algorithm and RRT algorithm, can plan the path from the starting point to the end point, but the generated path may have frequent mutation points, resulting in failure to meet the working condition requirements of stable posture changes. Summary of the invention

[0005] The main purpose of the present invention is to provide a multi-constraint robot joint space path planning method, which can quickly generate an initial path with no collision risk, low energy consumption, small joint mutation and fast running speed.

[0006] In order to achieve the above object, according to one aspect of the present invention, a multi-constraint robot joint space path planning method is provided, comprising:

[0007] Heuristic node search is used to randomly sample and generate new nodes in the robot joint configuration space and add them to the path tree;

[0008] Dynamically adjust the neighbor radius threshold according to the number of nodes;

[0009] For the connection between the generated new node and the existing nodes in the path tree, the connection cost is calculated based on the node path cost, and the connection with the minimum cost is selected, where the node path cost includes distance cost, collision risk cost and joint mutation cost;

[0010] Perform path rewriting and reconnection operations in the path tree;

[0011] When the generated node reaches the target point or meets a specific stopping condition, it traces back from the target point to the starting point and forms an initial path PA based on the connection relationship between nodes.

[0012] Furthermore, the neighbor radius threshold is determined by the following formula:

[0013]

[0014] where λ r is the proportional coefficient, which is determined according to the time consumption requirement, and N is the number of nodes in the current path tree.

[0015] Furthermore, for a path node a, the joint coordinates are (θ a1 ,θ a2 ,...,θ a6 ), path node b, joint coordinates are (θ b1 ,θ b2 ,...,θ b6 ), the distance cost Cost_d between path nodes a and b is calculated using the following formula:

[0016]

[0017] Furthermore, the minimum distance d between the robot and the scene is calculated.

[0018] The collision risk cost Cost_c is determined using the following formula:

[0019]

[0020] The remainder of the denominator ε>0.

[0021] Further, path node A=(θ A1 ,θ A2 ,...,θ A6 ),B=(θ B1 ,θ B2 ,...,θ B6 ),C=(θ C1 ,θ C2 ,...,θ C6 ), where A is the parent node of B, B is the parent node of C, the movement order of the path at the above three points is A->B->C, and the joint mutation cost Cost_s of node C is determined by the following formula:

[0022]

[0023] Furthermore, the steps of randomly sampling and generating new nodes in the robot joint configuration space by using heuristic node search and adding them to the path tree include:

[0024] Initialize the starting point, end point and empty path tree, and join the path tree from the starting point node;

[0025] Randomly generate a random number rd between 0 and 1, and determine whether rd is greater than the threshold rd_th;

[0026] If rd is less than rd_th, the sampling node q is the end point. If rd is greater than rd_th, a node is randomly selected in the configuration space reachable by the robot as q.

[0027] Find the node p closest to the sampled node q in the tree, and generate a new node q_new from p to q according to the set step size step;

[0028] Check whether the q_new point collides. If not, perform path rewriting and path reconnection operations. If it is not in the configuration space, re-random sampling.

[0029] Furthermore, the steps of performing path rewriting and reconnection operations in the path tree include:

[0030] Path rewriting operation: Find all path nodes q whose distance from q_new is less than the set neighbor radius threshold r, traverse the neighbor nodes, find the node q_min1 with the smallest sum of its original path cost and the path cost from q to q_new, and its path cost is cost_min1. Set q_min1 as the parent node of q_new;

[0031] Path reconnection operation: find all path nodes q whose distance from q_new is less than the set neighbor radius threshold r, traverse the neighboring nodes, and find the node q_min2 whose original path cost is less than the sum of the path cost from q_new to q and cost_min1. Its path cost is cost_min2, set q_new as the parent node of q_min2, and update the path cost of q_min2 to cost_min2.

[0032] Furthermore, when the generated node reaches the target point or meets a specific stop condition, the steps of tracing back from the target point to the starting point and forming an initial path according to the connection relationship between the nodes include:

[0033] When the generated q_new reaches the end point of the path, the path planning ends. nBacktrack and keep looking for its parent node PA i , until the starting point PA 1 , add all nodes to the path set PA,

[0034] PA={PA 1 ,PA 2 ,...,PA n};

[0035] Where n is the number of initial path nodes.

[0036] Furthermore, the multi-constraint robot joint space path planning method also includes:

[0037] Use the Bezier curve to smooth the initial path PA and generate an initial smooth path PB;

[0038] Use the depth-first search algorithm to combine the initial path with the initial smooth path to generate a smooth path PC without collision risk;

[0039] The dynamic cutting algorithm is used to delete redundant points on the path and generate the final path PD.

[0040] Furthermore, the steps of smoothing the initial path PA using a Bezier curve to generate an initial smooth path PB include:

[0041] Use Bezier curves to calculate n control points PA on the initial path PA 1 ,PA 2 ,...,PA n For smoothing, the Bezier curve formula is:

[0042] in is the binomial coefficient,

[0043] Get the initial smooth path PB = {PB 1 ,PB 2 ,...,PB n}, path node PB i It is expressed by the following formula:

[0044]

[0045] Wherein, i is a positive integer from 1 to n, and the starting point and the end point of the initial path PA and the initial smooth path PB are the same.

[0046] Further, the steps of using a depth-first search algorithm to combine the initial path and the initial smooth path to generate a smooth path PC without collision risk include:

[0047] For the initial smooth path, use the depth-first search algorithm to traverse the path;

[0048] During the traversal process, an empty stack path is first created, and the starting point path is added to the empty stack path;

[0049] Detect whether the current initial smooth path node collides with the subsequent initial path node and the path between the initial smooth path nodes;

[0050] If the current initial smooth path node and the subsequent initial path nodes and the path between the initial smooth path nodes all collide with obstacles, then mark the current initial path node as unreachable, and backtrack to the previous node to continue searching for the path;

[0051] If one of the paths between the current initial smooth path node and the subsequent initial path node and the initial smooth path node does not collide with the obstacle, the path formed by the current initial smooth path node and the path node that does not collide is added to the path PC;

[0052] If the current initial smooth path node and the subsequent initial path node and the path between the initial smooth path node do not collide with obstacles, then the current initial smooth path node and the initial smooth path node that does not collide are added to the path PC;

[0053] When a path is found with no collision from the starting point to the end point, the path is used as a smooth path without collision risk, and the path PC generation is completed.

[0054] Furthermore, the steps of using a dynamic clipping algorithm to delete redundant points on the path and generate the final path PD include:

[0055] Create a final set of paths, initially containing only the starting point;

[0056] Traverse each node in the smooth path PC;

[0057] For the currently traversed node, perform collision detection with the top node in the final path set;

[0058] If the detection finds that the path between the current node and the top node of the stack collides with an obstacle, the current node is added to the final path set;

[0059] If the path between the current node and the top node of the stack does not collide with any obstacles, continue traversing the next node until the entire smooth path is traversed;

[0060] A final path PD is generated according to the formed final path set.

[0061] Applying the technical solution of the present invention, a multi-constraint robot joint space path planning method includes: using heuristic node search to randomly sample and generate new nodes in the robot joint configuration space, and adding them to the path tree; dynamically adjusting the neighbor radius threshold according to the number of nodes; calculating the connection cost between the generated new node and the existing nodes in the path tree according to the node path cost, and selecting the connection with the minimum cost, wherein the node path cost includes the distance cost, the collision risk cost and the joint mutation cost; performing path rewriting and reconnection operations in the path tree; when the generated node reaches the target point or meets a specific stopping condition, backtracking from the target point to the starting point, and forming an initial path PA according to the connection relationship between the nodes. Through the above-mentioned multi-constraint robot joint space path planning method, the heuristic node search technology is adopted to more efficiently generate new nodes in the robot joint configuration space, avoid blind sampling, reduce the time of invalid search, and thus improve the overall efficiency of path planning. By dynamically adjusting the neighbor radius threshold, the algorithm can adaptively optimize the search range. When the node tree size is small, a larger search radius is used, which helps to quickly expand the search tree; when the node tree size increases, the search radius gradually decreases, which helps to locally optimize the path, reduce path mutations, and improve the stability of the path. The calculation of the node path cost includes the collision risk cost, and it will actively avoid obstacles when generating the path to ensure the path. The safety of the path is ensured, and the robot is prevented from colliding with obstacles in the environment. The introduction of joint mutation costs can reduce the acceleration and deceleration of the robot joints during movement, thereby reducing the energy consumption of the motor, improving the robot's motion performance and extending the life of the equipment. Path rewriting and reconnection operations can continuously optimize the path by connecting the nodes with the lowest cost, reducing the number of path control points, simplifying the path, increasing the robot's running speed, and reducing energy consumption. When the generated node reaches the target point or meets a specific stop condition, the algorithm can trace back from the target point to the starting point and quickly form an initial path based on the connection relationship between the nodes. This process is adaptive and can generate the most suitable path according to specific scenarios and needs. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] The drawings constituting a part of the present application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0063] Figure 1 The overall framework diagram of the multi-constraint robot joint space path planning method of the embodiment of the present invention is shown;

[0064] Figure 2a and Figure 2b A schematic diagram of joint mutation costs of a multi-constraint robot joint space path planning method according to an embodiment of the present invention is shown;

[0065] Figure 3 A QMC-RRT* algorithm flow chart of a multi-constraint robot joint space path planning method according to an embodiment of the present invention is shown;

[0066] Figure 4 A schematic diagram showing the path smoothing principle based on a depth-first search algorithm of a multi-constraint robot joint space path planning method according to an embodiment of the present invention is shown;

[0067] Figure 5 A flow chart showing a path smoothing algorithm implementation based on a depth-first search algorithm of a multi-constraint robot joint space path planning method according to an embodiment of the present invention is shown;

[0068] Figure 6 A schematic diagram showing the principle of dynamic path cutting of a multi-constraint robot joint space path planning method according to an embodiment of the present invention; and

[0069] Figure 7 A flow chart showing an implementation of a path dynamic cutting algorithm for a multi-constraint robot joint space path planning method according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0070] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0071] See also Figures 1 to 7 As shown, according to an embodiment of the present invention, a multi-constraint robot joint space path planning method includes:

[0072] Heuristic node search is used to randomly sample and generate new nodes in the robot joint configuration space and add them to the path tree;

[0073] Dynamically adjust the neighbor radius threshold according to the number of nodes;

[0074] For the connection between the generated new node and the existing nodes in the path tree, the connection cost is calculated based on the node path cost, and the connection with the minimum cost is selected, where the node path cost includes distance cost, collision risk cost and joint mutation cost;

[0075] Perform path rewriting and reconnection operations in the path tree;

[0076] When the generated node reaches the target point or meets a specific stopping condition, it traces back from the target point to the starting point and forms an initial path PA based on the connection relationship between nodes.

[0077] Through the above-mentioned multi-constraint robot joint space path planning method, the heuristic node search technology is adopted to more efficiently generate new nodes in the robot joint configuration space, avoid blind sampling, reduce the time of invalid search, and thus improve the overall efficiency of path planning. By dynamically adjusting the neighbor radius threshold, the algorithm can adaptively optimize the search range. When the node tree size is small, a larger search radius is used, which helps to quickly expand the search tree; when the node tree size increases, the search radius gradually decreases, which helps to locally optimize the path, reduce path mutations, and improve the stability of the path. The calculation of the node path cost includes the collision risk cost, and it will actively avoid obstacles when generating the path to ensure the path. The safety of the path is ensured, and the robot is prevented from colliding with obstacles in the environment. The introduction of joint mutation costs can reduce the acceleration and deceleration of the robot joints during movement, thereby reducing the energy consumption of the motor, improving the robot's motion performance and extending the life of the equipment. Path rewriting and reconnection operations can continuously optimize the path by connecting the nodes with the lowest cost, reducing the number of path control points, simplifying the path, increasing the robot's running speed, and reducing energy consumption. When the generated node reaches the target point or meets a specific stop condition, the algorithm can trace back from the target point to the starting point and quickly form an initial path based on the connection relationship between the nodes. This process is adaptive and can generate the most suitable path according to specific scenarios and needs.

[0078] Traditional robot path planning based on RRT* algorithm performs path planning in Cartesian coordinate system, without considering robot joint rotation. The robot terminal posture stability is poor, and large-scale rotation and joint rotation are prone to occur. The collision risk is not considered, and a path away from obstacles cannot be planned. The traditional RRT* algorithm path reconnection and rewriting operations are time-consuming, and the threshold of the neighbor radius cannot be adaptively adjusted, and it has the defect of low efficiency caused by sampling blindness. The present invention proposes a multi-constraint robot joint space path planning method based on QMC-RRT* (Quick Multi-constraint Rapidly-exploring Random Tree Star) algorithm, which performs distance constraints, joint mutation constraints and collision risk constraints on the robot. By comprehensively considering multiple factors such as distance, collision risk, joint mutation, etc., it not only improves the efficiency and stability of path planning, ensures the safety of the path, but also effectively reduces energy consumption and improves the robot's motion performance. The final generated initial path has fewer control points, which is more conducive to the actual operation of the robot.

[0079] The QMC-RRT* (Quick Multi-constraint Rapidly-exploring Random Tree Star) algorithm is an incremental random sampling method for path planning that aims to find the optimal path from the start point to the end point. It is an improved version of the RRT* (Rapidly-exploring Random Tree Star) algorithm that gradually optimizes the path by reconnecting the nodes in the tree, thereby converging to the global optimal solution.

[0080] In one embodiment, the neighbor radius threshold is determined by the following formula:

[0081]

[0082] where λ r is the proportional coefficient, which is determined according to the time consumption requirement, and N is the number of nodes in the current path tree.

[0083] The inverse formula of the neighbor radius threshold is as follows:

[0084]

[0085] λ r is the proportional coefficient, which can be adjusted manually according to the time requirement, λ r The larger the value, the more time it takes, and the path is relatively stable. r The smaller it is, the shorter the time consumption is and the path is unstable. N is the number of nodes in the current path tree. When the number of nodes N>3, the derivative function is less than 0, and the search radius decreases as the number of nodes increases.

[0086] In the QMC-RRT* algorithm, dynamically adjusting the neighbor radius can effectively balance the exploration and utilization characteristics of the algorithm. In the early stage of the path tree establishment, when the number of nodes is small, the algorithm tends to use a larger neighbor radius, which helps to explore the robot joint configuration space faster, accelerate the expansion of the path tree, and increase the speed of the algorithm to find new feasible paths. As the number of nodes increases, the algorithm gradually reduces the neighbor radius, which helps to perform more detailed path optimization in the explored space, reduce redundant nodes in the path, and improve the smoothness and stability of the path.

[0087] A smaller neighbor radius threshold helps the algorithm perform local optimization in dense areas of the path tree and find shorter and smoother paths. It can reduce the number of joint mutations, reduce energy consumption and wear during robot movement, and improve the accuracy and stability of the robot in reaching the target point, ensuring that the robot can complete the task smoothly and efficiently while avoiding obstacles.

[0088] The dynamically adjusted neighbor radius threshold can adaptively control the computational complexity of the algorithm according to the current search status. When there are fewer nodes in the search tree, a larger neighbor radius can be used to quickly expand the search tree and reduce the calculation time. When the number of nodes increases, by reducing the search radius, the algorithm can determine the connection of nodes more quickly and reduce unnecessary calculations, thereby significantly reducing the overall running time of the algorithm while maintaining the quality of path planning.

[0089] By setting the neighbor radius threshold, the path search range can be reduced, thereby reducing the amount of calculation in the path search process and releasing computing power.

[0090] The node path cost Cost can quantify the robot rotation range, collision risk, and joint mutation of the node. The node path cost is set as shown in the following formula:

[0091] Cost = λ d Cost_d+λ c Cost_c+λ s Cost_s

[0092] where λ d ,λ c ,λ s is the cost ratio coefficient, and both are greater than 0. The degree of constraint can be adjusted according to the specific application scenario. The larger the coefficient, the greater the degree of constraint. Cost_d is the distance cost, Cost_c is the collision risk cost, and Cost_s is the joint mutation cost.

[0093] In one embodiment, for a path node a, the joint coordinates are (θ a1 ,θ a2 ,...,θ a6 ), path node b, joint coordinates are (θ b1 ,θ b2 ,...,θ b6 ), the distance cost Cost_d between path nodes a and b is calculated using the Manhattan distance formula, as shown below:

[0094]

[0095] When the robot joints rotate with a large amplitude, the distance cost constraint also becomes larger, which can quantify the robot rotation well.

[0096] In one embodiment, by modeling the scene and the robot and using a collision detection library such as FCL (Flexible Collision Library), the minimum distance d between the robot and the scene can be calculated, and then the collision risk cost Cost_c can be determined using the following formula:

[0097]

[0098] The remainder of the denominator ε>0.

[0099] By adding the denominator remainder ε>0 to the collision risk cost formula, we can effectively prevent the cost from approaching infinity when the distance d approaches 0. When the scene is close to the robot, the collision risk cost also increases as the distance between the scene and the robot decreases. Cost_c can well quantify the collision risk between the robot and the scene.

[0100] In one embodiment, 10 -3 >ε>0, it can avoid the problem that the denominator remainder is too large and has too great an impact on the collision risk cost, resulting in poor accuracy of the collision risk cost calculation results.

[0101] In one embodiment, the path node B=(θ B1 ,θ B2 ,...,θ B6 ),C=(θ C1 ,θ C2 ,...,θ C6 ), where A is the parent node of B, B is the parent node of C, the movement order of the path at the above three points is A->B->C, and the joint mutation cost Cost_s of node C is determined by the following formula:

[0102]

[0103] Joint mutation refers to the acceleration of the joint during the movement, that is, non-uniform motion, which will cause the motor to accelerate and decelerate, consuming more energy. As shown in Figure 2(a), the coordinate sequence joint is reversed and needs to be decelerated. As shown in Figure 2(b), the coordinate sequence can move at a uniform speed. As shown in the distance cost formula, the distance cost is the same, but according to the joint mutation cost formula, the left joint mutation cost is 60° and the right joint mutation cost is 0°. Cost_s can be used to quantify the joint mutation situation well.

[0104] In one embodiment, the steps of randomly sampling and generating new nodes in the robot joint configuration space using heuristic node search and adding them to the path tree include:

[0105] Initialize the starting point, end point and empty path tree, and join the path tree from the starting point node;

[0106] Randomly generate a random number rd between 0 and 1, and determine whether rd is greater than the threshold rd_th;

[0107] If rd is less than rd_th, the sampling node q is the end point. If rd is greater than rd_th, a node is randomly selected in the configuration space reachable by the robot as q.

[0108] Find the node p closest to the sampled node q in the tree, and generate a new node q_new from p to q according to the set step size step;

[0109] Check whether the q_new point collides. If not, perform path rewriting and path reconnection operations. If it is not in the configuration space, re-random sampling.

[0110] In this embodiment, the heuristic search strategy can automatically adjust the search for new nodes according to environmental changes and obstacle distribution, and improve the algorithm's adaptability to complex scenes. During the heuristic node search process, a random number rd from 0 to 1 is randomly generated to determine whether rd is greater than the threshold rd_th. The threshold rd_th represents the probability that the node path does not encounter obstacles. When rd_th is set to be small, it means that the probability of no collision around the area where the sampling node is located is small, and it is easy to collide with obstacles, so avoid using the target point as the sampling point. On the contrary, when rd_th is set to be large, the probability of no collision around the area where the sampling node is located is small, and it is not easy to collide with obstacles. The target point is directly used as the sampling point with a high probability. Find the node p closest to the sampling node. At this time, the sampling node q needs to be the end point, the sampling direction needs to be adjusted, and a new sampling node needs to be selected for sampling. If rd is greater than rd_th, a node can be randomly selected as q in the configuration space accessible to the robot. If it is less than rd_th, q is the end point. Find the node p closest to the sampled node q in the tree, and generate a new node q_new from p to q according to the set step size step, where the node position of q_new is calculated by the following formula:

[0111]

[0112] Through the above method, we can first select nodes that are not prone to collision, then use the collision cost formula for verification, and select non-collision nodes for recording. During the whole process, we can greatly reduce the number of nodes that need to be verified by the collision cost formula, reduce the amount of calculation, and improve the calculation efficiency.

[0113] In one embodiment, the steps of performing path rewriting and reconnection operations in the path tree include:

[0114] Path rewriting operation: Find all path nodes q whose distance from q_new is less than the set neighbor radius threshold r, traverse the neighbor nodes, find the node q_min1 with the smallest sum of its original path cost and the path cost from q to q_new, and its path cost is cost_min1. Set q_min1 as the parent node of q_new;

[0115] Path reconnection operation: find all path nodes q whose distance from q_new is less than the set neighbor radius threshold r, traverse the neighboring nodes, and find the node q_min2 whose original path cost is less than the sum of the path cost from q_new to q and cost_min1. Its path cost is cost_min2, set q_new as the parent node of q_min2, and update the path cost of q_min2 to cost_min2.

[0116] Through the above operation method, the path can be rewritten and reconnected, so as to obtain an initial path that meets the requirements of short distance, no collision risk, low energy consumption, small joint mutation and fast running speed.

[0117] In one embodiment, when the generated node reaches the target point or meets a specific stop condition, the step of tracing back from the target point to the starting point and forming an initial path according to the connection relationship between the nodes includes:

[0118] When the generated q_new reaches the end point of the path, the path planning ends. n Backtrack and keep looking for its parent node PA i , until the starting point PA 1 , add all nodes to the path set PA,

[0119] PA={PA 1 ,PA 2 ,...,PA n};

[0120] Where n is the number of initial path nodes.

[0121] After verifying each generated node q_new through the above steps, the nodes in the q_new node with high collision risk, large rotation amplitude and many joint mutations can be eliminated, so that the number of path nodes left is small, with no collision risk and few joint mutations. These path nodes form a path set PA. Using the path set PA, an initial path with no collision risk, low energy consumption, small joint mutation and fast running rate can be quickly generated.

[0122] In one embodiment, the multi-constraint robot joint space path planning method further includes:

[0123] Use the Bezier curve to smooth the initial path PA and generate an initial smooth path PB;

[0124] Use the depth-first search algorithm to combine the initial path with the initial smooth path to generate a smooth path PC without collision risk;

[0125] The dynamic cutting algorithm is used to delete redundant points on the path and generate the final path PD.

[0126] Although the initial path PA generated by the above method has the advantages of no collision risk, low energy consumption, small joint mutation and fast running rate, since the initial path PA is not optimized, there is still a certain possibility of uneven joint rotation in the initial path. Moreover, since the path control points are not processed, the number of path control points is still relatively large, which will still lead to certain problems of unstable posture changes and low robot operation efficiency.

[0127] After smoothing the initial path PA with the Bezier curve and generating an initial smooth path PB, although the path is smoother, the path points have changed, so the smooth path may bring collision risks. Therefore, the depth-first search algorithm is continued to be used to combine the Bezier curve with the depth-first search algorithm, and the Bezier curve is used to smooth the initial path. The depth-first search algorithm is used to adjust the path nodes that may collide, so that a smooth path without collision risks can be generated. On this basis, the path planning method is further optimized, and the dynamic shear algorithm is used to delete redundant points on the path, so that the smooth path control points can be effectively reduced, the robot's running speed can be improved, and energy consumption can be reduced.

[0128] In one embodiment, the step of smoothing the initial path PA using a Bezier curve to generate an initial smoothed path PB includes:

[0129] Use Bezier curves to calculate n control points PA on the initial path PA 1 ,PA 2 ,...,PA n For smoothing, the Bezier curve formula is:

[0130] in is the binomial coefficient,

[0131] Get the initial smooth path PB = {PB 1 ,PB 2 ,...,PB n}, path node PB i It is expressed by the following formula:

[0132]

[0133] Wherein, i is a positive integer from 1 to n, and the starting point and the end point of the initial path PA and the initial smooth path PB are the same.

[0134] The starting point and end point of the initial path PA and the initial smooth path PB are the same, which can be expressed as the following formula:

[0135]

[0136] After processing the initial path PA with the Bezier curve, a smooth initial path can be obtained.

[0137] In one embodiment, the step of using a depth-first search algorithm to combine the initial path and the initial smooth path to generate a smooth path PC without collision risk includes:

[0138] For the initial smooth path, use the depth-first search algorithm to traverse the path;

[0139] During the traversal process, an empty stack path is first created, and the starting point path is added to the empty stack path;

[0140] Detect whether the current initial smooth path node collides with the subsequent initial path node and the path between the initial smooth path nodes;

[0141] If the current initial smooth path node and the subsequent initial path nodes and the path between the initial smooth path nodes all collide with obstacles, then mark the current initial path node as unreachable, and backtrack to the previous node to continue searching for the path;

[0142] If one of the paths between the current initial smooth path node and the subsequent initial path node and the initial smooth path node does not collide with the obstacle, the path formed by the current initial smooth path node and the path node that does not collide is added to the path PC;

[0143] If the current initial smooth path node and the subsequent initial path node and the path between the initial smooth path node do not collide with obstacles, then the current initial smooth path node and the initial smooth path node that does not collide are added to the path PC;

[0144] When a path is found with no collision from the starting point to the end point, the path is used as a smooth path without collision risk, and the path PC generation is completed.

[0145] The initial smooth path P generated by the Bezier curve B The collision with obstacles is not considered, there is a collision risk, and the path clipping operation cannot be used directly or in the next step. The depth-first search algorithm must be used to eliminate the collision risk.

[0146] like Figure 4 As shown, there are two types of collisions that occur on the initial smooth path. The first type is PB 2 ->PB 3 Collision occurred but PB 2 ->PA 3 No collision occurs, the second is PB 4 ->PB 5 With PB 4 ->PA 5 Collision occurs. Figure 5 As shown, using the depth-first search algorithm, first PB 1 Joining the path PC, in the first case, while PB 2 ->PB 3 A collision occurred, but PB 2 ->PA 3 If no collision occurs, PB 2 With PA 3 Join path PC. In the second case, due to PB 4 ->PB 5 With PB 4 ->PA 5 If both collide, then PB 4 It will be marked as unreachable, then go back to the previous path point and select PA 3 ->PA 4 , then the path PC joins PA 4 .Last PA 4 ->PB 5 With PB 5 ->PB 6 If no collision occurs, PB 5 With PB 6 Add path PC, the search ends, the final smooth path PC in the above schematic scenario = {PB 1 ,PB 2 ,PA 3 ,PA 4 ,PB 5 ,PB 6}.

[0147] The smooth path PC processed by the depth-first search algorithm has no collision risk and maintains smooth characteristics, as shown in Figure 4.

[0148] In one embodiment, the steps of using a dynamic clipping algorithm to delete redundant points on the path and generate a final path PD include:

[0149] Create a final set of paths, initially containing only the starting point;

[0150] Traverse each node in the smooth path PC;

[0151] For the currently traversed node, perform collision detection with the top node in the final path set;

[0152] If the detection finds that the path between the current node and the top node of the stack collides with an obstacle, the current node is added to the final path set;

[0153] If the path between the current node and the top node of the stack does not collide with any obstacles, continue traversing the next node until the entire smooth path is traversed;

[0154] A final path PD is generated according to the formed final path set.

[0155] After the smooth path PC is processed by the depth-first search algorithm, there may be redundant points on the path, such as Figure 6 As shown, PC 1 -> PC 3 No risk of collision, but PC 1 -> PC 4 There is a risk of collision, so it is necessary to delete the redundant point PC 2 Similarly, redundant point PC 4 It should also be deleted. The final path PD formed is PD = {PC 1 ,PC 3 ,PC 5 ,PC 6}.

[0156] like Figure 7 As shown, the specific steps of the dynamic shearing algorithm are to create the final path PD, add PC 1 , the path node of traversing PC is collided with the top node of the stack of path PD. If a collision occurs, the current node is added to path PD. If there is no collision, the traversal continues until the traversal of path PC ends. After the dynamic clipping algorithm, the redundant points of the smooth path can be deleted.

[0157] This method is mainly used in robots with six degrees of freedom, which are driven by six joints. Since robot movement is essentially driven by motors, this method is not limited to robot types such as six-axis robots, collaborative robots, SCARA robots, etc.

[0158] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0159] It should be noted that the terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein.

[0160] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A multi-constraint robot joint space path planning method, characterized in that: include: Heuristic node search is used to randomly sample and generate new nodes in the robot joint configuration space and add them to the path tree; Dynamically adjust the neighbor radius threshold according to the number of nodes; For the connection between the generated new node and the existing nodes in the path tree, the connection cost is calculated based on the node path cost, and the connection with the minimum cost is selected, where the node path cost includes distance cost, collision risk cost and joint mutation cost; Perform path rewriting and reconnection operations in the path tree; When the generated node reaches the target point or meets a specific stopping condition, it traces back from the target point to the starting point and forms an initial path PA based on the connection relationship between nodes.

2. The multi-constraint robot joint space path planning method according to claim 1, characterized in that: The neighbor radius threshold is determined by the following formula: where λ r is the proportional coefficient, which is determined according to the time consumption requirement, and N is the number of nodes in the current path tree.

3. The multi-constraint robot joint space path planning method according to claim 1, characterized in that: For a path node a, the joint coordinates are (θ a1 ,θ a2 ,...,θ a6 ), path node b, joint coordinates are (θ b1 ,θ b2 ,...,θ b6 ), the distance cost Cost_d between path nodes a and b is calculated using the following formula:

4. The multi-constraint robot joint space path planning method according to claim 1, characterized in that: Calculate the minimum distance d between the robot and the scene, The collision risk cost Cost_c is determined using the following formula: The remainder of the denominator ε>0.

5. The multi-constraint robot joint space path planning method according to claim 1, characterized in that: Path node A = (θ A1 ,θ A2 ,...,θ A6 ),B=(θ B1 ,θ B2 ,...,θ B6 ),C=(θ C1 ,θ C2 ,...,θ C6 ), where A is the parent node of B, B is the parent node of C, the movement order of the path at the above three points is A->B->C, and the joint mutation cost Cost_s of node C is determined by the following formula:

6. The multi-constraint robot joint space path planning method according to claim 1, characterized in that: The steps of randomly sampling and generating new nodes in the robot joint configuration space using heuristic node search and adding them to the path tree include: Initialize the starting point, end point and empty path tree, and join the path tree from the starting point node; Randomly generate a random number rd between 0 and 1, and determine whether rd is greater than the threshold rd_th; If rd is less than rd_th, the sampling node q is the end point. If rd is greater than rd_th, a node is randomly selected in the configuration space reachable by the robot as q. Find the node p closest to the sampled node q in the tree, and generate a new node q_new from p to q according to the set step size step; Check whether the q_new point collides. If not, perform path rewriting and path reconnection operations. If it is not in the configuration space, re-random sampling.

7. The multi-constraint robot joint space path planning method according to claim 6, characterized in that: The steps of performing path rewriting and reconnection operations in the path tree include: Path rewriting operation: Find all path nodes q whose distance from q_new is less than the set neighbor radius threshold r, traverse the neighbor nodes, find the node q_min1 with the smallest sum of its original path cost and the path cost from q to q_new, and its path cost is cost_min1. Set q_min1 as the parent node of q_new; Path reconnection operation: find all path nodes q whose distance from q_new is less than the set neighbor radius threshold r, traverse the neighboring nodes, and find the node q_min2 whose original path cost is less than the sum of the path cost from q_new to q and cost_min1. Its path cost is cost_min2, set q_new as the parent node of q_min2, and update the path cost of q_min2 to cost_min2.

8. The multi-constraint robot joint space path planning method according to claim 7, characterized in that: When the generated node reaches the target point or meets a specific stop condition, the steps of tracing back from the target point to the starting point and forming the initial path according to the connection relationship between the nodes include: When the generated q_new reaches the end point of the path, the path planning ends and the path planning ends. n Backtrack and keep looking for its parent node PA i , until the starting point PA1, add all nodes to the path set PA, PA={PA1,PA2,...,PA n }; Where n is the number of initial path nodes.

9. The multi-constraint robot joint space path planning method according to any one of claims 1 to 8, characterized in that: The multi-constraint robot joint space path planning method also includes: Use the Bezier curve to smooth the initial path PA and generate an initial smooth path PB; Use the depth-first search algorithm to combine the initial path with the initial smooth path to generate a smooth path PC without collision risk; The dynamic cutting algorithm is used to delete redundant points on the path and generate the final path PD.

10. The multi-constraint robot joint space path planning method according to claim 9, characterized in that: The steps of using the Bezier curve to smooth the initial path PA and generate an initial smooth path PB include: Use Bezier curves to calculate n control points PA1, PA2, ..., PA on the initial path PA n For smoothing, the Bezier curve formula is: in is the binomial coefficient, Get the initial smooth path PB = {PB1, PB2, ..., PB n }, path node PB i It is expressed by the following formula: Wherein, i is a positive integer from 1 to n, and the starting point and the end point of the initial path PA and the initial smooth path PB are the same.

11. The multi-constraint robot joint space path planning method according to claim 9, characterized in that: The steps of using the depth-first search algorithm to combine the initial path with the initial smooth path to generate a smooth path PC without collision risk include: For the initial smooth path, use the depth-first search algorithm to traverse the path; During the traversal process, an empty stack path is first created, and the starting point path is added to the empty stack path; Detect whether the current initial smooth path node collides with the subsequent initial path node and the path between the initial smooth path nodes; If the current initial smooth path node and the subsequent initial path node and the path between the initial smooth path node all collide with obstacles, then mark the current initial path node as unreachable, and backtrack to the previous node to continue searching for the path; if one of the paths between the current initial smooth path node and the subsequent initial path node and the initial smooth path node does not collide with obstacles, then add the path formed by the current initial smooth path node and the path node that does not collide with it to the path PC; If the current initial smooth path node and the subsequent initial path node and the path between the initial smooth path node do not collide with obstacles, then the current initial smooth path node and the initial smooth path node that does not collide are added to the path PC; When a path is found with no collision from the starting point to the end point, the path is used as a smooth path without collision risk, and the path PC generation is completed.

12. The multi-constraint robot joint space path planning method according to claim 11, characterized in that: The steps of using the dynamic cutting algorithm to delete redundant points on the path and generate the final path PD include: Create a final set of paths, initially containing only the starting point; Traverse each node in the smooth path PC; For the currently traversed node, perform collision detection with the top node in the final path set; If the detection finds that the path between the current node and the top node of the stack collides with an obstacle, the current node is added to the final path set; If the path between the current node and the top node of the stack does not collide with any obstacles, continue traversing the next node until the entire smooth path is traversed; A final path PD is generated according to the formed final path set.

Images