Path determination method and device of mechanical arm, robot, storage medium and product

Abstract

The invention discloses a path determination method and device of a mechanical arm, a robot, a storage medium and a computer program product. The method comprises the steps that the working space of the mechanical arm and the starting point and the target point of the path of the mechanical arm are obtained; estimating a path cost and determining a path cost threshold according to the starting point and the target point of the path; a fast search random tree is constructed bidirectionally from the starting point and the target point of the path, and an initial path meeting a path cost threshold value is obtained; and key points of the initial path are extracted, the extracted key points of the initial path are updated, and an optimized path of the initial path is obtained and serves as the path of the mechanical arm, so that the path of the mechanical arm is determined. According to the scheme, the path cost is estimated, the path cost threshold value is determined, the bidirectional fast search random tree is constructed to determine the initial path, the key points of the initial path are extracted and updated to optimize the initial path, the path with the low cost can be fast obtained, and the flexibility and the real-time performance of path planning of the mechanical arm can be improved.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and specifically relates to a method, device, robot, storage medium and computer program product for determining a path of a robotic arm, and more particularly to a method, device, robot, storage medium and computer program product for planning a robotic arm obstacle avoidance path based on rapid search of random trees and iterative path key points in a static obstacle environment. Background Art

[0002] With the rapid development of robotics technology, robots represented by serial robotic arms are increasingly used in industrial manufacturing, logistics sorting, medical and other fields, which has led to an increasing demand for intelligent robotic arms. In practical applications, robotic arms usually need to operate in scenes with a large number of static obstacles, requiring them to have efficient and reliable path planning capabilities, which is not only the basis for achieving precise motion control and autonomous operation, but also a prerequisite for improving production efficiency and safety. The path planning method based on manual teaching in related solutions often relies on pre-set paths and lacks flexibility and real-time performance.

[0003] The above contents are only used to assist in understanding the technical solutions of the present invention, and do not constitute an admission that the above contents are technologies in related solutions. Summary of the invention

[0004] The object of the present invention is to provide a method, device, robot, storage medium and computer program product for determining the path of a robotic arm, so as to solve the problem that the robotic arm path planning method based on manual teaching of the robot in the related scheme relies on a pre-set path and lacks flexibility and real-time performance, so as to estimate the path cost of the path of the robotic arm and determine the path cost threshold of the path of the robotic arm, construct a bidirectional fast search random tree to determine the initial path, extract the key points of the initial path and update it to optimize the initial path, so as to quickly obtain a path with lower cost, which is beneficial to improving the flexibility and real-time performance of the robotic arm path planning.

[0005] The present invention provides a method for determining the path of a robot arm, which is applied to planning the path of a robot arm in a workspace; the method for determining the path of the robot arm comprises: obtaining the workspace of the robot arm; and obtaining a starting point and a target point pre-set in the workspace of the robot arm as the starting point and the target point of the path of the robot arm; estimating the path cost of the path of the robot arm and determining the path cost threshold of the path of the robot arm according to the starting point and the target point of the path; bidirectionally constructing a fast search random tree from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold; extracting key points of the initial path; updating the extracted key points of the initial path to obtain an optimized path of the initial path as the path of the robot arm, so as to determine the path of the robot arm.

[0006] In some embodiments, based on the starting point and the target point of the path, the path cost of the path of the robotic arm is estimated and a path cost threshold of the path of the robotic arm is determined, including: calculating the Euclidean distance between the starting point and the target point of the path; estimating the path cost of the path of the robotic arm based on the Euclidean distance between the starting point and the target point of the path to obtain an estimated path cost; and determining the product value of the estimated path cost and a pre-set cost coefficient as a threshold of the estimated path cost, as the path cost threshold of the path of the robotic arm.

[0007] In some embodiments, a fast search random tree is bidirectionally constructed from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold, including: using an RRT algorithm to construct a fast search random tree from the starting point of the path, and recording the obtained fast search random tree as a first search random tree; and constructing a fast search random tree from the target point of the path, and recording the obtained fast search random tree as a second search random tree; using a target bias sampling method to alternately expand the nodes of the first search random tree and the nodes of the second search random tree, so that the first search random tree grows in the direction of the target point of the path, and the second search random tree grows in the direction of the starting point of the path, generating a new node, which is recorded as the generated first new node; based on the generated first new node, node distance calculation and collision detection are performed in the first search random tree and the second search random tree, respectively, to obtain an initial path that meets the path cost threshold.

[0008] In some embodiments, based on the generated first new node, node distance calculation and collision detection are performed in the first search random tree and the second search random tree respectively to obtain an initial path that meets the path cost threshold, including: based on the generated first new node, searching the first search random tree and the second search random tree respectively for a node that is closest to the generated first new node as the parent node of the generated first new node; when the generated first new node and the parent node of the generated first new node pass a preset collision detection, determining the cost of the generated first new node, and adding the generated first new node to the corresponding search random trees in the first search random tree and the second search random tree to obtain the added first new node; based on the added first new node, searching the first search random tree and the second search random tree respectively for a node that is closest to the generated first new node as the node that is closest to the added first new node, recorded as the nearest node of the added first new node; in the added When the distance between the first new node and the nearest node of the first new node is less than a preset connection threshold, and the connection between the first new node and the nearest node of the first new node passes the preset collision detection, it is considered that the first search random tree meets the second search random tree, and an initial search path from the starting point of the path to the target point is obtained; the cost of the initial search path is calculated, and it is determined whether the cost of the initial search path is less than the path cost threshold; if it is determined that the cost of the initial search path is less than the path cost threshold, the initial search path is used as the initial path that meets the path cost threshold; if it is determined that the cost of the initial search path is greater than or equal to the path cost threshold, return to reuse the RRT algorithm, build a fast search random tree from the starting point of the path, and record the obtained fast search random tree as the first search random tree; and build a fast search random tree from the target point of the path, and record the obtained fast search random tree as the second search random tree, so as to re-determine the initial path that meets the path cost threshold.

[0009] In some embodiments, extracting the key points of the initial path includes: taking the initial path as the original path, calculating the vertical distance from each node in the original path to the line connecting the starting point and the target point of the path, so as to determine the farthest node and distance, which are recorded as the farthest node and the farthest distance; when the farthest distance is less than or equal to a preset distance threshold, taking the path between the starting point and the target point of the path as a simplified path; when the farthest distance is greater than the preset distance threshold, dividing the original path into two segments from the farthest node, taking each of the two segments as a new original path, and returning Return to recalculate the vertical distance from each node in the new original path to the line connecting the starting point and the target point of the path, so as to obtain two simplified paths; merge the two simplified paths to obtain a merged path, and use the merged path as the simplified path if the merged path passes the preset collision check; if the simplified path passes the preset collision check, use the nodes in the simplified path as the extracted key points; if the simplified path does not pass the preset collision check, use the nodes of the original path as the extracted key points; merge all the extracted key points as the key points of the initial path arrived in advance.

[0010] In some embodiments, the key points of the extracted initial path are updated to obtain an optimized path of the initial path, including: based on the key points of the extracted initial path, constructing a set of key points of the extracted initial path, recorded as a key point set; and constructing a set of edges in the key point set for connecting two key points, recorded as an edge set; based on the key point set and the edge set, for each key point in the key point set, within the range of the edge set, new nodes are sampled in the surrounding space of each key point, recorded as sampled second new nodes; in the case where the sampled second new node passes a preset collision detection with the corresponding key point, the corresponding key point is used as the sampled second new node. The parent node of the sampled second new node is selected, and the cost of the sampled second new node is calculated; a path rewriting operation is performed on the sampled second new node, and a node with the smallest path cost is selected as the parent node for the sampled second new node, so as to obtain the parent node of the sampled second new node and the path cost of the sampled second new node; thereafter, a reconnection operation is performed on the path of the sampled second new node; when the path cost of the sampled second new node is reduced, the sampled second new node is added to the initial path, and the key points of the initial path are updated to obtain an updated key point set; this is iterated until a preset maximum number of iterations is reached, and the path formed by the updated key point set is used as the optimized path of the initial path.

[0011] Matching the above method, the present invention provides a path determination device for a robot arm, which is used for planning the path of a robot arm in a workspace; the path determination device for the robot arm includes: an acquisition unit, configured to acquire the workspace of the robot arm; and acquire a starting point and a target point pre-set in the workspace of the robot arm as the starting point and the target point of the path of the robot arm; a control unit, configured to estimate the path cost of the path of the robot arm and determine the path cost threshold of the path of the robot arm according to the starting point and the target point of the path; the control unit is also configured to bidirectionally construct a fast search random tree from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold; the control unit is also configured to extract key points of the initial path; the control unit is also configured to update the extracted key points of the initial path to obtain an optimized path of the initial path as the path of the robot arm, so as to achieve the determination of the path of the robot arm.

[0012] In some embodiments, the control unit estimates the path cost of the path of the robotic arm and determines the path cost threshold of the path of the robotic arm based on the starting point and the target point of the path, including: calculating the Euclidean distance between the starting point and the target point of the path; estimating the path cost of the path of the robotic arm based on the Euclidean distance between the starting point and the target point of the path to obtain an estimated path cost; and determining the product value of the estimated path cost and a pre-set cost coefficient as the threshold of the estimated path cost, as the path cost threshold of the path of the robotic arm.

[0013] In some embodiments, the control unit constructs a fast search random tree bidirectionally from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold, including: using the RRT algorithm to construct a fast search random tree from the starting point of the path, and recording the obtained fast search random tree as a first search random tree; and constructing a fast search random tree from the target point of the path, and recording the obtained fast search random tree as a second search random tree; using a target bias sampling method to alternately expand the nodes of the first search random tree and the nodes of the second search random tree, so that the first search random tree grows in the direction of the target point of the path, and the second search random tree grows in the direction of the starting point of the path, generating a new node, recorded as the generated first new node; based on the generated first new node, performing node distance calculation and collision detection in the first search random tree and the second search random tree, respectively, to obtain an initial path that meets the path cost threshold.

