Multi-constraint robot joint space path planning method

Abstract

This invention provides a method for multi-constraint robot joint space path planning. The method includes: randomly sampling and generating new nodes in the robot joint configuration space using heuristic node search, and adding them to the path tree; dynamically adjusting the nearest neighbor radius threshold based on the number of nodes; calculating the connection cost between the generated new nodes and existing nodes in the path tree based on the node path cost, and selecting the connection with the minimum cost, wherein the node path cost includes distance cost, collision risk cost, and joint mutation cost; performing path rewriting and reconnection operations in the path tree; and when a 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 based on the connection relationships between nodes. According to this invention, the multi-constraint robot joint space path planning method can quickly generate an initial path with no collision risk, low energy consumption, small joint mutations, and high operating speed.

Description

Technical Field

[0001] This invention relates to the field of robotics, and more specifically, to a method for joint space path planning in multi-constraint robots. Background Technology

[0002] With the modern manufacturing industry's continuous pursuit of automation and intelligence, the combination of industrial vision hardware and computer vision algorithms has endowed industrial robots with more powerful perception and decision-making capabilities. As the robot's eyes and brain, the vision controller analyzes image data collected from cameras to perform tasks such as object recognition, location positioning, size measurement, and environmental mapping in real time, providing precise navigation and control for the robot in complex industrial environments. This enhanced capability enables robots to undertake higher-level tasks on automated production lines, achieving more efficient and intelligent work processes.

[0003] However, industrial production environments are often fraught with uncertainty, and robots must confront obstacles along their paths when performing tasks. This places high demands on the robot's obstacle avoidance path planning, requiring not only that the path safely bypasses obstacles, but also emphasizing path stability, simplified control points, and optimized energy consumption to ensure efficient robot operation and smooth attitude changes.

[0004] Currently, obstacle avoidance path planning for industrial robots mainly relies on teach-in programming and single-path planning algorithms. While teach-in programming is intuitive and easy to use, its flexibility and adaptability are limited, making it difficult to cope with changing production environments. Single-path planning algorithms, such as A*, ant colony optimization, and RRT, can plan paths from the starting point to the destination, but the resulting paths may contain frequent abrupt changes, making it impossible to meet the requirements of stable attitude changes in operating conditions. Summary of the Invention

[0005] The main objective of this invention is to provide a method for joint space path planning of multi-constraint robots, which can quickly generate initial paths with no collision risk, low energy consumption, small joint mutations, and high running speed.

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

[0007] New nodes are generated by randomly sampling in the robot joint configuration space using heuristic node search and added to the path tree;

[0008] The nearest neighbor radius threshold is dynamically adjusted based on the number of nodes.

[0009] For the newly generated node and the existing node in the path tree, the connection cost is calculated based on the node path cost, and the connection with the minimum cost is selected. 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 a generated node reaches the target point or meets a specific stopping condition, it backtracks from the target point to the starting point and forms an initial path PA based on the connection relationship between the nodes.

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

[0013]

[0014] where λ r The scaling factor is determined based on the time consumption requirements, and N is the number of nodes in the current path tree.

[0015] Furthermore, for a certain 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, calculate the minimum distance d between the robot and the scene.

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

[0019]

[0020] The remainder term in the denominator ε > 0.

[0021] Furthermore, 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, and the path moves in the order of A->B->C. 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 using heuristic node search and adding them to the path tree include:

[0024] Initialize the start point, end point, and empty path tree, and add the start point node to the path tree;

[0025] 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, then sampling node q is the endpoint; if rd is greater than rd_th, then a node is randomly selected as q within the robot's reachable configuration space.

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

[0028] Check if a collision occurs at point q_new. If no collision occurs, perform path rewriting and path reconnection operations. If the point is not within the configuration space, perform random sampling again.

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

