A dual-arm co-Y-axis truss robot handling trajectory planning method and system

Abstract

The present invention relates to the technical field of robot transport trajectory planning, and discloses a dual-arm co-Y-axis truss robot transport trajectory planning method and system, the method analyzes several situations from the current position to the target position, and divides the transport action of the dual-arm co-Y-truss robot into three sub-actions: step-by-step action, direct action, and avoidance action. All possible paths from the current position to the position after placement are represented by a transport trajectory path diagram, and a first setting algorithm is used to determine the shortest path in the transport trajectory path diagram, and trajectory planning is performed according to the position information of the shortest path and the second setting algorithm to obtain the transport trajectory. In this way, the transport path planning of the co-Y-axis truss robot can be automatically realized with high efficiency.

Description

Technical Field

[0001] The present invention relates to the technical field of robot transport trajectory planning, and in particular to a dual-arm co-Y-axis truss robot transport trajectory planning method and system. Background Art

[0002] Truss robots can carry objects and complete various operations. Truss robots have changed the traditional logistics methods, effectively improved the working environment, reduced manual labor, and promoted the efficiency of enterprise production automation. At present, truss robots widely use one arm for operation. This method has the advantages of low price, large grabbing weight, and flexible configuration. The existing truss robots are mainly single-arm type and common X-axis type. There is no common Y-axis type truss robot and corresponding handling control strategy. Since the two arms of the double-arm common Y-axis truss robot may interfere on the Y-axis, when planning tasks, interference checking and interference avoidance algorithms need to be considered. The existing single-arm and common X-axis control strategies will not be applicable to the double-arm common Y-axis truss robot. In addition, the current truss robot controller does not handle the situation when there is interference in the workspace. Therefore, it is urgent to provide an efficient handling planning method for a double-arm common Y-axis robot. Summary of the invention

[0003] The present invention provides a dual-arm co-Y-axis truss robot handling trajectory planning method and system to solve the problem that the handling planning method of the co-Y-axis robot in the prior art is lacking and inefficient.

[0004] In order to achieve the above object, the present invention is implemented by the following technical solutions:

[0005] In the first aspect, a dual-arm co-Y-axis truss robot handling trajectory planning method is applied to a dual-arm co-Y-axis truss robot handling trajectory planning system, wherein the two arms of the dual-arm co-Y-axis truss robot have their own unique two degrees of freedom in Z and rotation around the Z axis, and the two arms share the two degrees of freedom in X and Y, and the method comprises:

[0006] S1: Split the transport action of the dual-arm co-Y-axis truss robot from the starting position to the target position into three sub-actions: step-by-step action, direct action, and avoidance action, and plan the trajectory of the dual-arm co-Y-axis truss robot from the starting position to the pre-grasping position using the three sub-actions;

[0007] S2: construct the grasping path map and placement path map of the dual-arm co-Y-axis truss robot according to the trajectory;

[0008] S3: construct a complete transport path map based on the grab path map and the placement path map;

[0009] S4: using the first setting algorithm to determine the shortest path in the transport trajectory path map, and performing trajectory planning according to the position information of the shortest path and the second setting algorithm to obtain the transport trajectory.

[0010] In a second aspect, the present application provides a dual-arm co-Y-axis truss robot handling trajectory planning system, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the steps of the above method are implemented when the processor executes the computer program. Beneficial Effects

[0011] The dual-arm co-Y-axis truss robot handling trajectory planning method provided by the present invention analyzes several situations from the current position to the target position, and divides the handling action of the dual-arm co-Y-axis truss robot into three sub-actions: step-by-step action, direct action, and avoidance action. All possible paths from the current position to the position after placement are represented by a handling trajectory path diagram, and the shortest path in the handling trajectory path diagram is determined by a first setting algorithm, and the trajectory planning is performed according to the position information of the shortest path and the second setting algorithm to obtain the handling trajectory. In this way, the handling path planning of the co-Y-axis truss robot can be automatically realized with high efficiency.

[0012] In a further scheme, a path diagram is formed by considering the existence of interference, and an optimal path from the starting position to the placement completion position can be found according to the type of task, so as to complete the task and improve the success rate of the dual-arm co-Y-axis truss robot's handling task. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 This is a schematic diagram of the truss robot’s grasping;