[0014] In some embodiments, the control unit, based on the generated first new node, performs node distance calculation and collision detection in the first search random tree and the second search random tree respectively to obtain an initial path that meets the path cost threshold, including: based on the generated first new node, searching the first search random tree and the second search random tree respectively for a node that is closest to the generated first new node as the parent node of the generated first new node; when the generated first new node and the parent node of the generated first new node pass a preset collision detection, determining the cost of the generated first new node, and adding the generated first new node to the corresponding search random trees in the first search random tree and the second search random tree to obtain the added first new node; based on the added first new node, searching the first search random tree and the second search random tree respectively for a node that is closest to the generated first new node as the node that is closest to the added first new node, recorded as the nearest node of the added first new node; When the distance between the first new node added and the nearest node of the first new node added is less than a preset connection threshold, and the connection between the first new node added and the nearest node of the first new node added passes the preset collision detection, it is considered that the first search random tree meets the second search random tree, and an initial search path from the starting point of the path to the target point is obtained; the cost of the initial search path is calculated, and it is determined whether the cost of the initial search path is less than the path cost threshold; if it is determined that the cost of the initial search path is less than the path cost threshold, the initial search path is used as the initial path that meets the path cost threshold; if it is determined that the cost of the initial search path is greater than or equal to the path cost threshold, return to reuse the RRT algorithm, build a fast search random tree from the starting point of the path, and record the obtained fast search random tree as the first search random tree; and build a fast search random tree from the target point of the path, and record the obtained fast search random tree as the second search random tree, so as to re-determine the initial path that meets the path cost threshold.

[0015] In some embodiments, the control unit extracts the key points of the initial path, including: taking the initial path as the original path, calculating the vertical distance from each node in the original path to the line connecting the starting point and the target point of the path, so as to determine the farthest node and distance, recorded as the farthest node and the farthest distance; when the farthest distance is less than or equal to a preset distance threshold, taking the path between the starting point and the target point of the path as a simplified path; when the farthest distance is greater than the preset distance threshold, dividing the original path into two segments from the farthest node, and taking each of the two segments as a new original path. path, and return to recalculate the vertical distance from each node in the new original path to the line connecting the starting point and the target point of the path, so as to obtain two simplified paths; merge the two simplified paths to obtain a merged path, and use the merged path as the simplified path if the merged path passes the preset collision check; if the simplified path passes the preset collision check, use the nodes in the simplified path as the extracted key points; if the simplified path does not pass the preset collision check, use the nodes of the original path as the extracted key points; merge all the extracted key points as the key points of the initial path arrived in advance.

[0016] In some embodiments, the control unit updates the key points of the extracted initial path to obtain an optimized path of the initial path, including: constructing a set of key points of the extracted initial path based on the key points of the extracted initial path, recorded as a key point set; and constructing a set of edges in the key point set for connecting two key points, recorded as an edge set; based on the key point set and the edge set, for each key point in the key point set, within the range of the edge set, sampling a new node in the surrounding space of each key point, recorded as a sampled second new node; in the case where the sampled second new node passes a preset collision detection with the corresponding key point, the corresponding key point is used as the sampled first node. The method comprises the following steps: selecting the parent node of the second new node and calculating the cost of the second new node sampled; performing a path rewriting operation on the second new node sampled, selecting a node with the smallest path cost as the parent node for the second new node sampled, obtaining the parent node of the second new node sampled, and the path cost of the second new node sampled; then, performing a reconnection operation on the path of the second new node sampled; when the path cost of the second new node sampled is reduced, adding the second new node sampled to the initial path, and updating the key points of the initial path to obtain an updated key point set; iterating in this way until the preset maximum number of iterations is reached, and taking the path formed by the updated key point set as the optimized path of the initial path.

[0017] Matching the above-mentioned device, the present invention further provides a robot on the other hand, including: the path determination device of the robot arm mentioned above.

[0018] In accordance with the above method, the present invention provides a storage medium on the other hand, wherein the storage medium includes a stored program, wherein when the program is running, the device where the storage medium is located is controlled to execute the steps of the above-mentioned method for determining the path of the robotic arm.

[0019] In accordance with the above method, the present invention provides a computer program product on another aspect, including a computer program, which implements the steps of the above-mentioned method for determining the path of a robotic arm when executed by a processor.

[0020] Therefore, the solution of the present invention is aimed at planning the path of the robot arm of a robot in a static obstacle environment (such as planning the obstacle avoidance path of the robot arm), obtains the working space of the robot arm, as well as the starting point and the target point of the path, estimates the path cost of the path of the robot arm according to the starting point and the target point of the path, and determines the path cost threshold of the path of the robot arm; bidirectionally constructs a fast search random tree from the starting point and the target point to obtain an initial path that meets the path cost threshold; uses a key point extraction algorithm to extract the key points of the initial path; and uses a target bias sampling method to update the extracted key points of the initial path to obtain an optimized path; thereby, the path cost of the path of the robot arm is estimated and the path cost threshold of the path of the robot arm is determined, a bidirectional fast search random tree is constructed to determine the initial path, the key points of the initial path are extracted and updated to optimize the initial path, and a low-cost path can be quickly obtained, which is beneficial to improving the flexibility and real-time performance of the robot arm path planning.

[0021] Other features and advantages of the present invention will be set forth in the description which follows, and in part will be apparent from the description, or may be learned by practice of the present invention.

[0022] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 A schematic diagram of a flow chart of an embodiment of a method for determining a path of a robot arm of the present invention;

[0024] Figure 2 A schematic diagram of a flow chart of an embodiment of estimating the path cost of the path of the robot arm and determining the path cost threshold of the path of the robot arm in the method of the present invention;

[0025] Figure 3 A schematic diagram of a flow chart of an embodiment of bidirectionally constructing a fast search random tree from the starting point and the target point of the path in the method of the present invention;

[0026] Figure 4 A schematic diagram of a flow chart of an embodiment of performing node distance calculation and collision detection in the first search random tree and the second search random tree respectively in the method of the present invention;

[0027] Figure 5 A schematic diagram of a flow chart of an embodiment of extracting key points of the initial path in the method of the present invention;

[0028] Figure 6 A schematic diagram of a flow chart of an embodiment of updating the extracted key points of the initial path in the method of the present invention;

[0029] Figure 7 It is a structural schematic diagram of an embodiment of a path determination device for a robot arm of the present invention;

[0030] Figure 8 A flow chart of an embodiment of a method for robot obstacle avoidance path planning based on fast search of random trees and iterative path key points in a static obstacle environment;

[0031] Fig. 9 A flowchart of the initial path planning operation;

[0032] Fig.10 It is a flowchart of the path key point extraction operation;

[0033] Fig.11 Flowchart of path key point update operation.

[0034] In conjunction with the accompanying drawings, the reference numerals in the embodiments of the present invention are as follows:

[0035] 102 - acquisition unit; 104 - control unit. DETAILED DESCRIPTION

[0036] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the specific embodiments of the present invention and the corresponding drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0037] Considering that the robot arm path planning method based on manual teaching in the relevant scheme relies on a pre-set path, it lacks flexibility and real-time performance. In addition, although the sampling-based rapid random search tree (Rapidly-Exploring Random Tree, RRT) algorithm can theoretically solve the path planning problem in any dimensional space, due to the high dimensionality of the multi-degree-of-freedom robot state space, the speed of path planning cannot meet the needs of real-time applications such as robot material sorting and intelligent welding. For example: taking a six-axis robot as an example, in application scenarios such as material sorting and welding that require obstacle avoidance, the obstacle avoidance path planning time is required to be less than 3 seconds, but the six-axis robot has 6 degrees of freedom, and its path search space is six-dimensional. The RRT algorithm randomly searches for the obstacle avoidance path from the starting point to the end point in the six-dimensional space. The speed is slow, resulting in a path search time of more than 10 seconds at some points, which is difficult to meet the application needs.

[0038] In addition, the initial path cost obtained by the RRT algorithm is often high, and the initial path may lose the optimal solution through biased sampling iteration, resulting in the quality of the optimized path often depending on the initial path, making the final generated path inefficient when applied to production, affecting production efficiency. Low path efficiency refers to the low quality of the obstacle avoidance path, which leads to a long time for the robot to travel from the starting point to the end point, and low efficiency of the robot. The actual reason is that the effect of path optimization is not good, and the cost of the optimized path will still be very high, that is, there will be many unnecessary turning points in the path, and the robot needs to go through a longer path to reach the end point from the starting point. Therefore, the running time of the robot will be relatively long, which will lead to low production efficiency in actual operation. If those unnecessary turning points in the obstacle avoidance path are removed (optimized), the robot can quickly reach the end point from the starting point through a shorter path, which can improve the production efficiency of the robot. Therefore, how to quickly obtain a low-cost obstacle avoidance path in a complex production environment has become the key to achieving autonomous and efficient operation of the robot.

[0039] Therefore, the solution of the present invention proposes a path determination method for a robotic arm, specifically a robotic arm obstacle avoidance path planning method based on a fast search random tree in a static obstacle environment. It uses a bidirectional fast search random tree to accelerate the search speed, and performs path replanning to filter high-cost paths. It can quickly obtain a lower-cost initial obstacle avoidance path, which is beneficial to improving the flexibility and real-time performance of the robotic arm path planning.

[0040] According to an embodiment of the present invention, a method for determining a path of a robot arm is provided. Figure 1 The schematic diagram of a flow chart of an embodiment of the method of the present invention is shown. The path determination method of the robot arm is applied to the planning of the path of the robot arm in the workspace, and is specifically applied to the planning of the obstacle avoidance path of the robot arm in the workspace; in the scheme of the present invention, Figure 1 As shown, the path determination method of the robot arm includes: steps S110 to S150.

[0041] In step S110 , the working space of the robot arm is obtained; and the starting point and the target point pre-set in the working space of the robot arm are obtained as the starting point and the target point of the path of the robot arm.

[0042] At step S120, the path cost of the path of the robot arm is estimated based on the starting point and the target point of the path and the path cost threshold of the path of the robot arm is determined; preferably, the path cost is estimated based on the starting point and the target point of the path, and the path cost threshold is determined based on the path cost.

[0043] At step S130, a fast search random tree is bidirectionally constructed from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold; preferably, an RRT algorithm is used to bidirectionally construct a fast search random tree from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold.

[0044] In step S140, key points of the initial path are extracted; preferably, the key points of the initial path are extracted using an RDP extraction algorithm.

[0045] At step S150, the key points of the extracted initial path are updated to obtain an optimized path of the initial path as the path of the robotic arm, so as to determine the path of the robotic arm; preferably, the target bias sampling method is used to update the key points of the extracted initial path to obtain an optimized path of the initial path as the path of the robotic arm, so as to determine the path of the robotic arm.