[0030] Path rewriting operation: Find all path nodes q that are less than the set nearest neighbor radius threshold r from q_new. Traverse the nearest neighbor nodes and find the node with the smallest sum of its original path cost and the path cost from q to q_new as q_min1. 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 that are less than the set nearest neighbor radius threshold r from q_new. Traverse the nearest neighbor 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 a generated node reaches the target point or meets a specific stopping condition, the steps of backtracking from the target point to the starting point and forming an initial path based on the connection relationships between nodes include:

[0033] Path planning ends when the generated q_new reaches the end of the path, and continues from the end point PA. nPerform backtracking, continuously searching for its parent node PA. i Continue until the starting point PA1, then add all nodes to the path set PA.

[0034] PA = {PA1, PA2, ..., PA} n};

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

[0036] Furthermore, multi-constraint robot joint space path planning methods also include:

[0037] The initial path PA is smoothed using a Bézier curve to generate an initial smooth path PB.

[0038] Using a depth-first search algorithm, a smooth path PC with no collision risk is generated by combining the initial path with the initial smooth path.

[0039] The dynamic shearing algorithm is used to delete redundant points on the path, generating the final path PD.

[0040] Furthermore, the steps to smooth the initial path PA using a Bézier curve to generate an initial smooth path PB include:

[0041] Using Bézier curves, n control points PA1, PA2, ..., PA3 on the initial path PA are... n To smooth the curve, the formula for the Bézier curve is:

[0042] in The coefficients are binomial coefficients.

[0043] The initial smooth path PB = {PB1, PB2, ..., PB} is obtained. n}, path node PB i It is expressed by the following formula:

[0044]

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

[0046] Furthermore, the steps of generating a smooth path PC without collision risk using a depth-first search algorithm, combining the initial path and the initial smooth path, include:

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

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

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

[0050] If the current initial smooth path node and the paths between subsequent initial path nodes and initial smooth path nodes all collide with obstacles, then the current initial path node is marked as unreachable, and backtracking is performed to the previous node to continue searching for a path.

[0051] If one of the paths between the current initial smooth path node and subsequent initial path nodes does not collide with an obstacle, then the path formed by the current initial smooth path node and the non-collision path node is added to path PC.

[0052] If the current initial smooth path node and the paths between subsequent initial path nodes and initial smooth path nodes do not collide with obstacles, then add the current initial smooth path node and the initial smooth path nodes that do not collide to the path PC.

[0053] When a path is found that has no collisions from the start point to the end point, the path is taken as a smooth path with no collision risk, and path PC generation is completed.

[0054] Furthermore, the steps for using a dynamic trimming algorithm to remove 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 current traversed node, perform collision detection with the top node of the stack in the final path set;

[0058] If a collision is detected between the current node and the top node of the stack and 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 has been traversed.

[0060] The final path PD is generated based on the resulting set of final paths.

[0061] The method for multi-constraint robot joint space path planning using the technical solution of this invention includes: randomly sampling and generating new nodes in the robot joint configuration space using heuristic node search, and adding them to the path tree; dynamically adjusting the nearest neighbor radius threshold according to the number of nodes; calculating the connection cost between the generated new nodes and existing nodes in the path tree based on the node path cost, and selecting the connection with the minimum cost, wherein the node path cost includes distance cost, collision risk cost, and joint mutation cost; performing path rewriting and reconnection operations in the path tree; and 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 based on the connection relationship between the nodes. The multi-constraint robot joint space path planning method described above employs heuristic node search technology, enabling more efficient generation of new nodes in the robot joint configuration space. This avoids blind sampling, reduces invalid search time, and thus improves the overall efficiency of path planning. By dynamically adjusting the nearest neighbor radius threshold, the algorithm adaptively optimizes the search range. When the node tree size is small, a larger search radius helps to quickly expand the search tree; as the node tree size increases, the search radius gradually decreases, which helps to locally optimize the path, reduce path mutations, and improve path stability. The calculation of node path cost includes collision risk cost, and obstacles are actively avoided during path generation to ensure path stability. To ensure path safety and prevent collisions between the robot and obstacles in the environment, the introduction of joint mutation costs reduces acceleration and deceleration of robot joints during movement, thereby reducing motor energy consumption, improving robot motion performance, and extending equipment life. Path rewriting and reconnection operations can continuously optimize the path by connecting nodes with the lowest cost, reducing the number of path control points, simplifying the path, increasing robot running speed, and reducing energy consumption. When the generated node reaches the target point or meets specific stopping conditions, the algorithm can backtrack from the target point to the starting point and quickly form an initial path based on the connection relationship between nodes. This process is adaptive and can generate the most suitable path according to specific scenarios and needs. Attached Figure Description