[0014] Figure 2 This is a schematic diagram of the placement of the truss robot;

[0015] Figure 3 This is a flow chart of a method for planning a handling trajectory of a dual-arm co-Y-axis truss robot according to a preferred embodiment of the present invention;

[0016] Figure 4 A schematic diagram of a dual-arm task in a preferred embodiment of the present invention in which interference occurs in the X-axis direction;

[0017] Figure 5 This is a schematic diagram of the two arms of the preferred embodiment of the present invention being close to each other in the Y-axis direction;

[0018] Figure 6 This is a schematic diagram of a preferred embodiment of the present invention in which the two arms are crossed in the Y-axis direction;

[0019] Figure 7 A schematic diagram of a direct action path of a preferred embodiment of the present invention;

[0020] Figure 8 A schematic diagram of a step-by-step action trajectory of a preferred embodiment of the present invention;

[0021] Fig. 9 A schematic diagram of an avoidance approach action trajectory of a preferred embodiment of the present invention;

[0022] Fig.10 A schematic diagram of a crossing avoidance action trajectory according to a preferred embodiment of the present invention;

[0023] Fig.11 A flowchart of calculation of avoidance points according to a preferred embodiment of the present invention;

[0024] Fig.12 A transport path diagram of a preferred embodiment of the present invention;

[0025] Fig.13 Flow chart of the handling strategy of the preferred embodiment of the present invention. Implementation

[0026] The technical solution of the present invention is described clearly and completely below. 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 creative work are within the scope of protection of the present invention.

[0027] Unless otherwise defined, the technical terms or scientific terms used in the present invention shall have the usual meanings understood by persons with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Similarly, words such as "one" or "one" do not indicate quantity restrictions, but indicate the existence of at least one. Words such as "connect" or "connected" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the object being described changes, the relative positional relationship also changes accordingly.

[0028] It should be understood that the handling process of the truss robot can be divided into two steps: grabbing and placing. The schematic diagrams of the grabbing and placing trajectories in the XZ plane are as follows: Figure 1 and Figure 2 For the sake of convenience, in this application, grabbing and placing are collectively referred to as "actions". Figure 1-2It can be seen that both the handling and placement processes move from the current position to above the action position, then move down to the action position, and then return to above the action position. The geometric shapes of the two paths are similar, so the same set of planning strategies can be used for the trajectory of grasping and placing.

[0029] The two arms of a dual-arm co-Y-axis truss robot have their own unique degrees of freedom in Z and Rz (rotation around the Z axis), and the two degrees of freedom X and Y are shared by the two arms. Therefore, when considering interference, the two degrees of freedom X and Y are mainly considered. Based on this, the present application provides a dual-arm co-Y-axis truss robot handling trajectory planning method.

[0030] See also Figure 3 The present application provides a dual-arm co-Y-axis truss robot handling trajectory planning method, which is applied to a dual-arm co-Y-axis truss robot handling trajectory planning system. The two arms of the dual-arm co-Y-axis truss robot have two unique degrees of freedom in Z and rotation around the Z axis, and the two arms share the two degrees of freedom of X and Y. The method includes:

[0031] S1: Split the transport action of the dual-arm co-Y-axis truss robot from the starting position to the target position into three sub-actions: step-by-step action, direct action, and avoidance action, and plan the trajectory of the dual-arm co-Y-axis truss robot from the starting position to the pre-grasping position using the three sub-actions;

[0032] S2: construct the grasping path map and placement path map of the dual-arm co-Y-axis truss robot according to the trajectory;

[0033] S3: construct a complete transport path map based on the grab path map and the placement path map;

[0034] S4: using the first setting algorithm to determine the shortest path in the transport trajectory path map, and performing trajectory planning according to the position information of the shortest path and the second setting algorithm to obtain the transport trajectory.

[0035] In this embodiment, the first setting algorithm may be a Dijkstra algorithm, and the second setting algorithm may be an S-type trajectory planning algorithm.