[0046] Figure 8 The present invention is a flow chart of an embodiment of a method for robot obstacle avoidance path planning based on fast search of random trees and iterative path key points in a static obstacle environment. Figure 8 The figure shows the overall framework of the robot arm obstacle avoidance path planning method in the solution of the present invention. This figure is only an example of an obstacle avoidance path planning task, and does not mean that the obstacle avoidance path planning method is limited to obstacle avoidance path planning in two-dimensional Cartesian space. Figure 8In the example shown, the path cost is first estimated based on the path starting point and target point, and the path cost threshold is determined. Then, a bidirectional fast search random tree is constructed to quickly obtain an initial obstacle avoidance path that meets the path cost threshold. The key point extraction algorithm is then used to obtain the path key points of the initial path (i.e., the initial obstacle avoidance path). Finally, new nodes are randomly sampled around the key points, and new key points are updated or inserted to reduce the cost of the initial path. Finally, the optimal or near-optimal obstacle avoidance path is obtained, thereby achieving the purpose of quickly obtaining a low-cost obstacle avoidance path in a complex environment.

[0047] like Figure 8 As shown, a robot arm obstacle avoidance path planning method based on fast search of random trees and iterative path key points in a static obstacle environment includes: step 1 and step 2.

[0048] In step 1, the state space of the robot arm, as well as the starting point and target point of the path are determined, the path cost is estimated to determine the path cost threshold, and a bidirectional fast search random tree is constructed and expanded based on target bias sampling to obtain an initial obstacle avoidance path that meets the path cost threshold. The problem of long path search time and low efficiency in high-dimensional space and easy acquisition of high-cost paths is overcome, and the low-cost initial obstacle avoidance path can be quickly obtained, and then step 2 is executed.

[0049] Step 2: After obtaining the initial obstacle avoidance path, extract the key points of the path, and randomly sample the space around the key points of the path to generate new nodes. Use the new nodes to update or add key points of the path, continuously reduce the cost of the initial path, achieve the acquisition of the optimal obstacle avoidance path, and improve the execution efficiency of the obstacle avoidance path in practical applications.

[0050] The solution of the present invention proposes a robot arm obstacle avoidance path planning solution based on a fast search random tree in a static obstacle environment, which uses a bidirectional fast search random tree to accelerate the search speed, and performs path replanning to filter high-cost paths, so as to achieve rapid acquisition of low-cost initial obstacle avoidance paths in high-dimensional and complex environments, and improve the flexibility and real-time performance of the robot arm path planning. In view of the problems of low path search efficiency and poor initial path quality in the robot arm path planning method in related solutions, the solution of the present invention accelerates the path search speed by constructing a bidirectional fast search random tree and adding two target bias samplings; at the same time, the path cost is estimated, high-cost paths are rejected and the paths are replanned, so that a low-cost initial obstacle avoidance path can be quickly obtained, which is conducive to improving the flexibility and real-time performance of the robot arm path planning.

[0051] In some embodiments, the specific process of estimating the path cost of the path of the robotic arm and determining the path cost threshold of the path of the robotic arm according to the starting point and the target point of the path in step S120 is described in the following exemplary embodiment.

[0052] Combine the following Figure 2 The flowchart of an embodiment of the method of the present invention for estimating the path cost of the path of the robot arm and determining the path cost threshold of the path of the robot arm is shown, which further illustrates the specific process of estimating the path cost of the path of the robot arm and determining the path cost threshold of the path of the robot arm in step S120, including: steps S210 to S230.

[0053] Step S210, calculating the Euclidean distance between the starting point and the target point of the path.

[0054] Step S220, based on the Euclidean distance between the starting point and the target point of the path, estimate the path cost of the path of the robot arm to obtain the estimated path cost; that is, use the Euclidean distance between the starting point and the target point of the path as the estimated path cost.

[0055] Step S230, the product value of the estimated path cost and the pre-set cost coefficient is determined as the threshold of the estimated path cost, which is used as the path cost threshold of the path of the robot arm. The pre-set cost coefficient is such as the path cost coefficient λ, and the path cost threshold is such as the path cost threshold Pc calculated by formula (1) max .

[0056] Fig. 9 This is a flowchart of the initial path planning operation. Figure 8 For the specific operation of step 1, please refer to Fig. 9 The example shown. Fig. 9 As shown, the process of the initial path planning operation includes:

[0057] Step 11: determine the state space of the robot arm, as well as the starting point and target point of the path, and then execute step 12.

[0058] The state space of the robot refers to the working space of the robot, which can refer to both Cartesian space and the joint space of the robot. For ease of description, the state space referred to in this embodiment is a two-dimensional Cartesian space, and the state node is defined as a vector composed of point coordinates in a rectangular coordinate system, denoted as q2(x,y). Given the state space S of the robot, the state space can be represented as the obstacle area S obj , obstacle-free area S free , starting point q init Represented as q init ∈S free , target point q goal Represented as q goal ∈S free .

[0059] Step 12: Estimate the path cost, calculate the path cost threshold, and then execute step 13.

[0060] The Euclidean distance between nodes in the state space of the robot is used as the path cost. After determining the starting point and the target point of the path, the path cost is estimated by calculating the Euclidean distance between the starting point and the target point. Then, the estimated path cost is multiplied by the cost coefficient to obtain the path cost threshold. The path cost threshold is calculated as follows:

[0061]

[0062] Among them, Pc max is the path cost threshold, λ is the path cost coefficient, x init is the horizontal coordinate of the starting point, y init is the ordinate of the starting point, x goal is the horizontal coordinate of the target point, y goal is the vertical coordinate of the target point.

[0063] The nodes in the state space of the robot are the nodes in the state space of the robot. The state space of the robot refers to the working space of the robot, which can be the joint space of the robot or the Cartesian space where the robot is located. Taking the six-axis robot as an example, if the robot joint space is selected as the state space, the state space is a six-dimensional space composed of the rotation ranges of the six joint angles of the robot. The rotation ranges of the six joint angles of the robot can be read from the teach pendant of the robot, or according to the rotation limit of the joint angle of the robot, the appropriate range size can be set manually. At this time, the starting node in the state space is the six joint angles of the robot at the initial posture, which can be read from the teach pendant or set manually; similarly, the target node in the state space is the six joint angles when the robot reaches the target point, which can be read from the teach pendant or set manually.

[0064] In the scheme of the present invention, the path cost of the path of the robotic arm is estimated based on the starting point and the target point of the path and the path cost threshold of the path of the robotic arm is determined. By estimating the path cost, high-cost paths are rejected and the path is replanned, which is conducive to obtaining an initial obstacle avoidance path with a lower cost.

[0065] In some implementations, the specific process of bidirectionally constructing a fast search random tree from the starting point and the target point of the path in step S130 to obtain an initial path that meets the path cost threshold is described in the following exemplary embodiment.

[0066] Combine the following Figure 3 The flowchart of an embodiment of bidirectionally constructing a fast search random tree from the starting point and the target point of the path in the method of the present invention further illustrates the specific process of bidirectionally constructing a fast search random tree from the starting point and the target point of the path in step S130, including: steps S310 to S330.

[0067] Step S310: Using the RRT algorithm, a fast search random tree is constructed from the starting point of the path, and the obtained fast search random tree is recorded as the first search random tree, such as the fast search random tree T init (V i ,E i ); and constructing a fast search random tree from the target point of the path, and recording the obtained fast search random tree as the second search random tree, such as the fast search random tree T goal (V g ,E g ).

[0068] Step S320, using the target bias sampling method, alternately expand the nodes of the first search random tree and the nodes of the second search random tree, so that the first search random tree grows toward the target point of the path, and the second search random tree grows toward the starting point of the path, to generate a new node, which is recorded as the generated first new node.

[0069] Step S330: Based on the generated first new node, node distance calculation and collision detection are performed in the first search random tree and the second search random tree respectively to obtain an initial path that meets the path cost threshold.

[0070] like Fig. 9 As shown, the process of the initial path planning operation also includes:

[0071] Step 13: construct a fast search random tree from the starting point and the target point respectively, and then execute step 14.

[0072] In step 13, in order to quickly search for the initial obstacle avoidance path from the starting point to the target point, init and the target point q goal Construct a fast search random tree T init (V i ,E i ), T goal (V g ,E g ), where T init (V i ,E i ) is from the starting point q init The constructed fast search random tree, T goal (V g ,E g ) is from the target point q goal The constructed fast search random tree, V i ,V g ∈S free is the set of state nodes in the tree, E i ,Eg ∈S free is the set of edges connecting two state nodes.

[0073] Step 14: target biased sampling, expanding the fast search of random tree nodes, and then executing step 15. Target biased sampling means that at each random sampling, the target point is selected as a random point with a certain probability, rather than a completely random selection.

[0074] In step 14, for the fast search random tree Tinit(Vi,Ei) and the fast search random tree T goal (V g ,E g ), using the target bias sampling method, alternately expand its nodes so that they grow in the direction of each other. For the fast search random tree Tinit(Vi,Ei), if its number of nodes is less than the T of the fast search random tree goal (V g ,E g ) nodes, the fast search random tree Tinit(Vi,Ei) has a 50% probability of finding the target point q goal Generate a new node qinew as a sampling point, and quickly search the random tree T goal (V g ,E g ) then a new node qinew is used as a sampling point to generate a new node; conversely, if the random tree T is searched quickly goal (V g ,E g ) has fewer nodes than the fast search random tree T init (V i ,E i ) number of nodes, then quickly search the random tree T goal (V g ,E g ) has a 50% probability of using the starting point qinit as the sampling point to generate a new node q gnew , while the fast search random tree Tinit(Vi,Ei) takes the new node q gnew Generate new nodes as sampling points.

[0075] In the solution of the present invention, a bidirectional fast search random tree is used to accelerate the search speed, and target bias sampling is added to accelerate the path search speed.

[0076] In some embodiments, in step S330, based on the generated first new node, node distance calculation and collision detection are performed in the first search random tree and the second search random tree respectively to obtain the specific process of the initial path that meets the path cost threshold. Please refer to the following exemplary description.

[0077] Combine the following Figure 4 The flowchart of an embodiment of performing node distance calculation and collision detection in the first search random tree and the second search random tree in the method of the present invention is shown, further illustrating the specific process of performing node distance calculation and collision detection in the first search random tree and the second search random tree in step S330, including: steps S410 to S470.

[0078] Step S410: Based on the generated first new node, search the first search random tree and the second search random tree for a node that is closest to the generated first new node as a parent node of the generated first new node.

[0079] Step S420, when the generated first new node and the parent node of the generated first new node pass the preset collision detection, determine the cost of the generated first new node, and add the generated first new node to the corresponding search random trees in the first search random tree and the second search random tree to obtain the added first new node.