[0062] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings:

[0063] Figure 1 A general framework diagram of a multi-constraint robot joint space path planning method according to an embodiment of the present invention is shown;

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

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

[0066] Figure 4 This diagram illustrates the path smoothing principle of the multi-constraint robot joint space path planning method based on the depth-first search algorithm, according to an embodiment of the present invention.

[0067] Figure 5 The flowchart illustrates the implementation of a path smoothing algorithm based on a depth-first search algorithm in the multi-constraint robot joint space path planning method according to an embodiment of the present invention.

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

[0069] Figure 7 A flowchart illustrating the path dynamic shearing algorithm implementation of the multi-constraint robot joint space path planning method according to an embodiment of the present invention is shown. Detailed Implementation

[0070] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and 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] New nodes are generated by randomly sampling in the robot joint configuration space using heuristic node search and added to the path tree;

[0073] The nearest neighbor radius threshold is dynamically adjusted based on the number of nodes.

[0074] For the newly generated node and the existing node in the path tree, the connection cost is calculated based on the node path cost, and the connection with the minimum cost is selected. 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 a generated node reaches the target point or meets a specific stopping condition, it backtracks from the target point to the starting point and forms an initial path PA based on the connection relationship between the nodes.

[0077] The multi-constraint robot joint space path planning method described above employs heuristic node search technology, enabling more efficient generation of new nodes in the robot joint configuration space. This avoids blind sampling, reduces invalid search time, and thus improves the overall efficiency of path planning. By dynamically adjusting the nearest neighbor radius threshold, the algorithm adaptively optimizes the search range. When the node tree size is small, a larger search radius helps to quickly expand the search tree; as the node tree size increases, the search radius gradually decreases, which helps to locally optimize the path, reduce path mutations, and improve path stability. The calculation of node path cost includes collision risk cost, and obstacles are actively avoided during path generation to ensure path stability. To ensure path safety and prevent collisions between the robot and obstacles in the environment, the introduction of joint mutation costs reduces acceleration and deceleration of robot joints during movement, thereby reducing motor energy consumption, improving robot motion performance, and extending equipment life. Path rewriting and reconnection operations can continuously optimize the path by connecting nodes with the lowest cost, reducing the number of path control points, simplifying the path, increasing robot running speed, and reducing energy consumption. When the generated node reaches the target point or meets specific stopping conditions, the algorithm can backtrack from the target point to the starting point and quickly form an initial path based on the connection relationship between 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 the RRT* algorithm operates in a Cartesian coordinate system, neglecting joint rotation. This results in poor end-effector posture stability, making the robot prone to large-amplitude rotations and joint changes. Furthermore, it fails to account for collision risks and cannot plan paths away from obstacles. Traditional RRT* algorithms also suffer from time-consuming path reconnection and rewriting operations, an inability to adaptively adjust the nearest neighbor radius threshold, and inefficiency due to blind sampling. This invention proposes a multi-constraint robot joint space path planning method based on the QMC-RRT* (Quick Multi-constraint Rapidly-exploring Random Tree Star) algorithm. This method imposes distance constraints, joint mutation constraints, and collision risk constraints on the robot. By comprehensively considering multiple factors such as distance, collision risk, and joint mutation, it not only improves the efficiency and stability of path planning and ensures path safety but also effectively reduces energy consumption, improves robot motion performance, and generates an initial path with fewer control points, making it more conducive to actual robot operation.

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

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

[0081]

[0082] where λ r The scaling factor is determined based on the time consumption requirements, and N is the number of nodes in the current path tree.

