A path planning method, system, and robotic arm for a robotic arm.

Abstract

This application relates to a path planning method, system, and robotic arm for a robotic arm. The method includes: acquiring obstacle point location information from a preset rasterized map; planning a collision-free path for the robotic arm's end effector using a graph search algorithm; detecting whether the robotic arm will collide or encounter unreachable points while moving along the collision-free path; if so, adding the node to the obstacle point location information; if not, performing trajectory planning on the collision-free path to generate the robotic arm's motion trajectory; detecting whether the robotic arm will collide or encounter unreachable points while moving along the trajectory; if so, adding the node to the obstacle point location information; wherein, if the obstacle point location information is updated, the collision-free path is replanned. This application provides a theoretical extension of the JPS algorithm for three-dimensional scenes. Furthermore, based on the search characteristics of the JPS algorithm, a complete planning scheme suitable for robotic arms is developed, significantly improving planning efficiency compared to traditional path planning algorithms.

Description

Technical Field

[0001] This application relates to the field of automated robots, and in particular to a path planning method, system and robotic arm for a robotic arm. Background Technology

[0002] The mainstream methods for industrial robot path planning remain limited to manual teaching and offline programming. The former requires manual on-site demonstration of points and writing motion programs; its advantage is speed and convenience, but its disadvantages include being cumbersome, inefficient, inaccurate, and difficult to find the optimal solution. The latter generates motion commands through upper-level software and simulation systems, requiring precise environmental modeling. Both methods incur high deployment costs. Furthermore, some industries with rapid product iteration cycles have high demands for automated deployment and flexible production. Traditional deployment methods are not suitable for the need for rapid product adjustments, resulting in low levels of automation in these industries and significantly limiting their production capacity.

[0003] To address the shortcomings of traditional methods, related research is also attempting to resolve the inconveniences they bring. In recent years, intelligent algorithms and optimization strategies, such as the probability-based Rapidly-exploring Random Trees (RRT) algorithm, the iterative optimization-based ant colony algorithm, and the machine vision-based improved RRT planning algorithm, have been applied in related research. However, these methods have certain deficiencies in terms of completeness and efficiency, and cannot meet the requirements of industrial sites. Summary of the Invention

[0004] In order to plan the path of a robotic arm more efficiently, this application provides a robotic arm path planning method, system and robotic arm.

[0005] Firstly, the robotic arm path planning method provided in this application adopts the following technical solution:

[0006] A robotic arm path planning method, comprising:

[0007] Obtain obstacle location information from a preset rasterized map;

[0008] Based on the required start and end points and the obtained obstacle point location information, a collision-free path is planned for the end effector of the robotic arm using a graph search algorithm;

[0009] Detect whether the robotic arm will collide or encounter unreachable points when moving along a collision-free path;

[0010] If the robotic arm encounters a collision or has an unreachable point while moving along a collision-free path, then that node is added to the obstacle point location information.

[0011] If not, the planned collision-free path will be used for trajectory planning to generate the motion trajectory of the robotic arm;

[0012] Detect whether the robotic arm will collide or encounter unreachable points when moving along this trajectory;

[0013] If the robotic arm collides or encounters an unreachable point while moving along this trajectory, the node is added to the obstacle location information.

[0014] If the obstacle location information is updated, a collision-free path is replanned using a graph search algorithm.

[0015] By adopting the above technical solution, the collision-free path of the end effector can be quickly planned by leveraging the advantages of rapid planning and low resource consumption of graph search algorithm. Then, the collision or reachability of other links of the overall robotic arm is detected, and obstacle point information is continuously added to the gridded map. Through iterative methods, a collision-free motion trajectory that the robotic arm can run is quickly planned.

[0016] Preferably, the process of planning a collision-free path in a graph search algorithm includes:

[0017] Search for jump nodes in a preset search order and according to jump node search rules;

[0018] During the process of finding jump nodes, neighboring nodes are pruned according to preset pruning rules and forced neighbor nodes are marked; a collision-free path is planned based on the obtained jump nodes and forced neighbor nodes.

[0019] By adopting the above technical solution, the pruning rules can reduce unnecessary node expansion during the jump node search process, effectively filtering out nodes and making the planning of collision-free paths more efficient.

[0020] Preferred, preset pruning rules include:

[0021] If the direction from parent node p(x) to current node x is a straight line, then in the path from parent node p(x) to current node x, if len(〈p(x),…,n〉\x)≤len(〈p(x),x,n〉), then node n is a neighbor node that needs to be pruned.