[0080] Step S430: based on the first new node added, that is, based on the first new node added to the corresponding search random trees in the first search random tree and the second search random tree, search the first search random tree and the second search random tree for the node closest to the generated first new node, respectively, as the node closest to the first new node added, and record it as the nearest node of the first new node added.

[0081] Step S440, when the distance between the first new node added and the nearest node of the first new node added is less than a preset connection threshold, and the connection between the first new node added and the nearest node of the first new node added passes the preset collision detection, it is considered that the first search random tree meets the second search random tree, and an initial search path from the starting point of the path to the target point is obtained.

[0082] Step S450, calculating the cost of searching the initial path, and determining whether the cost of searching the initial path is less than the path cost threshold.

[0083] Step S460: If it is determined that the cost of the searched initial path is less than the path cost threshold, the searched initial path is used as the initial path that meets the path cost threshold.

[0084] Step S470, if it is determined that the cost of the initial search path is greater than or equal to the path cost threshold, return to reuse the RRT algorithm, build a fast search random tree from the starting point of the path, and record the obtained fast search random tree as the first search random tree; and build a fast search random tree from the target point of the path, and record the obtained fast search random tree as the second search random tree, so as to redetermine the initial path that meets the path cost threshold.

[0085] like Fig. 9 As shown, the process of the initial path planning operation also includes:

[0086] Step 15: Determine the two trees, that is, quickly search for random trees T init (V i ,E i ) and fast search random tree T goal (V g ,E g ) is less than the threshold: if so, execute step 16, otherwise return to step 14.

[0087] In step 15, for the generated new node q inew , in the fast search random tree T init (V i ,E i ) to find and generate a new node q inew The nearest node q neares As its parent node, when the generated new node q inew and the nearest node q nearest When collision detection is passed, the new node q is calculated and generated inew The cost of generating a new node q inew Added to Fast Search Random Tree T init (V i ,E i ), the new node q generated inew The cost is calculated as:

[0088]

[0089] in, For node q nearest The cost, d(q nearest ,q inew ) represents node q inew and q inearest Similarly, q gnew Add to the fast search random tree T in the same way goal (V g ,E g )middle.

[0090] Collision detection refers to a technique used in the fields of computer graphics, robotics, etc. to determine whether two or more objects are in contact or overlap. In the solution of the present invention, collision detection refers to detecting whether a collision occurs between the robot body and an environmental obstacle (i.e., whether the robot body and the obstacle do not overlap in space). If the robot body does not collide with the environmental obstacle, it means that the collision detection has passed.

[0091] Step 16: Reversely search the path at the connection point to obtain the initial path, and then execute step 17.

[0092] In step 16, for adding to the fast search random tree T init (V i ,E i ) in the new node q inew , in the fast search random tree T goal (V g ,E g ) searches for the node qconnect closest to the new node qinew added to the fast search random tree Tinit(Vi,Ei). inew With q connect When the distance between them is less than the connection threshold and the connection between the nodes passes the collision detection, it is considered that the fast search random tree Tinit(Vi,Ei) and the fast search random tree T goal (V g ,E g ) encounter, an initial obstacle avoidance path from the starting point to the target point has been searched. Similarly, for the random tree T added to the fast search goal (V g ,E g ) in the new node q gnew Perform the same operation.

[0093] The connection threshold refers to whether the distance between two nodes in two trees is less than a threshold: if it is less than the threshold, the two trees can be connected. Therefore, the connection threshold is used to determine whether the distance between two fast search random trees is close enough and whether the two trees can be connected to generate a path. The distance threshold only refers to the distance between nodes. Although the calculation method of the connection threshold and the distance threshold is the same, the objects and goals of the action are different.

[0094] When quickly searching the random tree T init (V i ,E i ) and fast search random tree T goal (V g ,E g) meet, connect the nodes where the two trees meet, and from the connected nodes, according to the parent node information of each node, reversely search for paths in the two random trees respectively, and finally obtain an initial obstacle avoidance path from the starting point to the target point.

[0095] Step 17: Calculate the cost of the initial obstacle avoidance path from the starting point to the target point, and determine whether the cost of the initial obstacle avoidance path is less than the path cost threshold: if so, execute step 18; otherwise, return to step 13.

[0096] In step 17, the cost of the initial obstacle avoidance path from the starting point to the target point is calculated. If the cost is less than the path cost threshold Pc max , then output the initial obstacle avoidance path, otherwise repeat steps 13 to 16.

[0097] Step 18: When the cost of the initial obstacle avoidance path from the starting point to the target point is less than the path cost threshold Pc max In the case of , the searched initial obstacle avoidance path from the starting point to the target point is output as the initial path.

[0098] In the scheme of the present invention, see Fig. 9 In the example shown, the initial path planning operation is mainly used to obtain the initial obstacle avoidance path. By constructing a bidirectional fast search random tree and high-cost path replanning, it can overcome the problems of long path search time and high search path cost in high-dimensional space and complex obstacle environment, and quickly obtain an initial obstacle avoidance path with low path cost.

[0099] In the scheme of the present invention, a low-cost obstacle avoidance path is quickly obtained, which is mainly reflected in the initial obstacle avoidance path search stage and the initial obstacle avoidance path optimization stage; in the initial obstacle avoidance path search stage, a low-cost obstacle avoidance path can be quickly obtained by bidirectionally quickly searching the random tree, and in the initial obstacle avoidance path optimization stage, because only the key points of the path need to be iterated, it will not generate too much time consumption, and a low-cost optimal path can be obtained, so the purpose of quickly obtaining a low-cost obstacle avoidance path can be achieved.

[0100] In some implementations, the specific process of extracting the key points of the initial path in step S150 is described in the following exemplary embodiments.

[0101] Combine the following Figure 5 The flowchart of an embodiment of the method of the present invention for extracting the key points of the initial path further illustrates the specific process of extracting the key points of the initial path in step S140, including: steps S510 to S550.

[0102] Step S510, taking the initial path as the original path, calculating the vertical distance from each node in the original path to the line connecting the starting point and the target point of the path, to determine the farthest node and distance, recorded as the farthest node and the farthest distance.

[0103] Step S520: When the farthest distance is less than or equal to a preset distance threshold, the path between the starting point and the target point of the path is used as a simplified path.

[0104] Step S530, when the farthest distance is greater than a preset distance threshold, the original path is divided into two segments from the farthest node, each of the two paths is used as a new original path, and returned to recalculate the vertical distance from each node in the new original path to the line connecting the starting point and the target point of the path, so as to obtain two simplified paths; merge the two simplified paths to obtain a merged path, and when the merged path passes the preset collision detection, use the merged path as the simplified path, and when the merged path does not pass the preset collision detection, use the original path as the simplified path.

[0105] Step S540, when the simplified path passes the preset collision check, the nodes in the simplified path are used as the extracted key points; when the simplified path does not pass the preset collision check, the nodes of the original path are used as the extracted key points.

[0106] Step S550: merging all the extracted key points as the key points of the initial path in advance.

[0107] Fig.10 This is a flowchart of the path key point extraction operation. Fig.11 This is a flowchart of the path key point update operation. For the specific operation of step 2, please refer to Fig. 9 and Fig.10 Example shown.

[0108] like Fig.10 As shown in the figure, the process of path key point extraction operation includes:

[0109] Step 21, receiving the initial obstacle avoidance path obtained from steps 11 to 18, and then executing step 22.

[0110] Step 22: For the initial obstacle avoidance path obtained from step 11 to step 18, use it as the original path, calculate the vertical distance from each node in the original path to the line connecting the starting point and the target point of the path, and determine the node q with the farthest distance. far and distance d max , then proceed to step 23.

[0111] Among them, each point in the path can be obtained through step 16, that is: when the fast search random tree and the fast search random tree meet, the nodes where the two trees meet are connected, and from the two connected nodes, according to the parent node information of the nodes, the path nodes are reversely searched in the two random trees respectively, and finally a series of path nodes from the starting point to the target point are obtained, and these nodes constitute the initial obstacle avoidance path.

[0112] Step 23: Determine node q far To the path starting point q init and the target point q goal The distance d of the connection line max Is it less than or equal to the distance threshold: If so, execute step 24; otherwise, execute step 25.

[0113] Step 24: If node q far To the path starting point q init and the target point q goal The distance d of the connection line max If the distance is less than or equal to the distance threshold, the path starting point q init and the target point q goal As the simplified path, step 26 is then executed.

[0114] Step 25: If node q far To the path starting point q init and the target point q goal The distance d of the connection line max If the distance is greater than the threshold, then the node q far Divide the original path into path init and path goal Two segments are formed, and the two segments are used as the original path to recursively call step 22 to step 25, perform simplification operations on the two segments of the path respectively, obtain and merge the simplified path points of each path segment, if the simplified path points pass the collision detection, the simplified path points are used as key points, otherwise the original path points are retained as key points.

[0115] Step 26: Determine whether the simplified path passes the collision detection: if so, execute step 27; otherwise, execute step 28.

[0116] Step 27: If the simplified path passes the collision detection, the simplified path points are used as key points, and then step 29 is executed.

[0117] Step 28: If the simplified path does not pass the collision test, otherwise all points on the original path are retained as key points, and then step 29 is executed.

[0118] Step 29: Merge the key points as the key points of the initial obstacle avoidance path.

[0119] In the solution of the present invention, the path key point extraction operation can reduce the redundant nodes in the initial obstacle avoidance path, thereby improving the movement efficiency of the robot arm, reducing unnecessary pauses and adjustment time, and significantly improving the overall operation speed and accuracy of the robot arm while maintaining the safety of the path.

[0120] In the solution of the present invention, a robot obstacle avoidance path optimization solution based on iterative path key points is proposed, which uses the straight line arc segmentation (Ramer Douglas Peucker, RDP) algorithm to extract path key points, and randomly samples new nodes to iteratively update the path key points, continuously reducing the path cost, achieving the acquisition of the optimal obstacle avoidance path, and improving the path execution efficiency.

[0121] In some implementations, the specific process of updating the extracted key points of the initial path in step S150 to obtain an optimized path of the initial path is described in the following exemplary embodiments.

[0122] Combine the following Figure 6 The flowchart of an embodiment of updating the key points of the extracted initial path in the method of the present invention further illustrates the specific process of updating the key points of the extracted initial path in step S150, including: steps S610 to S660.