[0083] The formula for the reciprocal of the nearest neighbor radius threshold is as follows:

[0084]

[0085] λ r λ is a proportionality coefficient that can be manually adjusted according to time requirements. r The larger the value of λ, the longer the time required, and the more stable the path; conversely, λ decreases. r The smaller the value, the shorter the time required, but the path becomes unstable. N represents 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 nearest neighbor radius effectively balances the algorithm's exploration and utilization characteristics. In the early stages of path tree construction, when the number of nodes is small, the algorithm tends to use a larger nearest neighbor radius. This helps to explore the robot joint configuration space more quickly, accelerates the expansion of the path tree, and improves the speed at which the algorithm finds new feasible paths. As the number of nodes increases, the algorithm gradually reduces the nearest neighbor radius. This facilitates more refined path optimization within the explored space, reduces redundant nodes in the path, and improves the smoothness and stability of the path.

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

[0088] The dynamically adjusted nearest neighbor radius threshold can adaptively control the computational complexity of the algorithm based on the current search state. When there are few nodes in the search tree, a larger nearest neighbor radius can quickly expand the search tree and reduce computation time. However, as the number of nodes increases, by reducing the search radius, the algorithm can more quickly determine the connection between nodes and reduce unnecessary computation, thereby significantly reducing the overall running time of the algorithm while maintaining the quality of path planning.

[0089] By setting a nearest neighbor radius threshold, the path search range can be reduced, thereby reducing the amount of computation during the path search process and freeing up computing power.

[0090] The node path cost (Cost) quantifies the robot's rotation range, collision risk, and joint mutations at a node. The node path cost is set using the following formula:

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

[0092] where λ d , λ c , λ s These are cost ratio coefficients, all greater than 0. The degree of constraint can be adjusted according to the specific application scenario; the larger the coefficient, the stronger the 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 a large range, the distance cost constraint also increases, which can effectively quantify the robot's rotation.

[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 term in the denominator ε > 0.

[0099] By adding a remainder term ε > 0 to the denominator of the collision risk cost formula, the problem of the cost approaching infinity when the distance d approaches 0 can be effectively prevented. When the scene and the robot are close, the collision risk cost also increases as the distance between the scene and the robot decreases. Cost_c can effectively quantify the collision risk between the robot and the scene.

[0100] In one embodiment, 10 -3 Setting ε > 0 can avoid the problem that excessively large values ​​of the denominator remainder can have too great an impact on the collision risk cost, resulting in poor accuracy of the collision risk cost calculation.

[0101] In one embodiment, 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, and the path moves in the order of A->B->C. The joint mutation cost Cost_s of node C is determined by the following formula:

[0102]

[0103] Joint mutation refers to the acceleration that occurs during joint movement, i.e., non-uniform motion, which causes the motor to accelerate and decelerate, consuming more energy. As shown in Figure 2(a), the coordinate sequence joints reverse and need to decelerate. As shown in Figure 2(b), the coordinate sequence can move at a constant speed. As shown in the distance cost formula, the distance cost is the same. However, according to the joint mutation cost formula, the mutation cost of the left joint is 60°, and the mutation cost of the right joint is 0°. Cost_s can effectively quantify the joint mutation situation.

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

[0105] Initialize the start point, end point, and empty path tree, and add the start point node to the path tree;

[0106] 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, then sampling node q is the endpoint; if rd is greater than rd_th, then a node is randomly selected as q within the robot's reachable configuration space.

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

[0109] Check if a collision occurs at point q_new. If no collision occurs, perform path rewriting and path reconnection operations. If the point is not within the configuration space, perform random sampling again.

[0110] In this embodiment, the heuristic search strategy can automatically adjust the search for new nodes based on environmental changes and obstacle distribution, improving the algorithm's adaptability to complex scenarios. During the heuristic node search process, a random number rd between 0 and 1 is generated. It is then determined whether rd is greater than a threshold rd_th, which represents the probability that the node path will not encounter obstacles. A smaller rd_th indicates a lower probability of collisions around the sampling node's location, making it more likely to collide with obstacles; therefore, the target point is avoided as the sampling point. Conversely, a larger rd_th indicates a lower probability of collisions around the sampling node's location, making it less likely to collide with obstacles, and the target point is more likely to be used as the sampling point directly. The nearest node p to the sampling node is then found. At this point, the sampling node q is set as the endpoint, the sampling direction is adjusted, and a new sampling node is selected. If rd is greater than rd_th, a node can be randomly selected from the robot's reachable configuration space as q; if rd is less than rd_th, then q is the endpoint. Find the node p in the tree that is closest to the sampling node q. Based on the set step size, generate a new node q_new from p to q. The node position of q_new is calculated using the following formula:

[0111]

[0112] By using the above method, we can first select nodes that are not likely to collide, and then use the collision cost formula for verification. We can select nodes without collisions for recording. In the whole process, we can significantly reduce the number of nodes that need to be verified by the collision cost formula, reduce the amount of computation, and improve computational 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 that are less than the set nearest neighbor radius threshold r from q_new. Traverse the nearest neighbor nodes and find the node with the smallest sum of its original path cost and the path cost from q to q_new as q_min1. 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 that are less than the set nearest neighbor radius threshold r from q_new. Traverse the nearest neighbor 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] By using the above methods, the path can be rewritten and reconnected, thereby obtaining an initial path that meets the requirements of short distance, no collision risk, low energy consumption, small joint mutations, and fast running speed.