[0022] If the direction in which the parent node p(x) moves to the current node x is an oblique line in any plane, then in the path from the parent node p(x) to the current node x, if len(〈p(x),…,n〉\x) <len(<p(x),x,n> If n is a neighboring node that needs pruning, then node n is the node that needs pruning.

[0023] Among them, for grid regions that cannot be reached from parent node p(x) in one step, if reaching parent node p(x) requires crossing a plane, then the current node x must be passed when calculating the path length;

[0024] If the direction in which the parent node p(x) moves to the current node x is any three-dimensional diagonal line, then in the path from the parent node p(x) to the current node x, if len(<p(x),…,n> \x) <len(<p(x),x,n> If a node n is a neighboring node that needs pruning, then that node is a neighboring node that needs pruning.

[0025] In the case of a grid region that is not reachable from the parent node p(x) in one step, if the path to the grid point passes through the edges of the xy, yz, and zx planes, and these edges all pass through the parent node p(x), then the current node x must be passed through when calculating the path length.

[0026] In the formula, p(x) is the parent node, n is the node to be judged, x is the current node, and len(<p(x),…,n> \x) represents the path length from parent node p(x) through all points to node n, but without passing through node x, len(<p(x),x,n> ) represents the path length from parent node p(x) through node x to node n.

[0027] Preferably, the rule for marking a mandatory neighbor node needs to satisfy the condition that the node is not a neighbor node that needs to be pruned by the current node x, and also satisfy len(<p(x),…,n> \x) <len(<p(x),x,n> ).

[0028] Preferably, when starting from node x and using The direction reaches node y, and The node y that minimizes the value of k in the equation is a jump node, and at least satisfies:

[0029] Node y is the target node; or,

[0030] Node y has at least one neighbor node that is a mandatory neighbor node; or,

[0031] In the diagonal movement direction, there exists a node z that can be reached by moving several steps along the same straight line from node y, which is the target node or at least one of its neighbor nodes that is a forced neighbor node.

[0032] Preferably, before detecting whether a collision or an unreachable point will occur when the robotic arm moves along a collision-free path, the method further includes path optimization of the generated collision-free path;

[0033] The path optimization method includes:

[0034] Obtain the free path nodes between two adjacent nodes on the collision-free path;

[0035] Connect the adjacent free path nodes to each other to form an extended collision-free path.

[0036] By adopting the above technical solution, affected by the foregoing collision-free path planning algorithm, the number of nodes actually forming the collision-free path is small, and the small number of nodes cannot well meet the number of path points required for the movement of the robotic arm. Moreover, too few path points will cause a large deviation in the path point fitting during the trajectory planning of the robotic arm. Therefore, through this extended optimization method, the number of nodes can be effectively increased on the premise of meeting collision-free to meet the subsequent required requirements.

[0037] Preferably, after forming the extended collision-free path, the method further includes:

[0038] Select three adjacent free path nodes between two adjacent nodes of the extended collision-free path in sequence for screening and iteration. The three adjacent free path nodes are defined as p1, p2, and p3 in sequence;

[0039] During the screening process, if dist(p1, p3) < dist(p1, p2) + dist(p2, p3), then remove the p2 point, and set the current p1 point as the p3 point of the previous iteration for iteration;

[0040] If dist(p1, p3) ≥ dist(p1, p2) + dist(p2, p3), then set the current p1 point as the p2 point of the previous iteration for iteration;

[0041] Connect the remaining adjacent free path nodes to each other and update the collision-free path;

[0042] where, dist(p i , p j ) represents the distance between point p i and point p j .

[0043] By adopting the above technical solution, this method can remove the right-angle corners formed between the extended collision-free paths, making the collision-free path become a relatively smooth path.

[0044] Preferably, during each update process of the obstacle point position information, judge whether the collision-free path reaches the maximum planning threshold times. If so, stop the planning of the collision-free path.

[0045] By adopting the above technical solution, it can effectively avoid the situation where the process cannot end due to an infinite loop.

[0046] Secondly, the robotic arm path planning system provided in this application adopts the following technical solution:

[0047] A robotic arm path planning system, comprising:

[0048] The initialization module is used to initialize the raster map and obtain the location information of obstacle points in the preset raster map;

[0049] The collision-free path planning module is used to plan a collision-free path using a graph search algorithm based on the required start and end points and the obtained obstacle point location information.

[0050] The collision-free path detection module is used to detect whether the robotic arm will collide or encounter unreachable points when moving along a collision-free path; if the robotic arm will collide or encounter unreachable points when moving along a collision-free path, the node will be added to the obstacle point location information.

[0051] If not, the planned collision-free path is planned using the robotic arm trajectory planning module to generate the robotic arm's motion trajectory.

[0052] The robotic arm trajectory detection module is used to detect whether the robotic arm will collide or encounter unreachable points when moving along the trajectory.

[0053] If the robotic arm collides or encounters an unreachable point while moving along this trajectory, the node is added to the obstacle location information.

[0054] If the obstacle location information is updated, a collision-free path is replanned using a graph search algorithm.

[0055] Thirdly, the robotic arm provided in this application adopts the following technical solution:

[0056] A robotic arm for executing a motion trajectory planned by the above method.

[0057] In summary, this application provides a theoretical extension of the JPS algorithm for 3D scenes. Furthermore, based on the search characteristics of the JPS algorithm, a complete planning scheme suitable for robotic arms was developed, significantly improving planning efficiency compared to traditional path planning algorithms. Attached Figure Description

[0058] Figure 1 This is a flowchart illustrating a robotic arm path planning method according to one embodiment of the present invention.

[0059] Figure 2 This is a pruning diagram in one embodiment of the present invention when the direction of the parent node moving to the current node is a straight line, wherein the direction pointed to by the arrow is the direction of movement from the parent node to the current node.

[0060] Figure 3 This is a pruning diagram of one embodiment of the present invention when the direction of the parent node moving to the current node is an oblique line in any plane, wherein the direction pointed to by the arrow is the movement direction from the parent node to the current node.

[0061] Figure 4 This is a pruning diagram of an embodiment of the present invention when the direction in which the parent node moves to the current node is any three-dimensional diagonal line, wherein the direction pointed to by the arrow is the movement direction from the parent node to the current node.

[0062] Figure 5 This is a schematic diagram of an extended collision-free path according to one embodiment of the present invention, wherein the left side is the collision-free path and the right side is the extended collision-free path.

[0063] Figure 6 Yes Figure 5 The example provided illustrates the extended collision-free path after removing the right-angle turning point. The left side shows the extended collision-free path, and the right side shows the collision-free path after removing the right-angle turning point. Detailed Implementation

[0064] The following is in conjunction with the appendix Figure 1-6 This application will be described in further detail.

[0065] This application discloses a path planning method for a robotic arm. The method mainly uses a graph search algorithm to plan the path of the robotic arm. After planning a collision-free path for the end effector, it considers whether the entire robotic arm will collide. This minimizes the overhead of solving the forward and inverse kinematics of the robotic arm and the collision detection process caused by planning in Cartesian space. The method iteratively searches for effective paths and then performs trajectory planning to generate an executable file for the robotic arm.

[0066] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0067] Furthermore, the term "and / or" in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, or B existing alone. Additionally, the character " / " in this article, unless otherwise specified, generally indicates that the preceding and following related objects have an "or" relationship.

[0068] The embodiments of the present invention will now be described in further detail with reference to the accompanying drawings.

[0069] Reference Figure 1 This invention provides a robotic arm path planning method, the main process of which is described below.

[0070] Step S100: Initialize the rasterized map.

[0071] In this embodiment, the rasterized map used is the Octomap rasterized map. Initializing the Octomap rasterized map requires three prerequisites: setting up the Gazebo simulation environment, acquiring environmental point cloud data using Intel RealSense, and generating the Octomap map. After setting up the Gazebo simulation environment based on the real-world working conditions, communication is established between ROS and Gazebo. The robotic arm model described by URDF and the Intel RealSense model are published in the Gazebo environment. Communication between Gazebo and Rviz is established for visualization, acquiring real-time point cloud data. This point cloud data is provided to Moveit's PlanningSense service. The filtered point cloud is used to construct the Octomap environmental obstacle map. The Octomap map is saved as a local .bt format file. Initializing the Octomap map involves loading the local .bt format map file through a self-written map publishing service node.

[0072] To determine the location of obstacles in a rasterized map, point cloud data of obstacles in the environment can be generated by using a depth camera. This point cloud data can then be transformed using Octomap and recorded in the nodes of the rasterized map in the form of an octree.

[0073] Octomap employs an octree structure to manage point cloud information and can dynamically update the observed environment. Due to its pruning strategy and the storage advantages of the octree structure, a rasterized map with millions of nodes uses less than 1MB of memory. The use of an unordered hash table for node storage also results in extremely fast node access. Using Octomap to build rasterized environment maps also significantly improves the efficiency of path planning.

[0074] Step S200: Obtain obstacle point location information in the preset raster map.

[0075] The obstacle location information refers to the specific coordinates of the obstacle point in the rasterized map. Here, the obstacle location information can be expanded by inflating the nodes containing the obstacle; in this case, additional nodes along the edge of the obstacle are also defined as obstacle location information. This process allows for the planning of collision-free paths that avoid obstacles.

[0076] Here, the Octomap-related dynamicEDT3D function library can be used to preprocess the original Octomap data, setting all grid points within the expansion coefficient values ​​on the obstacle edges as obstacle points, and regenerating the Octomap raster map used for planning.

[0077] Step S300: Based on the required start point position, end point position, and the obtained obstacle point position information, a collision-free path is planned for the end effector of the robotic arm using a graph search algorithm.

[0078] Here, before planning a collision-free path for the end effector of the robotic arm using a graph search algorithm, it is necessary to obtain the initial pose of the robotic arm and set the target pose. However, in this step, a collision-free path is only planned for the location of the end effector.

[0079] Specifically, in this embodiment of the application, the graph search algorithm preferably adopts the JPS algorithm, but any known graph search-based algorithm can also be used for path planning. The JPS algorithm is a specific method that continuously prunes nodes that do not need expansion during the search for skip nodes to reduce the computational load. Its specific implementation includes the following steps:

[0080] Step S310: Search for jump nodes according to the preset search order and the jump node search rules.

[0081] Step S320: During the process of finding jump nodes, prune neighboring nodes according to preset pruning rules and mark forced neighboring nodes.

[0082] Step S330: Plan a collision-free path based on the obtained jump nodes and forced neighbor nodes.

[0083] In step S320, during each search for a jump node, the pruning of neighboring nodes is based on the association of the nodes surrounding the current node, the parent node (i.e., the previous node), the current node's location, and the obstacle's location. The corresponding pruning involves removing irrelevant neighboring nodes and marking the necessary forced neighboring nodes. The search for jump nodes aims to mark the necessary locations traversed by the end effector. For each jump node search, one node is moved along one direction to determine if the current node is a jump node. The preset search order can be to first search along the x-axis, y-axis, z-axis, xy-plane, yz-plane, or zx-plane. When an obstacle is encountered during the search, another node is moved along the xyz diagonal direction to continue searching along the x-axis, y-axis, z-axis, xy-plane, yz-plane, or zx-plane.

[0084] Here, the forced neighbor node and the skip node are the node positions that need to be traversed during the collision-free path planning in step S330. Corresponding to the coordinate system constructed by the grid map, each node has a unique node coordinate, and the current node, parent node, and the marked skip node and forced neighbor node also have a unique node coordinate.

[0085] In a 3D rasterized coordinate system, each node has at most 26 neighboring nodes (i.e., all nodes within a 3x3x3 area excluding the center node). There are three ways to move from the parent node to the current node: one is to move horizontally along the coordinate system direction to the neighboring node, with each movement being a distance of one; another is to move diagonally along a plane to the neighboring node, with each movement being a distance of... Another type involves oblique movement with displacement in each dimension, moving to neighboring nodes, with each movement covering a distance of...

[0086] For specific JPS pruning rules, the supply and demand under the constraint of three-dimensional rasterized coordinates include:

[0087] If the direction from parent node p(x) to current node x is a straight line, then in the path from parent node p(x) to current node x, if len(<p(x),…,n> \x)≤len(<p(x),x,n> If ), then node n is a neighboring node that needs pruning.

[0088] Wherein, p(x) is the parent node, n is the node to be judged, x is the current node, len(<p(x),…,n>\x) represents the path length from the parent node p(x) through each point to the node n but not through the node x, and len(<p(x),x,n>) represents the path length from the parent node p(x) through the node x to the node n.

[0089] Condition 2:

[0090] If the direction from the parent node p(x) to the current node x is an oblique line in any plane, then in the path from the parent node p(x) to the current node x, if len(<p(x),…,n>\x) < len(<p(x),x,n〉), then the node n is a neighbor node that needs to be pruned.

[0091] Among them, based on Condition 2, it also needs to be further defined through Definition 1. Definition 1 is: When reaching the lattice point area that the parent node p(x) cannot reach in one step from the parent node p(x), if it is necessary to cross a plane from the parent node p(x) to reach the parent node p(x), then the current node x must be passed when calculating the path length.

[0092] Condition 3:

[0093] If the direction from the parent node p(x) to the current node x is any three-dimensional oblique line direction, then in the path from the parent node p(x) to the current node x, if len(<p(x),…,n>\x) < len(<p(x),x,n>), then the node n is a neighbor node that needs to be pruned.

[0094] Among them, based on Condition 3, it also needs to be further defined through Definition 2. Definition 2 is:

[0095] When reaching the lattice point area that the parent node p(x) cannot reach in one step from the parent node p(x), if the path to reach this lattice point passes through the edges where the xy, yz, and zx planes intersect, and these edges all pass through the parent node p(x), then the current node x must be passed when calculating the path length.

[0096] The plane crossing in Definition 1 and Definition 2 means: Assume that the current expansion is along the xy plane direction and assume that the z-axis coordinate of the current plane is 0. Assume that the middle layer of the 3×3×3 cube is z = 0, the top layer is z = 1, and the bottom layer is z = -1. If there is a lattice point z = +1 in the path when calculating the path length, then it is defined that the path has a plane crossing behavior when reaching the lattice point that the parent node p(x) cannot reach in one step from the parent node p(x).

[0097] For the limitation of the forced neighbor node, it needs to satisfy Definition 3:

[0098] This node is not a neighbor node that needs to be pruned by the current node, and satisfies len((p(x), ..., n>\x)<len((p(x), x, n)).

[0099] Reference Figure 2 Under the condition of satisfying condition 1, assume the coordinates of the current node x are (x i ,y i ,z i Then the coordinates of its parent node p(x) are (x... i-1 ,y i ,z i If the positional difference between the two is (Δx = ±1, Δy = 0, Δz = 0), then pruning is performed along the x-axis. If there are no obstacle nodes among the neighboring nodes, then under condition 1, 25 neighboring nodes will be pruned, leaving (x i+1 ,y i ,z i The natural node of ). If at coordinate (x i ,y i ,z i-1 If there is an obstacle node at position (x), then according to Definition 3, the coordinates can be obtained as (x...). i+1 ,y i ,z i-1 The node at coordinates (x) is a forced neighbor node. i ,y i-1 ,z i-1 If there is an obstacle node at position (x), then according to Definition 3, the coordinates can be obtained as (x...). i+1 ,y i-1 ,z i-1 The node at coordinates (x) is a forced neighbor node. i ,y i-1 ,z i-1 ) and (x i ,y i-1 ,z i There is an obstacle node at each of the following locations: (x) and (x - y). Therefore, according to Definition 3, the coordinates can be obtained as (x - y). i+1 ,y i-1 ,z i-1 (x) and coordinates (x) i+1 ,y i-1 ,z i All nodes are forced neighbor nodes.

[0100] Reference Figure 3 Under the condition of satisfying condition 2, assume the coordinates of the current node x are (x i ,y i ,z iThen the coordinates of its parent node p(x) are (x... i-1 ,y i-1 ,z i From this, we can see that the pruning process takes place in the xy plane. Therefore, under condition 2, (x i+1 ,y i+1 ,z i ), (x i ,y i+1 ,z i ), (x i+1 ,y i ,z i All are natural nodes. For example, if the coordinates are (x... i ,y i-1 ,z i If there is an obstacle node at position (x), then according to Definition 3, the coordinates can be obtained as (x...). i+1 ,y i-1 ,z i At coordinates (x) is a forced neighbor node. i ,y i ,z i+1 If there is an obstacle node at position (x), then according to Definition 1 and Definition 3, we can obtain (x) i+1 ,y i ,z i+1 ), (x i ,y i+1 ,z i+1 ), and (x i+1 ,y i+1 ,z i+1 For forced neighbor nodes, here, relative to pruning in the xy plane, the obstacle node on the z-axis has already caused it to cross the plane. If at coordinates (x... i ,y i-1 ,z i ) and (x i ,y i-1 ,z i+1 There is an obstacle node at each of the following locations: (x) and (x - y). Therefore, according to Definition 1 and Definition 3, the coordinates can be obtained as (x - y). i+1 ,y i-1 ,z i ) and (x i+1 ,y i-1 ,z i+1 The node at position ) represents a forced neighbor node;

[0101] Reference Figure 4 Under the condition of satisfying condition 3, assume the coordinates of the current node x are (x i ,y i ,z iThen the coordinates of its parent node p(x) are (x... i-1 ,y i-1 ,z i-1 From this, we can see that the pruning process takes place in the xyz direction. Therefore, under condition 3, (x i+1 ,y i+1 ,z i ), (x i ,y i+1 ,z i ), (x i+1 ,y i ,z i ), (x i+1 ,y i+1 ,z i+1 ), (x i+1 ,y i ,z i+1 ), (x i ,y i+1 ,z i+1 ), (x i ,y i ,z i+1 All are natural nodes. For example, if the coordinates are (x... i ,y i-1 ,z i If there is an obstacle node at position (x), then according to Definition 2 and Definition 3, we can obtain (x) i ,y i-1 ,z i+1 ), (x i+1 ,y i-1 ,z i ), and (x i+1 ,y i-1 ,z i+1 The grid point at coordinates (x) is a forced neighbor node. i ,y i-1 ,z i-1 If there is an obstacle node at position (x), then according to Definition 2 and Definition 3, we can obtain (x) i+1 ,y i-1 ,z i-1 The grid point at () is a forced neighbor node.

[0102] Therefore, as can be seen from the above examples, in a three-dimensional rasterized coordinate path, by defining 1, 2, 3 and conditions 1, 2, 3, we can prune the surrounding neighboring nodes during the search for jump points and mark the existing forced neighboring nodes.

[0103] The selection of the jump node must satisfy the following condition: when starting from node x and... The direction reaches node y, and The node y that minimizes the value of k in the equation is a jump node, and at least satisfies:

[0104] Node y is the target node; or,

[0105] Node y has at least one neighbor node that is a mandatory neighbor node; or,

[0106] In the diagonal movement direction, there exists a node z that can be reached by moving several steps along the same straight line from a target node y or a node y with at least one neighbor node that is a forced neighbor node. It represents any one of the 26 directions for moving to a neighboring node.

[0107] Therefore, if a node exists in the diagonal movement direction, and can reach the target node by moving a distance of several nodes in a straight line, or has at least one neighbor node that is a forced neighbor node, then this node also needs to be determined as a jump node. Here, the target node refers to the endpoint of the jump node search.

[0108] During the search for jump nodes, the search proceeds in the order specified by the search directions. If the direction is a straight line, the search continues along that direction. If a jump node is found, it is added to the corresponding repository. If an obstacle or boundary is encountered during the search, it indicates that extending the search in a straight line from the current node along that direction is worthless. In this case, the search continues with a different direction based on the last jump node found. However, if the direction is diagonal, it is decomposed into vertical and horizontal directions for searching. If searching in these two straight directions is worthless, the search continues to the next diagonal grid point. For 3D scenes, if the diagonal vector direction is 3D, it is decomposed into a 2D diagonal vector direction and a 1D straight vector direction. The 2D diagonal vector direction is further decomposed into a 1D straight vector direction for searching.

[0109] After a complete search of jump nodes, the locations of all jump nodes and forced neighbor nodes can be obtained. Connecting them in order will yield a collision-free path suitable for the current obstacle location information.

[0110] Step S400: Optimize the generated collision-free path.

[0111] Here, for the collision-free path generated in step S300, the large span between two nodes (between jump nodes, between forced neighbor nodes and jump nodes, or between forced neighbor nodes) and the small number of nodes after collision-free path planning make it difficult to directly meet the required number of path points for the robotic arm's movement. Furthermore, too few path points can lead to significant deviations during the subsequent fitting process. Therefore, it is necessary to add nodes between the selected nodes to eliminate these effects. However, under ideal conditions, step S400 can be omitted.

[0112] Therefore, specific optimization methods include:

[0113] Step S410: Obtain the free path nodes between two adjacent nodes on the collision-free path.

[0114] Among them, a free path node refers to a non-obstacle node located between two nodes on a collision-free path, which is connected to each other and will not pass through obstacle nodes. A non-obstacle node is a node other than the obstacle point information.

[0115] Step S420: Connect adjacent free path nodes to form an extended collision-free path.

[0116] Since the path obtained in step S300 is guaranteed to be a collision-free path, there will always be a path that forms the required extended collision-free path during the process of expanding it into an extended collision-free path. For example... Figure 5 The diagram illustrates a comparison between the collision-free path and the expanded collision-free path after increasing the number of nodes in one scenario. By introducing free-path nodes, it can be observed that the number of nodes satisfying the collision-free path requirement is increased. However, due to the limitations imposed by free-path nodes, the expanded collision-free path contains more right angles. The presence of these right-angled paths causes the trajectory motion to constantly undergo acceleration and deceleration, resulting in noticeable jerks in the robotic arm's movement, negatively impacting the machine and its execution efficiency. Therefore, these right-angled sections need to be smoothed.

[0117] However, during the smoothing process, the collision-free path after smoothing needs to be very similar to the original collision-free path. This is because the robotic arm's movement is a coupled motion of multiple joints and links. If the path after removing the right-angle points becomes a direct line connecting the starting point and the target point after optimization, even if there are no obstacles between the starting point and the target point, a link of the robotic arm may collide with other obstacles in space, or the robotic arm may be unreachable at a certain point on this path. This may cause the same path to be generated every time path planning is performed. Although the path planning shows success, the robotic arm cannot execute the corresponding path action.

[0118] Therefore, the specific processing methods include:

[0119] Step S430: Sequentially select three adjacent free path nodes between two adjacent nodes of the extended collision-free path for screening and iteration. The three adjacent free path nodes are sequentially defined as p1, p2, and p3.

[0120] Among them, during the first screening process, three adjacent free path nodes are selected from one end. After the first iteration is completed, one more free path node is selected backward, and the first free path node is discarded. Therefore, during multiple iterations, any three adjacent free path nodes will be sequentially subjected to the screening operation in step S440.

[0121] Step S440: During the screening process, if dist(p1, p3) < dist(p1, p2) + dist(p2, p3), then eliminate point p2, and set the current p1 point as the p3 point of the previous iteration for iteration.

[0122] If dist(p1, p3) ≥ dist(p1, p2) + dist(p2, p3), then set the current p1 point as the p2 point of the previous iteration for iteration.

[0123] Among them, dist(p i , p j ) represents the distance between point p i and point p j . This screening method uses the side length theorem of a triangle. When the situation described in step S440 occurs, it means that the position of point p2 is a right-angle turning point, so point p2 is about to be eliminated. And when the situation described in step S450 occurs, it means that p1, p2, and p3 may be on a straight line.

[0124] Step S460: Connect the remaining adjacent free path nodes to each other and update the collision-free path.

[0125] After eliminating the right-angle turning points, connecting the two adjacent points of this point will result in two turning points with angles greater than or equal to 135°.

[0126] As Figure 6 shown, based on the extended collision-free path with Figure 5 as the basis, the positions of the three right-angle turning points are eliminated, and at the same time, the overall trajectory does not change significantly. Therefore, the collision-free path obtained by this screening method can better eliminate the right-angle turning points in the collision-free path, which is beneficial for generating a smooth motion trajectory in step S700.

[0127] Step S500: Detect whether the robotic arm will collide or have unreachable points when moving along a collision-free path.

[0128] Although a collision-free path for the end effector is planned in step S400, the large space occupied by the robotic arm means that its remaining links may collide with obstacles or there may be nodes that the robotic arm cannot reach. Therefore, it is necessary to assess the robotic arm as a whole to detect whether collisions or unreachable points will occur when the robotic arm moves along the collision-free path.

[0129] Moveit can generate an ACM matrix for self-collision detection, simplifying the self-collision detection process. Using Moveit's integrated FCL library, AABB bounding box collision detection can be performed on the robotic arm, including collisions between the robotic arm and environmental obstacles, as well as self-collision detection. Self-collision detection requires inputting the joint angle values ​​of the robotic arm at the intended position, which can be obtained through IKFast inverse kinematics solving. Motion space rationality detection is performed by judging the inverse kinematics solution results of path points. If there is no valid inverse kinematics solution at a point, the point is considered unreachable by the current robotic arm; otherwise, a valid solution exists, and the point is considered reachable. If a valid solution exists but a collision occurs, the point is also considered unreachable.

[0130] Step S600: If the robotic arm will collide or has an unreachable point while moving along a collision-free path, then add the node to the obstacle point location information.

[0131] For nodes that may collide or have unreachable points, these points need to be designated as obstacle points and added to the obstacle point location information. Once the obstacle point location information is updated, the loop returns to step S300 to replan a new collision-free path. Similarly, the updated collision-free path still needs to be checked in step S500.

[0132] Step S700: If not, perform trajectory planning on the planned collision-free path to generate the motion trajectory of the robotic arm.

[0133] The TOPP-RA trajectory planning algorithm can be used to plan the motion trajectory of the robotic arm.

[0134] Step S800: Detect whether the robotic arm will collide or have unreachable points when moving along the motion trajectory.

[0135] Similarly, collision and non-reachable point detection are also required for the motion trajectory of the robotic arm generated in step S700. The specific detection method can be the same as that in step S500.

[0136] If, after executing step S800, the robotic arm moves along the planned trajectory without collisions or encountering unreachable points, then the planned trajectory is the desired trajectory that the robotic arm can execute. Here, the local parameters generated by TOPP-RA, including time, joint angles, joint angular velocities, and joint angular accelerations, are published to the Moveit trajectory controller via ROS to drive the robotic arm's movement in Gazebo. Upon reaching the target point, control commands are issued to the Vacuum Gripper suction cup to grasp the target object.

[0137] Step S900: If the robotic arm will collide or has an unreachable point while moving along the trajectory, then add the node to the obstacle point location information.

[0138] Similarly, after step S900, once the obstacle point position information is updated, the loop immediately returns to step S300 to replan a new collision-free path. Likewise, the updated collision-free path still needs to be checked in step S500, and after the robotic arm's motion trajectory is regenerated, it needs to be checked again in step S800.

[0139] In this method, to prevent infinite iterations, the number of times a collision-free path is planned in step S300 is recorded. When the maximum planning threshold number of collision-free paths is reached, the loop is forcibly stopped. In one embodiment, the overall planning time can also be timed to determine if it has reached the maximum limit. When the overall planning time reaches the preset maximum value, the loop is also forcibly stopped. This method effectively avoids the situation where the process cannot end in an infinite loop.

[0140] Here, after the loop ends, the collision-free trajectory of the last planned path can also be output to facilitate subsequent manual debugging and achieve the purpose of bypassing the currently planned path by setting multiple target points.

[0141] Based on the same inventive concept, this application also discloses a robotic arm path planning system, which includes:

[0142] The initialization module is used to initialize the raster map and obtain the location information of obstacle points in the preset raster map.

[0143] The collision-free path planning module is used to plan a collision-free path using a graph search algorithm based on the required start and end points and the obtained obstacle point location information.

[0144] The collision-free path optimization module is used to optimize the generated collision-free paths.

[0145] The collision-free path detection module is used to detect whether the robotic arm will collide or encounter unreachable points when moving along a collision-free path.

[0146] If the robotic arm encounters a collision or has an unreachable point while moving along a collision-free path, then that node is added to the obstacle location information.

[0147] If not, the robotic arm trajectory planning module is used to plan the trajectory of the planned collision-free path to generate the motion trajectory of the robotic arm.

[0148] The robotic arm trajectory detection module is used to detect whether the robotic arm will collide or encounter unreachable points when moving along the trajectory.

[0149] If the robotic arm collides with or encounters an unreachable point while moving along this trajectory, the node is added to the obstacle location information.

[0150] If the obstacle location information is updated, the collision-free path planning module will replan the collision-free path.

[0151] Based on the same inventive concept, embodiments of the present invention provide a robotic arm for performing tasks such as... Figure 1 A computer program for a robotic arm path planning method.

[0152] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional modules is used as an example. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. The specific working process of the system, device, and unit described above can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0153] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces, or indirect coupling or communication connection between apparatuses or units, and may be electrical, mechanical, or other forms.

[0154] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0155] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0156] The above description of the embodiments is only used to provide a detailed introduction to the technical solutions of this application. However, the description of the above embodiments is only for the purpose of helping to understand the method and core ideas of the present invention, and should not be construed as a limitation of the present invention. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this invention should be covered within the protection scope of this invention.

Claims

1. A method of robot path planning, characterized by, The method comprises the following steps: acquiring position information of obstacle points in a preset raster map; planning a collision-free path for an end effector of a robot arm by a graph search algorithm according to a required start position, an end position and the acquired position information of obstacle points; detecting whether a collision or an unreachable point will occur when the robot arm moves along the collision-free path; if a collision or an unreachable point will occur when the robot arm moves along the collision-free path, adding the node into the position information of obstacle points, the node being the node at which the collision or the unreachable point will occur; if not, planning a motion trajectory of the robot arm by trajectory planning based on the planned collision-free path; detecting whether a collision or an unreachable point will occur when the robot arm moves along the motion trajectory; if a collision or an unreachable point will occur when the robot arm moves along the motion trajectory, adding the node into the position information of obstacle points; wherein if the position information of obstacle points is updated, the collision-free path is re-planned by the graph search algorithm.

2. The method of claim 1, wherein, In the process of planning the collision-free path by the graph search algorithm, the following steps are included: finding a jump node according to a preset finding sequence and a jump node finding rule; pruning neighbor nodes and marking forced neighbor nodes according to a preset pruning rule during the process of finding the jump node; planning the collision-free path according to the acquired jump node and the forced neighbor nodes.

3. The method of claim 2, wherein, The preset pruning rule comprises the following steps: if the direction of movement of a parent node to a current node is a straight line, if len(<p,…,n>\x)≤len(<p,x,n>) in the path from the parent node to the current node, the node n is a neighbor node that needs to be pruned; if the direction of movement of a parent node to a current node is a diagonal line in any plane, if len(<p,…,n>\x)<len(<p,x,n>) in the path from the parent node to the current node, the node n is a neighbor node that needs to be pruned; wherein if the grid point region from a parent node p1(x) to a parent node p2(x) is not one-step reachable, if the parent node p1(x) needs to cross a plane to reach the parent node p2(x), the current node x must be passed when calculating the path length; if the direction of movement of a parent node to a current node is a diagonal line in any three-dimensional direction, if len(<p,…,n>\x)<len(<p,x,n>) in the path from the parent node to the current node, the node n is a neighbor node that needs to be pruned; wherein if the grid point region from a parent node p1(x) to a parent node p2(x) is not one-step reachable, if the path to the grid point passes through the edges intersecting the xy, yz and zx planes, and all the edges pass through the parent node p(x), the current node x must be passed when calculating the path length; wherein p is the parent node of the current node x, n is the node to be judged, x is the current node, len(<p,…,n>\x) represents the path length from the parent node to the node n without passing through the node x, and len(<p,x,n>) represents the path length from the parent node to the node n passing through the node x.

4. The method of claim 3, wherein, The rule of marking the forced neighbor node requires that the node is not a neighbor node that needs to be pruned by the current node, and that len(<p,...,n>\x)<len(<p,x,n>).

5. The method of claim 2, wherein, When arriving at node y from node x in the direction and the node y with the smallest value of k for the equation is the hop node and at least satisfies: The node y is a target node; or, The node y has at least one neighbor node as a forced neighbor node; or, In the oblique moving direction, there is a node z that can move from the node y as a target node or the node y with at least one neighbor node as a forced neighbor node along the same straight line direction for several steps.

6. The method of claim 1, wherein, Before detecting whether the robot arm will collide or have an unreachable point when moving along the collision-free path, the method further comprises path optimization on the generated collision-free path; The path optimization method comprises: Obtaining free path nodes between adjacent two nodes on the collision-free path; Connecting the adjacent free path nodes to form an extended collision-free path.

7. The method of claim 6, wherein, After forming the extended collision-free path, the method further comprises: Selecting three adjacent free path nodes between adjacent two nodes on the extended collision-free path in turn for screening and iteration, and the three adjacent free path nodes are defined in turn as p1, p2 and p3; In the screening process, if dist(p1, p3)<dist(p1, p2)+dist(p2, p3), p2 is removed, and the current p1 is set as p3 of the previous iteration for iteration; If dist(p1, p3)≥dist(p1, p2)+dist(p2, p3), the current p1 is set as p2 of the previous iteration for iteration; Connecting the remaining adjacent free path nodes to each other and updating the collision-free path; where dist(p i ,p j ) denotes the distance between points p i and p j .

8. The method of claim 1, wherein, In the process of updating the position information of the obstacle point each time, it is judged whether the collision-free path reaches the maximum planning threshold number of times, and if so, the planning of the collision-free path is stopped.

9. A robotic arm path planning system, characterized by, Comprise: An initialization module for initializing a rasterized map and obtaining preset position information of obstacle points in the rasterized map; A collision-free path planning module for planning a collision-free path by a graph search algorithm according to a required start point position, an end point position and the obtained position information of obstacle points; A collision-free path detection module for detecting whether the robot arm will collide or have an unreachable point when moving along the collision-free path; If the robot arm will collide or have an unreachable point when moving along the collision-free path, the node is added to the position information of the obstacle points; If not, a robot arm trajectory planning module is used to plan a trajectory of the robot arm according to the planned collision-free path to generate a motion trajectory of the robot arm; A robot arm trajectory detection module for detecting whether the robot arm will collide or have an unreachable point when moving along the motion trajectory; If the robot arm will collide or have an unreachable point when moving along the motion trajectory, the node is added to the position information of the obstacle points; If the position information of the obstacle points is updated, the collision-free path is re-planned by the collision-free path planning module.

10. A robot arm, characterized in that, For executing the motion trajectory planned by any one of the methods of claims 1 to 8.

Images