[0036] The above-mentioned dual-arm co-Y-axis truss robot handling trajectory planning method analyzes several situations from the current position to the target position, and divides the handling action of the dual-arm co-Y-axis truss robot into three sub-actions: step-by-step action, direct action, and avoidance action. All possible paths from the current position to the position after placement are represented by a handling trajectory path diagram, and the shortest path in the handling trajectory path diagram is determined by the first setting algorithm, and the trajectory planning is performed according to the position information of the shortest path and the second setting algorithm to obtain the handling trajectory. In this way, the handling path planning of the co-Y-axis truss robot can be automatically realized with high efficiency.

[0037] The above method steps are described in detail below:

[0038] When the X coordinates of the dual-arm tasks are inconsistent, such as Figure 4 As shown in the figure, the two arms interfere with each other in the X direction. At this time, the two arms need to complete the tasks in sequence. That is, one arm moves to its X coordinate to perform the task first, and after the task is completed, the other arm moves to its X coordinate to perform the task.

[0039] It should be noted that there are two types of conflicts between the two arms in the Y-axis direction. One is that the positions of the two arms in the Y-axis direction are less than the safety distance, that is, the positions of the two arms in the Y-axis direction are very close, such as Figure 5 As shown; one is that the two arms will cross in the Y-axis direction, such as Figure 6 shown.

[0040] After combining the conflicts of the X-axis and the Y-axis, the conflict types of the two arms can be obtained as shown in Table 1:

[0041] Table 1 Conflict types of the two arms