[0117] In one embodiment, when a generated node reaches the target point or meets a specific stopping condition, the step of backtracking from the target point to the starting point and forming an initial path based on the connection relationships between nodes includes:

[0118] Path planning ends when the generated q_new reaches the end of the path, and continues from the end point PA. n Perform backtracking, continuously searching for its parent node PA. i Continue until the starting point PA1, then add all nodes to the path set PA.

[0119] PA = {PA1, PA2, ..., PA} n};

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

[0121] After verifying each generated node q_new through the aforementioned steps, nodes with high collision risk, large rotation range, and many joint mutations can be removed, resulting in fewer path nodes remaining 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, few joint mutations, and high running speed can be quickly generated.

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

[0123] The initial path PA is smoothed using a Bézier curve to generate an initial smooth path PB.

[0124] Using a depth-first search algorithm, a smooth path PC with no collision risk is generated by combining the initial path with the initial smooth path.

[0125] The dynamic shearing algorithm is used to delete redundant points on the path, generating 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 mutations, and fast running speed, since the initial path PA is not optimized, there is still a possibility of some joint rotation non-smoothness. Moreover, since the path control points are not processed, the number of path control points is still relatively large, which will still lead to some instability in posture changes and low robot running efficiency.

[0127] After smoothing the initial path PA using Bézier curves to generate an initial smooth path PB, although the path is smoother, the change in path points may introduce collision risks. Therefore, a depth-first search algorithm is employed, combining Bézier curves with the depth-first search algorithm. The Bézier curves are used to smooth the initial path, while the depth-first search algorithm adjusts path nodes that could lead to collisions, thus generating a collision-free smooth path. Furthermore, the path planning method is optimized by using a dynamic shearing algorithm to remove redundant points on the path, effectively reducing the number of control points on the smooth path, improving robot speed, and reducing energy consumption.

[0128] In one embodiment, the step of smoothing the initial path PA using a Bézier curve to generate an initial smooth path PB includes:

[0129] Using Bézier curves, n control points PA1, PA2, ..., PA3 on the initial path PA are... n To smooth the curve, the formula for the Bézier curve is:

[0130] in The coefficients are binomial coefficients.

[0131] The initial smooth path PB = {PB1, PB2, ..., PB} is obtained. n}, path node PB i It is expressed by the following formula:

[0132]

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

[0134] The initial path PA and the initial smooth path PB have the same start and end points, which can be expressed by the following formula:

[0135]

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