[0123] Step S610, based on the extracted key points of the initial path, construct a set of key points of the initial path extracted, recorded as a key point set; and construct a set of edges in the key point set for connecting two key points, recorded as an edge set.

[0124] Step S620: based on the key point set and the edge set, for each key point in the key point set within the range of the edge set, a new node is sampled in the surrounding space of each key point, and recorded as a sampled second new node.

[0125] Step S630: When the sampled second new node passes the preset collision detection with the corresponding key point, the corresponding key point is used as the parent node of the sampled second new node, and the cost of the sampled second new node is calculated.

[0126] Step S640, perform a path rewriting operation on the sampled second new node, select a node with the smallest path cost as the parent node for the sampled second new node, obtain the parent node of the sampled second new node, and the path cost of the sampled second new node; then, perform a reconnection operation on the path of the sampled second new node.

[0127] Step S650: When the path cost of the sampled second new node is reduced, the sampled second new node is added to the initial path, and the key points of the initial path are updated to obtain an updated key point set.

[0128] Step S660, iterating in this way until a preset maximum number of iterations is reached, and then using the path formed by the updated key point set as the optimized path of the initial path.

[0129] like Fig.11 As shown in FIG. 1 , a flowchart of the path key point update operation includes:

[0130] Step 31, receiving all key points on the initial obstacle avoidance path extracted from steps 21 to 29, and then executing step 32.

[0131] Step 32: All key points on the initial obstacle avoidance path extracted from steps 21 to 29 are combined and output as the initial obstacle avoidance path key point set P. key (V k ,E k ), where V k ∈S free is the set of key points, E k ∈S free is a set of edges connecting two key points, and then step 33 is executed.

[0132] Step 33: Sample new nodes in the space around the key point, and then execute step 34.

[0133] In step 33, new nodes are sampled in the space around the key points. Specifically, a key point set P is randomly selected. key (V k ,E k ) is a key point q k As the sampling reference point, the spherical space R with a radius of r around it space ∈S as the sampling space, randomly obtain new sampling points, and generate new nodes q knew .

[0134] For the new node q k_new , search for the closest key point q in the key point set k_nearest , if the new node q k_new and key point q k_nearest If collision detection is passed between them, the key point q k_nearest As a new node q k_new The parent node of , and calculate the new node q k_new The cost of q k_cost , calculated as:

[0135]

[0136] in, The key point q k_nearest The cost, d(q k_nearest ,q k_new ) represents the new node q k_new and key node q k_nearest The Euclidean distance between .

[0137] Step 34: Path rewriting operation, select a parent node with a small cost for the new node, and then execute step 35.

[0138] In step 34, obtain node q k_new After the path cost of node q k_new Perform a path rewrite operation and select a parent node with a lower cost for it, with node q k_new The center is r and the radius is s , search for key points within the range, if there is a key point q k_rewrite , so that the node q is calculated according to formula (3) k_new After the cost, the cost becomes smaller, and the node q k_new and q k_rewrite If the collision detection is passed, the node q is modified. k_new The parent node is q k_rewrite .

[0139] Step 35: Path reconnection operation, calculate the path cost after the new node is added, and then execute step 36.

[0140] In step 35, determine the node q k_new After the parent node and path cost of the node q, the path is reconnected. k_new The center is r and the radius is s , search for key points within this range.

[0141] Step 36, determine whether the path cost is reduced: if so, execute step 37, otherwise return to step 33.

[0142] Step 37: Add the new node to the path, update the key points of the path, and then execute step 38.

[0143] After searching for key points within the range in step 35, if the key point q k_relink Take node q k_new After becoming the parent node, its path cost decreases, so the node q k_new As the key point q k_relink The parent node of joins the key point set P key (V k ,E k ), discard the key point q k_relinkThe original parent node is used to continuously reduce the path cost.

[0144] Step 38: Repeat steps 33 to 38 until the maximum number of iterations is reached, and output the key point set as the final optimized path.

[0145] In order to solve the problem that the biased sampling method loses the optimal solution, resulting in a high cost of the final generated path, which affects production efficiency, the solution of the present invention obtains the key points of the path by trimming the initial path, and then randomly samples the space around the key points, continuously updates the key points or inserts new path points to reduce the path cost, thereby obtaining the optimal or near-optimal path. In the solution of the present invention, a method of randomly sampling new nodes to iterate and update the key points of the initial obstacle avoidance path is adopted, which solves the problem that the path optimization method in the related solution loses the optimal solution and is prone to generate a high-cost optimized path, thereby reducing the initial path cost to obtain the optimal or near-optimal optimized path.

[0146] In the scheme of the present invention, see Fig.10 and Fig.11 In the example shown, the path key point extraction operation and the path key point update operation are mainly used to optimize the initial obstacle avoidance path obtained in the initial path planning operation. By iteratively updating the path key points, the high-cost path key points in the initial path (i.e., unnecessary turning points in the path) are replaced to continuously reduce the cost of the path, and finally obtain an optimal path from the starting point to the end point, thereby reducing the running time and energy consumption of the robot arm from the starting point to the end point and improving the execution efficiency of the robot arm.

[0147] In the scheme of the present invention, a bidirectional fast search random tree is first constructed to accelerate the path search efficiency, and then the high-cost path searched is replanned to obtain a low-cost path. Overall, the problems of low path search efficiency and poor path quality of the robot arm path planning method in related schemes in high-dimensional space and complex obstacle environments are solved, and a low-cost initial obstacle avoidance path can be quickly obtained.

[0148] By adopting the technical solution of this embodiment, for the planning of the robot arm path of the robot in a static obstacle environment (such as the planning of the robot arm obstacle avoidance path), the working space of the robot arm, as well as the starting point and the target point of the path are obtained, and the path cost of the path of the robot arm is estimated according to the starting point and the target point of the path, and the path cost threshold of the path of the robot arm is determined; a fast search random tree is bidirectionally constructed from the starting point and the target point to obtain an initial path that meets the path cost threshold; the key points of the initial path are extracted by using a key point extraction algorithm; and the key points of the extracted initial path are updated by using a target bias sampling method to obtain an optimized path; thereby, the path cost of the path of the robot arm is estimated and the path cost threshold of the path of the robot arm is determined, a bidirectional fast search random tree is constructed to determine the initial path, the key points of the initial path are extracted and updated to optimize the initial path, so that a low-cost path can be quickly obtained, which is conducive to improving the flexibility and real-time performance of the robot arm path planning.

[0149] According to an embodiment of the present invention, a path determination device for a robot arm corresponding to the path determination method for a robot arm is also provided. Figure 7 The schematic diagram of the structure of an embodiment of the device of the present invention is shown. The path determination device of the robot arm is applied to plan the path of the robot arm in the workspace, and is specifically applied to plan the obstacle avoidance path of the robot arm in the workspace; in the scheme of the present invention, Figure 7 As shown, the path determination device of the robot arm includes: an acquisition unit 102 and a control unit 104.

[0150] The acquisition unit 102 is configured to acquire the working space of the robot arm and acquire the starting point and the target point pre-set in the working space of the robot arm as the starting point and the target point of the path of the robot arm. The specific functions and processing of the acquisition unit 102 refer to step S110.

[0151] The control unit 104 is configured to estimate the path cost of the path of the robot arm and determine the path cost threshold of the path of the robot arm according to the starting point and the target point of the path; preferably, the path cost is estimated according to the starting point and the target point of the path, and the path cost threshold is determined according to the path cost. The specific functions and processing of the control unit 104 refer to step S120.

[0152] The control unit 104 is further configured to construct a fast search random tree from the starting point and the target point of the path in both directions to obtain an initial path that meets the path cost threshold; preferably, the RRT algorithm is used to construct a fast search random tree from the starting point and the target point of the path in both directions to obtain an initial path that meets the path cost threshold. The specific functions and processing of the control unit 104 are also shown in step S130.

[0153] The control unit 104 is further configured to extract key points of the initial path, preferably using an RDP extraction algorithm to extract key points of the initial path. The specific functions and processing of the control unit 104 are also shown in step S140.

[0154] The control unit 104 is further configured to update the key points of the extracted initial path to obtain an optimized path of the initial path as the path of the robot arm, so as to determine the path of the robot arm, preferably using a target bias sampling method to update the key points of the extracted initial path to obtain an optimized path of the initial path as the path of the robot arm, so as to determine the path of the robot arm. The specific functions and processing of the control unit 104 are also shown in step S150.

[0155] Figure 8 The present invention is a flow chart of an embodiment of a method for robot obstacle avoidance path planning based on fast search of random trees and iterative path key points in a static obstacle environment. Figure 8 The figure shows the overall framework of the robot arm obstacle avoidance path planning method in the solution of the present invention. This figure is only an example of an obstacle avoidance path planning task, and does not mean that the obstacle avoidance path planning method is limited to obstacle avoidance path planning in two-dimensional Cartesian space. Figure 8 In the example shown, the path cost is first estimated based on the path starting point and target point, and the path cost threshold is determined. Then, a bidirectional fast search random tree is constructed to quickly obtain an initial obstacle avoidance path that meets the path cost threshold. The key point extraction algorithm is then used to obtain the path key points of the initial path (i.e., the initial obstacle avoidance path). Finally, new nodes are randomly sampled around the key points, and new key points are updated or inserted to reduce the cost of the initial path. Finally, the optimal or near-optimal obstacle avoidance path is obtained, thereby achieving the purpose of quickly obtaining a low-cost obstacle avoidance path in a complex environment.

[0156] like Figure 8 As shown, a robot arm obstacle avoidance path planning method based on fast search of random trees and iterative path key points in a static obstacle environment includes: step 1 and step 2.

[0157] In step 1, the state space of the robot arm, as well as the starting point and target point of the path are determined, the path cost is estimated to determine the path cost threshold, and a bidirectional fast search random tree is constructed and expanded based on target bias sampling to obtain an initial obstacle avoidance path that meets the path cost threshold. The problem of long path search time and low efficiency in high-dimensional space and easy acquisition of high-cost paths is overcome, and the low-cost initial obstacle avoidance path can be quickly obtained, and then step 2 is executed.

[0158] Step 2: After obtaining the initial obstacle avoidance path, extract the key points of the path, and randomly sample the space around the key points of the path to generate new nodes. Use the new nodes to update or add key points of the path, continuously reduce the cost of the initial path, achieve the acquisition of the optimal obstacle avoidance path, and improve the execution efficiency of the obstacle avoidance path in practical applications.