[0042] No conflict on Y axis (y) Y-axis proximity (y') Y-axis crossing (y'') X-axis equal (x) xy xy' xy'' X-axis inequality (x') x'y x'y' x'y'

[0043] For xy type tasks, you can use Figure 7 At this time, the X coordinates of the target positions of the two arms are equal, and there is no interference in the Y axis direction, so the two arms can move directly from the current position to the target position.

[0044] For x'y type tasks, the X coordinates of the dual-arm tasks are inconsistent, and there is no conflict in the Y axis direction, so Figure 8 The step-by-step motion trajectory shown allows one arm to move to its target position first, and then the other arm moves to the target position after it completes the task.

[0045] For xy' and x'y' type tasks, the positions of the two arms in the Y direction are very close. In this case, an avoidance action (approach) trajectory can be used, such as Fig. 9 As shown. First, select an arm that has priority to perform the action, let the arm move to the target position, and the other arm is in an avoidance state. After the task is completed, let the avoidance arm move to the target position to perform the task, and the arm that has priority to perform the action is in an avoidance state.

[0046] For xy'' and x'y'' type tasks, the two arms cross in the Y direction. In this case, the avoidance action (crossing) trajectory can be adopted, as shown in Figure 10. The idea is to select one arm to perform the action first, and the other arm is in the avoidance state. After the task is completed, the two arms exchange states.

[0047] The difference between the avoidance (approach) action and the avoidance (cross) action is that the arm selection strategy for executing the action first is different. The avoidance (approach) action gives priority to the arm with a larger remaining range of motion of the joint to perform the task first. The avoidance (cross) action gives priority to the arm corresponding to the point where the current position and the target position are closer.

[0048] If arm #1 performs the action first, arm #2 will be in the avoidance state, and the avoidance direction of arm #2 is in the positive direction of the Y axis. After arm #1 completes the task, it will be in the avoidance state, and the avoidance direction is in the negative direction of the Y axis. Fig.11 The method shown calculates two avoidance points.

[0049] Specifically, determining the arm of the priority action and the arm of the subsequent action when the task type corresponds, and determining the avoidance directions of the two avoidance points according to the priority order;

[0050] Calculate the first avoidance position and the second avoidance position, and determine whether the first avoidance position and the second avoidance position exceed the joint limit respectively. If they exceed the joint limit, end the process. If they do not exceed the joint limit, further determine whether there is interference. If there is no interference, obtain the first avoidance point and the second avoidance point. If there is interference, recalculate the first avoidance position and the second avoidance position.

[0051] Specifically, when calculating the first avoidance position and the second avoidance position, an initial avoidance position is given first, then the first avoidance position is calculated, and finally the initial avoidance position is updated. Therefore, the avoidance position is calculated as follows:

[0052] Initial avoidance position = 0;

[0053] The first avoidance position = avoidance initial position + avoidance direction * avoidance step length;

[0054] Avoidance initial position = first avoidance position;

[0055] Among them, the calculation method of the second avoidance position is the same as the calculation method of the first avoidance position, and will not be repeated here.

[0056] Therefore, when the dual-arm truss robot controller receives a task, all possible paths from the current position to the position after placement can be represented by a graph structure, such as Fig.12As shown in the figure, since the avoidance (approach) action and the avoidance (cross) action are just different strategies for selecting the arm that performs the action first, and the two task types cannot exist at the same time, the avoidance (approach) action and the avoidance (cross) action are merged into the avoidance action when constructing the path graph. The starting position vertex stores the starting position information of the two arms; the placement completion position stores the position information of the two arms after placement; the step-by-step grasping and step-by-step placement respectively store the position point information of grasping or placing using the step-by-step action; the same applies to direct grasping, direct placement, avoidance grasping, and avoidance placement. The length of the path between two vertices is used to represent the weight between the vertices. If there is interference between the trajectories of the vertices, the weight is infinite.

[0057] After the controller receives the task, in order to plan a non-interference and shortest path from the starting position to the placement completion position, it is necessary to first build a path graph, then find the shortest path based on the shortest path search algorithm, and finally perform trajectory planning. The overall process of the entire handling strategy is as follows: Fig.13 shown.

[0058] In a complete example, the specific steps of the above dual-arm co-Y-axis truss robot handling trajectory planning method are as follows:

[0059] Plan the trajectory from the starting position to the pre-grasping position using step-by-step grasping, direct grasping action, and avoidance grasping action, and then perform interference checks on the trajectories of the three grasping paths. If the trajectory does not interfere, the length of the path is returned as the weight of the two vertices; if the trajectory interferes, positive infinity is returned as the weight between the two vertices. So far, the path graph from the starting position to step-by-step grasping, direct grasping, and avoidance grasping has been constructed. Next, start to build the path graph from the grasping position to the placement position.

[0060] The weight between the starting position and the three grabbing action vertices is used to determine whether to construct a subsequent placement path. Take the starting position to the step-by-step grab as an example: if there is no interference between the starting position and the step-by-step grab, that is, the weight is not equal to positive infinity, then start planning the trajectory from the step-by-step grab to the step-by-step placement, direct placement, and avoidance placement, and perform interference checks on these three trajectories. If there is no interference, the weight is the path length, and if there is interference, the weight is infinite. If there is interference between the starting position and the step-by-step grab, there is no need to construct a subsequent placement path diagram. The same applies to direct grabbing and avoidance grabbing. At this point, the placement path diagram has been constructed.

[0061] Finally, check whether the weights of step-by-step placement, direct placement, and avoidance placement to all parent nodes are positive infinity. If they are all positive infinity, it means that there is interference from any grabbing action to the path of the placement action, and the weight between the vertex and the placement position is positive infinity; otherwise, the weight between the vertex and the placement position is 0. So far, the complete transport trajectory path diagram has been constructed.

[0062] According to the transport trajectory path diagram, the Dijkstra algorithm can be used to find the shortest path from the starting position to the placement position. The path found at this time is a path with no interference and the shortest distance from the starting point to the end point.

[0063] Using the shortest path location information found by the Dijkstra algorithm and the S-type trajectory planning algorithm, a complete transport trajectory can be planned, and then the trajectory can be sent to the robot joint servo driver in real time to complete the transport action.

[0064] The present application also provides a dual-arm co-Y-axis truss robot handling trajectory planning system, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the steps of the above method when executing the computer program. The dual-arm co-Y-axis truss robot handling trajectory planning system can implement various embodiments of the dual-arm co-Y-axis truss robot handling trajectory planning method described above, and can achieve the same beneficial effects, which will not be described in detail here.

[0065] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A dual-arm co-Y-axis truss robot handling trajectory planning method, applied to a dual-arm co-Y-axis truss robot handling trajectory planning system, wherein the two arms of the dual-arm co-Y-axis truss robot have two unique degrees of freedom in Z and rotation around the Z axis, and the two arms share the two degrees of freedom in X and Y, characterized in that: The method comprises: S1: Split the transport action of the dual-arm co-Y-axis truss robot from the starting position to the target position into three sub-actions: step-by-step action, direct action, and avoidance action, and plan the trajectory of the dual-arm co-Y-axis truss robot from the starting position to the pre-grasping position using the three sub-actions; S2: constructing a grasping path map and a placing path map of the dual-arm co-Y-axis truss robot according to the trajectory; S3: constructing a complete transport path map according to the grabbing path map and the placing path map; S4: using a first setting algorithm to determine the shortest path in the transport trajectory path map, and performing trajectory planning according to the position information of the shortest path and the second setting algorithm to obtain a transport trajectory; When constructing the path map in S2, the following steps are also included: Get the task type of the dual-arm co-Y-axis truss robot; Determine an avoidance action according to the task type, wherein the avoidance action includes an avoidance approach action and an avoidance crossing action; Determine the avoidance method corresponding to each avoidance action; A path is determined according to the avoidance method.

2. The dual-arm co-Y-axis truss robot handling trajectory planning method according to claim 1 is characterized in that: The step S2 constructs a grasping path diagram of a dual-arm co-Y-axis truss robot according to the trajectory, including: The position involved in the trajectory is regarded as a vertex, and whether there is interference on the three trajectories is determined. If there is no interference on the trajectory, the length of the path is returned as the weight of the two vertices. If there is interference on the trajectory, positive infinity is returned as the weight between the two vertices. The grasping path diagram is obtained based on the weights and each vertex.

3. The dual-arm co-Y-axis truss robot handling trajectory planning method according to claim 2 is characterized in that: The step S2 of constructing a placement path diagram of a dual-arm co-Y-axis truss robot according to the trajectory includes: Determine the weight between the starting position and the target vertex. If the weight is not equal to positive infinity, start planning the trajectory from step-by-step capture to step-by-step placement, direct placement, and avoidance placement, and perform interference checks on these three trajectories. If there is no interference, the weight is the path length. If there is interference, the weight is infinite. If there is interference between the starting position and the target vertex, there is no need to construct a subsequent placement path graph, where the target vertex is the vertex corresponding to the step-by-step grasping, direct grasping, and avoidance grasping actions; The crawling path graph is obtained according to the weights and vertices.

4. The dual-arm co-Y-axis truss robot handling trajectory planning method according to claim 2 is characterized in that: The S3 includes: Check whether the weights of step-by-step placement, direct placement, and avoidance placement to all their parent nodes are all positive infinity. If they are all positive infinity, it means that there is interference in the path from any grabbing action to the placement action, and the weight between the vertex and the position where the placement is completed is positive infinity; otherwise, the weight between the vertex and the position where the placement is completed is 0. The complete transport trajectory path diagram is obtained based on the grabbing path diagram, the placement path diagram, and the weights.

5. The dual-arm co-Y-axis truss robot handling trajectory planning method according to claim 1 is characterized in that: The task types include X-axis equal and Y-axis non-conflict, X-axis equal and Y-axis close, X-axis equal and Y-axis cross, X-axis unequal and Y-axis non-conflict, X-axis unequal and Y-axis close, and X-axis unequal and Y-axis cross.

6. The dual-arm co-Y-axis truss robot handling trajectory planning method according to claim 5 is characterized in that: Determining the avoidance method corresponding to each avoidance action includes: Determine the arm of the priority action and the arm of the post-action when the task type corresponds, and determine the avoidance direction of the two avoidance points according to the priority order; Calculate the first avoidance position and the second avoidance position, and determine whether the first avoidance position and the second avoidance position exceed the joint limit respectively. If they exceed the joint limit, end the process. If they do not exceed the joint limit, further determine whether there is interference. If there is no interference, obtain the first avoidance point and the second avoidance point. If there is interference, recalculate the first avoidance position and the second avoidance position.

7. The dual-arm co-Y-axis truss robot handling trajectory planning method according to claim 6 is characterized in that: The calculation method for calculating the first avoidance position and the second avoidance position is as follows: First avoidance position = avoidance initial position + avoidance direction * avoidance step length; The second avoidance position = avoidance initial position + avoidance direction * avoidance step length.

8. A dual-arm co-Y-axis truss robot handling trajectory planning system, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method described in any one of claims 1 to 7 are implemented.

Images