[0137] In one embodiment, the steps of generating a smooth path PC with no collision risk using a depth-first search algorithm, combining the initial path and the initial smooth path, include:

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

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

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

[0141] If the current initial smooth path node and the paths between subsequent initial path nodes and initial smooth path nodes all collide with obstacles, then the current initial path node is marked as unreachable, and backtracking is performed to the previous node to continue searching for a path.

[0142] If one of the paths between the current initial smooth path node and subsequent initial path nodes does not collide with an obstacle, then the path formed by the current initial smooth path node and the non-collision path node is added to path PC.

[0143] If the current initial smooth path node and the paths between subsequent initial path nodes and initial smooth path nodes do not collide with obstacles, then add the current initial smooth path node and the initial smooth path nodes that do not collide to the path PC.

[0144] When a path is found that has no collisions from the start point to the end point, the path is taken as a smooth path with no collision risk, and path PC generation is completed.

[0145] For the initial smooth path P generated using Bézier curves B It does not consider collisions with obstacles, has a collision risk, and cannot be used directly or in the next path cutting operation. It is necessary to first use a depth-first search algorithm to eliminate the collision risk.

[0146] like Figure 4As shown, the initial smooth path experiences two types of collisions: the first is a collision between PB2->PB3 but not between PB2->PA3; the second is collisions between both PB4->PB5 and PB4->PA5. Figure 5 As shown, using a depth-first search algorithm, PB1 is first added to path PC. In the first case, although PB2->PB3 collides, PB2->PA3 does not, so PB2 and PA3 are added to path PC. In the second case, since both PB4->PB5 and PB4->PA5 collide, PB4 will be marked as unreachable. Therefore, we backtrack to the previous path point and select PA3->PA4, so path PC is added to PA4. Finally, neither PA4->PB5 nor PB5->PB6 collide, so PB5 and PB6 are added to path PC, and the search ends. The final smoothed path PC in the above illustrated scenario is {PB1, PB2, PA3, PA4, PB5, PB6}.

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

[0148] In one embodiment, the steps of using a dynamic trimming algorithm to delete redundant points on the path and generate the 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 current traversed node, perform collision detection with the top node of the stack in the final path set;

[0152] If a collision is detected between the current node and the top node of the stack and 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 has been traversed.

[0154] The final path PD is generated based on the resulting set of final paths.

[0155] The smoothed path PC processed by the depth-first search algorithm may contain redundant points, such as... Figure 6 As shown, PC1->PC3 has no collision risk, but PC1->PC4 has a collision risk, so redundant point PC2 needs to be deleted. Similarly, redundant point PC4 should also be deleted. The resulting final path PD is PD = {PC1, PC3, PC5, PC6}.

[0156] like Figure 7 As shown, the specific steps of the dynamic trimming algorithm are as follows: create the final path PD, add it to PC1, traverse the path nodes of PC and perform collision detection with the top node of the stack of path PD. If a collision occurs, add the current node to path PD; if no collision occurs, continue traversing until the traversal of path PC is completed. After the dynamic trimming algorithm, redundant points in the smooth path can be deleted.

[0157] This method is primarily used for six-degree-of-freedom robots, driven by six joints. Since robot motion is inherently motor-driven, this method is not limited to robot degree-of-freedom types, such as six-axis robots, collaborative robots, and SCARA robots.

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

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

[0160] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for joint space path planning in a multi-constraint robot, characterized in that, include: New nodes are generated by randomly sampling in the robot joint configuration space using heuristic node search and added to the path tree; The nearest neighbor radius threshold is dynamically adjusted based on the number of nodes. For the newly generated node and the existing node in the path tree, the connection cost is calculated based on the node path cost, and the connection with the minimum cost is selected. 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 a generated node reaches the target point or meets a specific stopping condition, it backtracks from the target point to the starting point and forms an initial path PA based on the connection relationship between the nodes.