[0159] The solution of the present invention proposes a robot arm obstacle avoidance path planning solution based on a fast search random tree in a static obstacle environment, which uses a bidirectional fast search random tree to accelerate the search speed, and performs path replanning to filter high-cost paths, so as to achieve rapid acquisition of low-cost initial obstacle avoidance paths in high-dimensional and complex environments, and improve the flexibility and real-time performance of the robot arm path planning. In view of the problems of low path search efficiency and poor initial path quality in the robot arm path planning method in related solutions, the solution of the present invention accelerates the path search speed by constructing a bidirectional fast search random tree and adding two target bias samplings; at the same time, the path cost is estimated, high-cost paths are rejected and the paths are replanned, so that a low-cost initial obstacle avoidance path can be quickly obtained, which is conducive to improving the flexibility and real-time performance of the robot arm path planning.

[0160] In some embodiments, the control unit 104 estimates the path cost of the path of the robot arm and determines the path cost threshold of the path of the robot arm according to the starting point and the target point of the path, including:

[0161] The control unit 104 is further configured to calculate the Euclidean distance between the starting point and the target point of the path. The specific functions and processing of the control unit 104 are also shown in step S210.

[0162] The control unit 104 is further configured to estimate the path cost of the path of the robot arm according to the Euclidean distance between the starting point and the target point of the path, and obtain an estimated path cost; that is, the Euclidean distance between the starting point and the target point of the path is used as the estimated path cost. The specific functions and processing of the control unit 104 are also shown in step S220.

[0163] The control unit 104 is further configured to determine the product value of the estimated path cost and the preset cost coefficient as the threshold of the estimated path cost, as the path cost threshold of the path of the robot arm. The preset cost coefficient is such as the path cost coefficient λ, and the path cost threshold is such as the path cost threshold Pc calculated by formula (1): max The specific functions and processing of the control unit 104 can also be found in step S230.

[0164] Fig. 9 This is a flowchart of the initial path planning operation. Figure 8 For the specific operation of step 1, please refer to Fig. 9 The example shown. Fig. 9 As shown, the process of the initial path planning operation includes:

[0165] Step 11: determine the state space of the robot arm, as well as the starting point and target point of the path, and then execute step 12.

[0166] The state space of the robot refers to the working space of the robot, which can refer to both Cartesian space and the joint space of the robot. For ease of description, the state space referred to in this embodiment is a two-dimensional Cartesian space, and the state node is defined as a vector composed of point coordinates in a rectangular coordinate system, denoted as q2(x,y). Given the state space S of the robot, the state space can be represented as the obstacle area S obj , obstacle-free area S free , starting point q init Represented as q init ∈S free , target point q goal Represented as q goal ∈S free .

[0167] Step 12: Estimate the path cost, calculate the path cost threshold, and then execute step 13.

[0168] The Euclidean distance between nodes in the state space of the robot is used as the path cost. After determining the starting point and the target point of the path, the path cost is estimated by calculating the Euclidean distance between the starting point and the target point. Then, the estimated path cost is multiplied by the cost coefficient to obtain the path cost threshold. The path cost threshold is calculated as follows:

[0169]

[0170] Among them, Pc max is the path cost threshold, λ is the path cost coefficient, x init is the horizontal coordinate of the starting point, y init is the ordinate of the starting point, x goal is the horizontal coordinate of the target point, ygoal is the vertical coordinate of the target point.

[0171] In the scheme of the present invention, the path cost of the path of the robotic arm is estimated based on the starting point and the target point of the path and the path cost threshold of the path of the robotic arm is determined. By estimating the path cost, high-cost paths are rejected and the path is replanned, which is conducive to obtaining an initial obstacle avoidance path with a lower cost.

[0172] In some implementations, the control unit 104 constructs a fast search random tree bidirectionally from the starting point and the target point of the path to obtain an initial path that satisfies the path cost threshold, including:

[0173] The control unit 104 is further configured to use the RRT algorithm to construct a fast search random tree from the starting point of the path, and record the obtained fast search random tree as the first search random tree, such as the fast search random tree T init (V i ,E i ); and constructing a fast search random tree from the target point of the path, and recording the obtained fast search random tree as the second search random tree, such as the fast search random tree T goal (V g ,E g ). The specific functions and processing of the control unit 104 also refer to step S310.

[0174] The control unit 104 is further configured to adopt a target bias sampling method to alternately expand the nodes of the first search random tree and the nodes of the second search random tree, so that the first search random tree grows toward the target point of the path, and the second search random tree grows toward the starting point of the path, to generate a new node, which is recorded as the generated first new node. The specific functions and processing of the control unit 104 are also shown in step S320.

[0175] The control unit 104 is further configured to perform node distance calculation and collision detection in the first search random tree and the second search random tree based on the generated first new node, respectively, to obtain an initial path that meets the path cost threshold. The specific functions and processing of the control unit 104 are also shown in step S330.

[0176] like Fig. 9 As shown, the process of the initial path planning operation also includes:

[0177] Step 13: construct a fast search random tree from the starting point and the target point respectively, and then execute step 14.

[0178] In step 13, in order to quickly search for the initial obstacle avoidance path from the starting point to the target point, init and the target point q goal Construct fast search random tree Tinit(Vi,Ei), T goal (V g ,E g ), where Tinit(Vi,Ei) is a fast search random tree constructed from the starting point qinit, T goal (V g ,E g ) is from the target point q goal The constructed fast search random tree, V i ,V g ∈S free is the set of state nodes in the tree, E i ,E g ∈S free is the set of edges connecting two state nodes.

[0179] Step 14: target biased sampling, expanding the fast search of random tree nodes, and then executing step 15. Target biased sampling means that at each random sampling, the target point is selected as a random point with a certain probability, rather than a completely random selection.

[0180] In step 14, for the fast search random tree T init (V i ,E i ) and fast search random tree T goal (V g ,E g ), using the target bias sampling method, alternately expanding its nodes so that they grow in the direction of each other, for fast search random tree T init (V i ,E i ) If the number of its nodes is less than T of the fast search random tree goal (V g ,E g ) nodes, the fast search random tree Tinit(Vi,Ei) has a 50% probability of finding the target point q goal Generate a new node qinew as a sampling point, and quickly search the random tree T goal (V g ,E g ) then a new node qinew is used as a sampling point to generate a new node; conversely, if the random tree T is searched quickly goal (V g ,E g ) is less than the number of nodes in the fast search random tree Tinit(Vi,Ei), then the fast search random tree Tgoal (V g ,E g ) has a 50% probability of using the starting point qinit as the sampling point to generate a new node q gnew , while the fast search random tree Tinit(Vi,Ei) takes the new node q gnew Generate new nodes as sampling points.

[0181] In the solution of the present invention, a bidirectional fast search random tree is used to accelerate the search speed, and target bias sampling is added to accelerate the path search speed.

[0182] In some implementations, the control unit 104 performs node distance calculation and collision detection in the first search random tree and the second search random tree based on the generated first new node, respectively, to obtain an initial path that satisfies the path cost threshold, including:

[0183] The control unit 104 is further configured to search for a node closest to the generated first new node in the first search random tree and the second search random tree, based on the generated first new node, as a parent node of the generated first new node. The specific functions and processing of the control unit 104 are also shown in step S410.

[0184] The control unit 104 is further configured to determine the cost of the generated first new node when the generated first new node and the parent node of the generated first new node pass the preset collision detection, and add the generated first new node to the corresponding search random tree in the first search random tree and the second search random tree to obtain the added first new node. The specific functions and processing of the control unit 104 are also shown in step S420.

[0185] The control unit 104 is further configured to search for the node closest to the generated first new node in the first search random tree and the second search random tree, based on the first new node added, that is, based on the first new node added to the corresponding search random tree in the first search random tree and the second search random tree, and use the node closest to the added first new node as the node closest to the added first new node, and record it as the nearest node of the added first new node. The specific functions and processing of the control unit 104 are also shown in step S430.

[0186] The control unit 104 is further configured to consider that the first search random tree meets the second search random tree when the distance between the first new node added and the nearest node of the first new node added is less than a preset connection threshold, and the connection between the first new node added and the nearest node of the first new node added passes a preset collision detection, and obtain an initial search path from the starting point of the path to the target point. The specific functions and processing of the control unit 104 are also shown in step S440.

[0187] The control unit 104 is further configured to calculate the cost of searching the initial path and determine whether the cost of searching the initial path is less than the path cost threshold. The specific functions and processing of the control unit 104 are also shown in step S450.

[0188] The control unit 104 is further configured to use the searched initial path as the initial path that meets the path cost threshold if it is determined that the cost of the searched initial path is less than the path cost threshold. The specific functions and processing of the control unit 104 are also shown in step S460.

[0189] The control unit 104 is further configured to, if it is determined that the cost of the searched initial path is greater than or equal to the path cost threshold, return to reuse the RRT algorithm, build a fast search random tree from the starting point of the path, and record the obtained fast search random tree as the first search random tree; and build a fast search random tree from the target point of the path, and record the obtained fast search random tree as the second search random tree, so as to re-determine the initial path that meets the path cost threshold. The specific functions and processing of the control unit 104 are also shown in step S470.

[0190] like Fig. 9 As shown, the process of the initial path planning operation also includes:

[0191] Step 15: Determine the two trees, namely the fast search random tree Tinit(Vi,Ei) and the fast search random tree T goal (V g ,E g ) is less than the threshold: if so, execute step 16, otherwise return to step 14.

[0192] In step 15, for the generated new node q inew , in the fast search random tree T init (V i ,E i ) to find and generate a new node q inew The nearest node q neares As its parent node, when the generated new node qinew and the nearest node q nearest When collision detection is passed, the new node q is calculated and generated inew The cost of generating a new node q inew Added to Fast Search Random Tree T init (V i ,E i ), the new node q generated inew The cost is calculated as:

[0193]

[0194] in, For node q nearest The cost, d(q nearest ,q inew ) represents node q inew and q inearest Similarly, q gnew Add to the fast search random tree T in the same way goal (V g ,E g )middle.

[0195] Step 16: Reversely search the path at the connection point to obtain the initial path, and then execute step 17.

[0196] In step 16, for adding to the fast search random tree T init (V i ,E i ) in the new node q inew , in the fast search random tree T goal (V g ,E g ) and add to the fast search random tree T init (V i ,E i ) in the new node q inew The nearest node q connect , when node q inew With q connect When the distance between them is less than the connection threshold and the connection between the nodes passes the collision detection, it is considered that the fast search random tree Tinit(Vi,Ei) and the fast search random tree T goal (V g ,E g ) encounter, an initial obstacle avoidance path from the starting point to the target point has been searched. Similarly, for the random tree T added to the fast search goal (V g ,E g ) in the new node q gnew Perform the same operation.