2. The multi-constraint robot joint space path planning method according to claim 1, characterized in that, The nearest neighbor radius threshold is determined by the following formula: where λ r The scaling factor is determined based on the time consumption requirements, 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 term in 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, and the path moves in the order of A->B->C. 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 start point, end point, and empty path tree, and add the start point node to the path tree; 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, then sampling node q is the endpoint; if rd is greater than rd_th, then a node is randomly selected as q within the robot's reachable configuration space. Find the node p that is closest to the sampling node q in the tree, and generate a new node q_new from p to q according to the set step size; Check if a collision occurs at point q_new. If no collision occurs, perform path rewriting and path reconnection operations. If the point is not within the configuration space, perform random sampling again.

7. The multi-constraint robot joint space path planning method according to claim 6, characterized in that, The steps for performing path rewriting and reconnection operations in a path tree include: Path rewriting operation: Find all path nodes q that are less than the set nearest neighbor radius threshold r from q_new. Traverse the nearest neighbor nodes and find the node with the smallest sum of its original path cost and the path cost from q to q_new as q_min1. Its path cost is cost_min1. Set q_min1 as the parent node of q_new. Path reconnection operation: Find all path nodes q that are less than the set nearest neighbor radius threshold r from q_new. Traverse the nearest neighbor 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 a generated node reaches the target point or meets a specific stopping condition, the steps of backtracking from the target point to the starting point and forming an initial path based on the connection relationships between nodes include: Path planning ends when the generated q_new reaches the end of the path, and continues from the end point PA. n Perform backtracking, continuously searching for its parent node PA. i Continue until the starting point PA1, then add all nodes to the path set PA. PA={PA1,PA2,...,PA n }; Where n is the number of nodes in the initial path.

9. The multi-constraint robot joint space path planning method according to any one of claims 1 to 8, characterized in that, Multi-constraint robot joint space path planning methods also include: The initial path PA is smoothed using a Bézier curve to generate an initial smooth path PB. Using a depth-first search algorithm, a smooth path PC with no collision risk is generated by combining the initial path with the initial smooth path. The dynamic shearing algorithm is used to delete redundant points on the path, generating the final path PD.

10. The multi-constraint robot joint space path planning method according to claim 9, characterized in that, The steps to smooth the initial path PA using Bézier curves and generate an initial smooth path PB include: Using Bézier curves, n control points PA1, PA2, ..., PA3 on the initial path PA are... n To smooth the curve, the formula for the Bézier curve is: in The coefficients are binomial coefficients. The initial smooth path PB = {PB1, PB2, ..., PB} is obtained. n }, path node PB i It is expressed by the following formula: Where i is a positive integer from 1 to n, and the starting point and ending point of the initial path PA are the same as those of the initial smooth path PB.

11. The multi-constraint robot joint space path planning method according to claim 9, characterized in that, The steps for generating a smooth, collision-free path PC using the depth-first search algorithm, combining the initial path and the initial smooth path, include: For the initial smooth path, a depth-first search algorithm is used to traverse the path; During the traversal, an empty stack path is first created, and the starting path is added to the empty stack path; Detect whether the current initial smooth path node collides with the paths of subsequent initial path nodes and initial smooth path nodes; If the current initial smooth path node and all subsequent initial path nodes collide with obstacles, then the current initial path node is marked as unreachable, and the search continues back to the previous node; if one of the paths between the current initial smooth path node and subsequent initial path nodes does not collide with an obstacle, then the path formed by the current initial smooth path node and the non-collision path node is added to path PC. If the current initial smooth path node and the paths between subsequent initial path nodes and initial smooth path nodes do not collide with obstacles, then add the current initial smooth path node and the initial smooth path nodes that do not collide to the path PC. When a path is found that has no collisions from the start point to the end point, the path is taken as a smooth path with no collision risk, and path PC generation is completed.

12. The multi-constraint robot joint space path planning method according to claim 11, characterized in that, The steps for using a dynamic trimming algorithm to remove 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 current traversed node, perform collision detection with the top node of the stack in the final path set; If a collision is detected between the current node and the top node of the stack and 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 has been traversed. The final path PD is generated based on the resulting set of final paths.

Images