[0197] When quickly searching the random tree T init (V i ,E i ) and fast search random tree T goal (V g ,E g ) meet, connect the nodes where the two trees meet, and from the connected nodes, according to the parent node information of each node, reversely search for paths in the two random trees respectively, and finally obtain an initial obstacle avoidance path from the starting point to the target point.

[0198] Step 17: Calculate the cost of the initial obstacle avoidance path from the starting point to the target point, and determine whether the cost of the initial obstacle avoidance path is less than the path cost threshold: if so, execute step 18; otherwise, return to step 13.

[0199] In step 17, the cost of the initial obstacle avoidance path from the starting point to the target point is calculated. If the cost is less than the path cost threshold Pc max , then output the initial obstacle avoidance path, otherwise repeat steps 13 to 16.

[0200] Step 18: When the cost of the initial obstacle avoidance path from the starting point to the target point is less than the path cost threshold Pc max In the case of , the searched initial obstacle avoidance path from the starting point to the target point is output as the initial path.

[0201] In the scheme of the present invention, see Fig. 9 In the example shown, the initial path planning operation is mainly used to obtain the initial obstacle avoidance path. By constructing a bidirectional fast search random tree and high-cost path replanning, it can overcome the problems of long path search time and high search path cost in high-dimensional space and complex obstacle environment, and quickly obtain an initial obstacle avoidance path with low path cost.

[0202] In the scheme of the present invention, a low-cost obstacle avoidance path is quickly obtained, which is mainly reflected in the initial obstacle avoidance path search stage and the initial obstacle avoidance path optimization stage; in the initial obstacle avoidance path search stage, a low-cost obstacle avoidance path can be quickly obtained by bidirectionally quickly searching the random tree, and in the initial obstacle avoidance path optimization stage, because only the key points of the path need to be iterated, it will not generate too much time consumption, and a low-cost optimal path can be obtained, so the purpose of quickly obtaining a low-cost obstacle avoidance path can be achieved.

[0203] In some implementations, the control unit 104 extracts the key points of the initial path, including:

[0204] The control unit 104 is further configured to use the initial path as the original path, calculate the vertical distance from each node in the original path to the line connecting the starting point and the target point of the path, so as to determine the farthest node and distance, which are recorded as the farthest node and the farthest distance. The specific functions and processing of the control unit 104 are also shown in step S510.

[0205] The control unit 104 is further configured to use the path between the starting point and the target point of the path as a simplified path when the farthest distance is less than or equal to a preset distance threshold. The specific functions and processing of the control unit 104 are also shown in step S520.

[0206] The control unit 104 is further configured to divide the original path into two segments from the farthest node when the farthest distance is greater than a preset distance threshold, and use each of the two segments as a new original path and return, so as to recalculate the vertical distance from each node in the new original path to the line connecting the starting point and the target point of the path, so as to obtain two simplified paths; merge the two simplified paths to obtain a merged path, and use the merged path as a simplified path when the merged path passes the preset collision detection, and use the original path as a simplified path when the merged path does not pass the preset collision detection. The specific functions and processing of the control unit 104 are also shown in step S530.

[0207] The control unit 104 is further configured to use the nodes in the simplified path as the extracted key points when the simplified path passes the preset collision check; and use the nodes in the original path as the extracted key points when the simplified path does not pass the preset collision check. The specific functions and processing of the control unit 104 are also shown in step S540.

[0208] The control unit 104 is further configured to merge all the extracted key points as the key points of the initial path in advance. The specific functions and processing of the control unit 104 are also shown in step S550.

[0209] Fig.10 This is a flowchart of the path key point extraction operation. Fig.11 This is a flowchart of the path key point update operation. For the specific operation of step 2, please refer to Fig. 9 and Fig.10 Example shown.

[0210] like Fig.10 As shown in the figure, the process of path key point extraction operation includes:

[0211] Step 21, receiving the initial obstacle avoidance path obtained from steps 11 to 18, and then executing step 22.

[0212] Step 22: For the initial obstacle avoidance path obtained from step 11 to step 18, use it as the original path, calculate the vertical distance from each node in the original path to the line connecting the starting point and the target point of the path, and determine the node q with the farthest distance. far and distance d max , then proceed to step 23.

[0213] Step 23: Determine node q far To the path starting point q init and the target point q goal The distance d of the connection line max Is it less than or equal to the distance threshold: If so, execute step 24; otherwise, execute step 25.

[0214] Step 24: If node q far To the path starting point q init and the target point q goal The distance d of the connection line max If the distance is less than or equal to the distance threshold, the path starting point q init and the target point q goal As the simplified path, step 26 is then executed.

[0215] Step 25: If node q far To the path starting point q init and the target point q goal The distance d of the connection line max If the distance is greater than the threshold, then the node q far Divide the original path into path init and path goal Two segments are formed, and the two segments are used as the original path to recursively call step 22 to step 25, perform simplification operations on the two segments of the path respectively, obtain and merge the simplified path points of each path segment, if the simplified path points pass the collision detection, the simplified path points are used as key points, otherwise the original path points are retained as key points.

[0216] Step 26: Determine whether the simplified path passes the collision detection: if so, execute step 27; otherwise, execute step 28.

[0217] Step 27: If the simplified path passes the collision detection, the simplified path points are used as key points, and then step 29 is executed.

[0218] Step 28: If the simplified path does not pass the collision test, otherwise all points on the original path are retained as key points, and then step 29 is executed.

[0219] Step 29: Merge the key points as the key points of the initial obstacle avoidance path.

[0220] In the solution of the present invention, a robot obstacle avoidance path optimization solution based on iterative path key points is proposed, which uses the straight line arc segmentation (Ramer Douglas Peucker, RDP) algorithm to extract path key points, and randomly samples new nodes to iteratively update the path key points, continuously reducing the path cost, achieving the acquisition of the optimal obstacle avoidance path, and improving the path execution efficiency.

[0221] In some implementations, the control unit 104 updates the extracted key points of the initial path to obtain an optimized path of the initial path, including:

[0222] The control unit 104 is further configured to construct a set of key points of the extracted initial path based on the key points of the extracted initial path, recorded as a key point set; and construct a set of edges in the key point set for connecting two key points, recorded as an edge set. The specific functions and processing of the control unit 104 are also shown in step S610.

[0223] The control unit 104 is further configured to sample a new node in the surrounding space of each key point in the key point set within the range of the edge set based on the key point set and the edge set, and record it as the sampled second new node. The specific function and processing of the control unit 104 are also shown in step S620.

[0224] The control unit 104 is further configured to, when the sampled second new node passes a preset collision detection with the corresponding key point, use the corresponding key point as the parent node of the sampled second new node and calculate the cost of the sampled second new node. The specific functions and processing of the control unit 104 are also shown in step S630.

[0225] The control unit 104 is further configured to perform a path rewriting operation on the sampled second new node, select a node with the smallest path cost as a parent node for the sampled second new node, obtain the parent node of the sampled second new node, and the path cost of the sampled second new node; then, perform a reconnection operation on the path of the sampled second new node. The specific functions and processing of the control unit 104 are also shown in step S640.

[0226] The control unit 104 is further configured to add the sampled second new node to the initial path and update the key points of the initial path to obtain an updated key point set when the path cost of the sampled second new node is reduced. The specific functions and processing of the control unit 104 are also shown in step S650.

[0227] The control unit 104 is further configured to iterate in this way until a preset maximum number of iterations is reached, and then use the path formed by the updated key point set as the optimized path of the initial path. The specific functions and processing of the control unit 104 are also shown in step S660.

[0228] like Fig.11 As shown in FIG. 1 , a flowchart of the path key point update operation includes:

[0229] Step 31, receiving all key points on the initial obstacle avoidance path extracted from steps 21 to 29, and then executing step 32.

[0230] Step 32: All key points on the initial obstacle avoidance path extracted from steps 21 to 29 are combined and output as the initial obstacle avoidance path key point set P. key (V k ,E k ), where V k ∈S free is the set of key points, E k ∈S free is a set of edges connecting two key points, and then step 33 is executed.

[0231] Step 33: Sample new nodes in the space around the key point, and then execute step 34.

[0232] In step 33, new nodes are sampled in the space around the key points. Specifically, a key point set P is randomly selected. key (V k ,E k ) is a key point q k As the sampling reference point, the spherical space R with a radius of r around it space ∈S as the sampling space, randomly obtain new sampling points, and generate new nodes q knew .

[0233] For the new node q k_new , search for the closest key point q in the key point set k_nearest , if the new node q k_new and key point q k_nearest If collision detection is passed between them, the key point q k_nearest As a new node q k_new The parent node of , and calculate the new node q k_new The cost of q k_cost , calculated as:

[0234]

[0235] in, The key point q k_nearest The cost, d(q k_nearest ,q k_new ) represents the new node q k_new and key node q k_nearest The Euclidean distance between .

[0236] Step 34: Path rewriting operation, select a parent node with a small cost for the new node, and then execute step 35.

[0237] In step 34, obtain node q k_new After the path cost of node q k_new Perform a path rewrite operation and select a parent node with a lower cost for it, with node q k_new The center is r and the radius is s , search for key points within the range, if there is a key point q k_rewrite , so that the node q is calculated according to formula (3) k_new After the cost, the cost becomes smaller, and the node q k_new and q k_rewrite If the collision detection is passed, the node q is modified. k_new The parent node is q k_rewrite .

[0238] Step 35: Path reconnection operation, calculate the path cost after the new node is added, and then execute step 36.

[0239] In step 35, determine the node q k_new After the parent node and path cost of the node q, the path is reconnected. k_new The center is r and the radius is s , search for key points within this range.

[0240] Step 36, determine whether the path cost is reduced: if so, execute step 37, otherwise return to step 33.

[0241] Step 37: Add the new node to the path, update the key points of the path, and then execute step 38.

[0242] After searching for key points within the range in step 35, if the key point q k_relink Take node q k_new After becoming the parent node, its path cost decreases, so the node q k_new As the key point q k_relink The parent node of joins the key point set P key (V k ,E k ), discard the key point q k_relink The original parent node is used to continuously reduce the path cost.

[0243] Step 38: Repeat steps 33 to 38 until the maximum number of iterations is reached, and output the key point set as the final optimized path.

[0244] In order to solve the problem that the biased sampling method loses the optimal solution, resulting in a high cost of the final generated path, which affects production efficiency, the solution of the present invention obtains the key points of the path by trimming the initial path, and then randomly samples the space around the key points, continuously updates the key points or inserts new path points to reduce the path cost, thereby obtaining the optimal or near-optimal path. In the solution of the present invention, a method of randomly sampling new nodes to iterate and update the key points of the initial obstacle avoidance path is adopted, which solves the problem that the path optimization method in the related solution loses the optimal solution and is prone to generate a high-cost optimized path, thereby reducing the initial path cost to obtain the optimal or near-optimal optimized path.

[0245] In the scheme of the present invention, see Fig.10 and Fig.11 In the example shown, the path key point extraction operation and the path key point update operation are mainly used to optimize the initial obstacle avoidance path obtained in the initial path planning operation. By iteratively updating the path key points, the high-cost path key points in the initial path (i.e., unnecessary turning points in the path) are replaced to continuously reduce the cost of the path, and finally obtain an optimal path from the starting point to the end point, thereby reducing the running time and energy consumption of the robot arm from the starting point to the end point and improving the execution efficiency of the robot arm.

[0246] In the scheme of the present invention, a bidirectional fast search random tree is first constructed to accelerate the path search efficiency, and then the high-cost path searched is replanned to obtain a low-cost path. Overall, the problems of low path search efficiency and poor path quality of the robot arm path planning method in related schemes in high-dimensional space and complex obstacle environments are solved, and a low-cost initial obstacle avoidance path can be quickly obtained.

[0247] Since the processing and functions implemented by the device of this embodiment basically correspond to the embodiments, principles and examples of the aforementioned method, for the details not fully described in this embodiment, please refer to the relevant descriptions in the aforementioned embodiments, and no further elaboration will be made here.

[0248] According to an embodiment of the present invention, a robot corresponding to the path determination device of a robot arm is also provided. The robot may include: the path determination device of the robot arm described above.

[0249] Since the processing and functions implemented by the robot of this embodiment basically correspond to the embodiments, principles and examples of the aforementioned device, for the details not fully described in this embodiment, please refer to the relevant descriptions in the aforementioned embodiments and will not be repeated here.

[0250] According to an embodiment of the present invention, a computer program product corresponding to the path determination method of a robot arm is also provided, including a computer program, which implements the steps of the path determination method of the robot arm described above when executed by a processor.

[0251] Since the processing and functions implemented by the product of this embodiment basically correspond to the embodiments, principles and examples of the aforementioned method, for the details not fully described in this embodiment, please refer to the relevant descriptions in the aforementioned embodiments, and no further elaboration will be made here.

[0252] According to an embodiment of the present invention, a storage medium corresponding to a path determination method for a robotic arm is also provided, wherein the storage medium includes a stored program, wherein when the program is running, the device where the storage medium is located is controlled to execute the steps of the path determination method for the robotic arm described above.

[0253] Since the processing and functions implemented by the storage medium of this embodiment basically correspond to the embodiments, principles and examples of the aforementioned method, for the details not fully described in this embodiment, please refer to the relevant descriptions in the aforementioned embodiments, and no further elaboration will be made here.

[0254] In summary, it is easy for those skilled in the art to understand that, under the premise of no conflict, the above-mentioned advantageous methods can be freely combined and superimposed.

[0255] The above description is only an embodiment of the present invention and is 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 substitution, improvement, etc. made within the spirit and principle of the present invention shall be included in the scope of the claims of the present invention.

Claims

1. A method for determining a path of a robot arm, characterized in that: Applied to planning the path of the robot's manipulator in the workspace; The path determination method of the robot arm comprises: Acquire the working space of the robot arm; and acquire the starting point and the target point pre-set in the working space of the robot arm as the starting point and the target point of the path of the robot arm; According to the starting point and the target point of the path, estimating the path cost of the path of the robot arm and determining the path cost threshold of the path of the robot arm; Bidirectionally constructing a fast search random tree from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold; Extracting key points of the initial path; The key points of the extracted initial path are updated to obtain an optimized path of the initial path as the path of the robotic arm, so as to determine the path of the robotic arm.

2. The method for determining the path of a robot arm according to claim 1, characterized in that: According to the starting point and the target point of the path, estimating the path cost of the path of the robot arm and determining the path cost threshold of the path of the robot arm includes: Calculate the Euclidean distance between the starting point and the target point of the path; estimating a path cost of the path of the robot arm according to a Euclidean distance between a starting point and a target point of the path to obtain an estimated path cost; The product value of the estimated path cost and a preset cost coefficient is determined as a threshold of the estimated path cost, which is used as a path cost threshold of the path of the robot arm.

3. The method for determining the path of a robot arm according to claim 1 or 2, characterized in that: A fast search random tree is constructed bidirectionally from the starting point and the target point of the path to obtain an initial path that meets the path cost threshold, including: Using the RRT algorithm, a fast search random tree is constructed from the starting point of the path, and the obtained fast search random tree is recorded as a first search random tree; and a fast search random tree is constructed from the target point of the path, and the obtained fast search random tree is recorded as a second search random tree; Adopting the target bias sampling method, alternately expanding the nodes of the first search random tree and the nodes of the second search random tree, so that the first search random tree grows in the direction of the target point of the path, and the second search random tree grows in the direction of the starting point of the path, and a new node is generated, which is recorded as the generated first new node; Based on the generated first new node, node distance calculation and collision detection are performed in the first search random tree and the second search random tree respectively to obtain an initial path that meets the path cost threshold.

4. The method for determining the path of a robot arm according to claim 3, characterized in that: Based on the generated first new node, node distance calculation and collision detection are performed in the first search random tree and the second search random tree respectively to obtain an initial path that meets the path cost threshold, including: Based on the generated first new node, searching the first search random tree and the second search random tree for a node that is closest to the generated first new node as a parent node of the generated first new node; When the generated first new node and the parent node of the generated first new node pass a preset collision detection, determine the cost of the generated first new node, and add the generated first new node to the corresponding search random tree in the first search random tree and the second search random tree to obtain the added first new node; Based on the first new node added, searching the first search random tree and the second search random tree for a node closest to the generated first new node, taking the node closest to the first new node added as the node closest to the first new node added, and recording the node as the closest node to the first new node added; When the distance between the first new node added and the nearest node of the first new node added is less than a preset connection threshold, and the line between the first new node added and the nearest node of the first new node added passes a preset collision detection, it is considered that the first search random tree meets the second search random tree, and an initial search path from the starting point of the path to the target point is obtained; Calculating the cost of searching the initial path, and determining whether the cost of searching the initial path is less than the path cost threshold; If it is determined that the cost of the searched initial path is less than the path cost threshold, the searched initial path is used as the initial path that meets the path cost threshold; If it is determined that the cost of the initial search path is greater than or equal to the path cost threshold, return to reuse the RRT algorithm, build a fast search random tree from the starting point of the path, and record the obtained fast search random tree as the first search random tree; and build a fast search random tree from the target point of the path, and record the obtained fast search random tree as the second search random tree, so as to redetermine the initial path that meets the path cost threshold.

5. The method for determining the path of a robot arm according to any one of claims 1 to 4, characterized in that: Extracting the key points of the initial path includes: Taking the initial path as the original path, calculating the vertical distance from each node in the original path to the line connecting the starting point and the target point of the path, so as to determine the farthest node and distance, which are recorded as the farthest node and the farthest distance; When the farthest distance is less than or equal to a preset distance threshold, the path between the starting point and the target point of the path is used as a simplified path; In the case where the farthest distance is greater than a preset distance threshold, the original path is divided into two segments from the farthest node, each of the two segments is used as a new original path, and returned to recalculate the vertical distance from each node in the new original path to the line connecting the starting point and the target point of the path, so as to obtain two simplified paths; the two simplified paths are merged to obtain a merged path, and the merged path is used as the simplified path if the merged path passes the preset collision detection; In the case where the simplified path passes the preset collision check, the nodes in the simplified path are used as the extracted key points; in the case where the simplified path does not pass the preset collision check, the nodes of the original path are used as the extracted key points; All the extracted key points are merged as the key points of the initial path obtained in advance.

6. The method for determining the path of a robot arm according to any one of claims 1 to 5, characterized in that: Updating the extracted key points of the initial path to obtain an optimized path of the initial path includes: Based on the extracted key points of the initial path, construct a set of the extracted key points of the initial path, recorded as a key point set; and construct a set of edges in the key point set for connecting two key points, recorded as an edge set; Based on the key point set and the edge set, for each key point in the key point set, within the range of the edge set, a new node is sampled in the surrounding space of each key point, and recorded as a second new node sampled; When the sampled second new node passes a preset collision detection with the corresponding key point, the corresponding key point is used as the parent node of the sampled second new node, and the cost of the sampled second new node is calculated; Performing a path rewriting operation on the sampled second new node, selecting a node with the smallest path cost as a parent node for the sampled second new node, obtaining the parent node of the sampled second new node and the path cost of the sampled second new node; then, performing a reconnection operation on the path of the sampled second new node; When the path cost of the sampled second new node is reduced, the sampled second new node is added to the initial path, and the key points of the initial path are updated to obtain an updated key point set; The process is repeated until a preset maximum number of iterations is reached, and then the path formed by the updated key point set is used as the optimized path of the initial path.

7. A path determination device for a robot arm, characterized in that: Applied to planning the path of the robot's manipulator in the workspace; The path determination device of the robot arm comprises: An acquisition unit is configured to acquire a working space of the robot arm; and acquire a starting point and a target point pre-set in the working space of the robot arm as a starting point and a target point of a path of the robot arm; A control unit configured to estimate a path cost of the path of the robot arm and determine a path cost threshold of the path of the robot arm according to a starting point and a target point of the path; The control unit is further configured to construct a fast search random tree from a starting point and a target point of the path in both directions to obtain an initial path that meets the path cost threshold; The control unit is further configured to extract key points of the initial path; The control unit is further configured to update the key points of the extracted initial path to obtain an optimized path of the initial path as the path of the robotic arm, so as to determine the path of the robotic arm.

8. A robot, characterized in that: include: The path determination device for a robot arm as claimed in claim 7.

9. A storage medium, characterized in that: The storage medium includes a stored program, wherein when the program is executed, the device where the storage medium is located is controlled to execute the path determination method for a robot arm according to any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the path determination method of the robot arm according to any one of claims 1 to 6 are implemented.

Images